my team cant test this code on an arena yet so I was seeing if maybe someone could check it over for any major errors
from vex import *
import urandom
import math
# Brain should be defined by default
brain=Brain()
# Robot configuration code
controller_1 = Controller(PRIMARY)
motor_16 = Motor(Ports.PORT16, GearSetting.RATIO_18_1, False)
motor_2 = Motor(Ports.PORT2, GearSetting.RATIO_18_1, False)
motor_4 = Motor(Ports.PORT4, GearSetting.RATIO_18_1, False)
optical_6 = Optical(Ports.PORT6)
gps_sensor = Gps(Ports.PORT15, 0.00, 0.00, MM, 180)
motor_1 = Motor(Ports.PORT1, GearSetting.RATIO_18_1, False)
motor_11 = Motor(Ports.PORT11, GearSetting.RATIO_18_1, False)
motor_10 = Motor(Ports.PORT10, GearSetting.RATIO_18_1, True)
motor_20 = Motor(Ports.PORT20, GearSetting.RATIO_18_1, True)
re_use = Motor(Ports.PORT3, GearSetting.RATIO_18_1, False)
inertial_sensor = Inertial(Ports.PORT5)
# wait for rotation sensor to fully initialize
wait(30, MSEC)
# Make random actually random
def initializeRandomSeed():
wait(100, MSEC)
random = brain.battery.voltage(MV) + brain.battery.current(CurrentUnits.AMP) * 100 + brain.timer.system_high_res()
urandom.seed(int(random))
# Set random seed
initializeRandomSeed()
# Color to String Helper
def convert_color_to_string(col):
if col == Color.RED:
return "red"
if col == Color.GREEN:
return "green"
if col == Color.BLUE:
return "blue"
if col == Color.WHITE:
return "white"
if col == Color.YELLOW:
return "yellow"
if col == Color.ORANGE:
return "orange"
if col == Color.PURPLE:
return "purple"
if col == Color.CYAN:
return "cyan"
if col == Color.BLACK:
return "black"
if col == Color.TRANSPARENT:
return "transparent"
return ""
def play_vexcode_sound(sound_name):
# Helper to make playing sounds from the V5 in VEXcode easier and
# keeps the code cleaner by making it clear what is happening.
print("VEXPlaySound:" + sound_name)
wait(5, MSEC)
# add a small delay to make sure we don't print in the middle of the REPL header
wait(200, MSEC)
# clear the console to make sure we don't have the REPL in the console
print("\033[2J")
#endregion VEXcode Generated Robot Configuration
#for gps the first digits are x then next is y, the 180 at end is the direction relative to face of gps in deggress
myVariable = 0
a = 0
b = 0
right_motors = MotorGroup(motor_10, motor_20)
left_motors = MotorGroup(motor_1, motor_11)
motor_1 = Motor(Ports.PORT1, GearSetting.RATIO_18_1, False)
motor_11 = Motor(Ports.PORT11, GearSetting.RATIO_18_1, False)
motor_10 = Motor(Ports.PORT10, GearSetting.RATIO_18_1, True)
motor_20 = Motor(Ports.PORT20, GearSetting.RATIO_18_1, True)
right_motors = MotorGroup(motor_10, motor_20)
left_motors = MotorGroup(motor_1, motor_11)
drivetrain = SmartDrive(left_motors, right_motors, gps_sensor, 319.19, 320, 40, MM, 1)
distanceas = 0
waits = 0
reddd = 0
blueee = 0
timerr = 0
message1 = Event()
second = Event()
first = Event()
wait1 = Event()
wait2 = Event()
redd = Event()
bluee = Event()
timmmer = Event()
run = True
motor_busy = False
blueee = 1
reddd = 1
timerr = 1
timerr = 1
motor_4.spin_for(REVERSE, 9, DEGREES)
stop_break=True
stop_break2=True
first_run=True
run_other=0
brain.screen.clear_screen()
optical_G = Optical(Ports.PORT6)
motor_d = Motor(Ports.PORT4, True)
motor_d.set_position(0, DEGREES)
motor_d.set_stopping(HOLD)
current_position = 0
ball_queue = []
ball_present = False
motor_busy = False
MOTOR_SPEED = 100
BLUE_POSITION = 0
RED_POSITION = -72
DETECTION_WAIT = 10 #time between deections so I don't etect the ball too fast its in ms btw
PROCESS_WAIT = 5 # time for it to basically go up the robot, will need tuning, its in ms btw
MAX_QUEUE_SIZE = 5
DETECTION_COOLDOWN=500
last_detection_time=0
throttle_left=10
throttle_right=10
throttle_on=False
stop1=False
#hey so this is detection without delays s no matter what I can detect what is going to happen and no delay guy b
def detect_balls():
global ball_present, ball_queue, stop1
while True:
if optical_G.is_near_object():
color = optical_G.color()
if (color == Color.RED or color == Color.BLUE) and not ball_present:
if len(ball_queue) < MAX_QUEUE_SIZE:
ball_queue.append(color)
ball_present = True
else:
brain.screen.print_at("Queue Full!!", 10, 80)
else:
ball_present = False
wait(DETECTION_WAIT, MSEC)
def process_balls():
global motor_busy, current_position, ball_queue, target_position, BLUE_POSITION, stop1
al = True
while True:
wait(10,MSEC)
if stop1 == True:
al=True
if stop1 == True:#PROBLEMS
al=False
if al==False:
re_use.stop()
if not motor_busy and len(ball_queue) > 0 :
next_color = ball_queue.pop(0)
if next_color == Color.RED:
motor_4.spin(FORWARD)# might need checking for direction
motor_busy = True
wait(0.3,SECONDS)
elif next_color == Color.BLUE:
motor_4.spin(REVERSE)# might need checking for direction
motor_busy = True
wait(0.3,SECONDS)
motor_busy = False
wait(10,MSEC)
elif al == True:
motor_4.spin(FORWARD)# might need checking for direction
re_use.spin(REVERSE)# might need checking for direction
#target_position = RED_POSITION if next_color == Color.RED else BLUE_POSITION
# if current_position != target_position:
# motor_busy = True
# if target_position > current_position:
# direction = FORWARD
# degrees = target_position - current_position
# else:
# direction = REVERSE
# degrees = current_position - target_position
# motor_d.set_velocity(MOTOR_SPEED, PERCENT)
# color_name = "RED" if next_color == Color.RED else "BLUE"
# motor_d.spin_for(direction, degrees, DEGREES, wait=False)
# while motor_d.is_spinning():
# wait(5, MSEC)
# current_position = target_position
# motor_d.stop(HOLD)
# motor_busy = False
# else:
# color_name = "RED" if next_color == Color.RED else "BLUE"
# wait(PROCESS_WAIT, MSEC)
def when_started4():
global myVariable, a, b, distanceas, waits, reddd, blueee, timerr, message1, second, first, wait1, wait2, redd, bluee, timmmer
motor_16.set_velocity(70, PERCENT)
motor_2.set_velocity(100, PERCENT)
motor_4.set_velocity(75, PERCENT)
def when_started6():
global myVariable, a, b, distanceas, waits, reddd, blueee, timerr, message1, second, first, wait1, wait2, redd, bluee, timmmer
optical_6.set_light_power(100, PERCENT)
optical_6.set_light(LedStateType.ON)
def onevent_controller_1buttonR1_pressed_0():
global myVariable, a, b, distanceas, waits, reddd, blueee, timerr, message1, second, first, wait1, wait2, redd, bluee, timmmer
motor_16.spin(FORWARD)
motor_2.spin(FORWARD)
def onevent_controller_1buttonL2_released_0():
global myVariable, a, b, distanceas, waits, reddd, blueee, timerr, message1, second, first, wait1, wait2, redd, bluee, timmmer
motor_2.stop()
motor_16.stop()
def onevent_controller_1buttonR2_released_0():
global myVariable, a, b, distanceas, waits, reddd, blueee, timerr, message1, second, first, wait1, wait2, redd, bluee, timmmer
motor_16.stop()
motor_2.stop()
def onevent_controller_1buttonL1_released_0():
global myVariable, a, b, distanceas, waits, reddd, blueee, timerr, message1, second, first, wait1, wait2, redd, bluee, timmmer
motor_16.stop()
motor_2.stop()
def onevent_controller_1buttonL1_pressed_0():
global myVariable, a, b, distanceas, waits, reddd, blueee, timerr, message1, second, first, wait1, wait2, redd, bluee, timmmer
motor_2.spin(REVERSE)
motor_16.spin(FORWARD)
def onevent_controller_1buttonR1_released_0():
global myVariable, a, b, distanceas, waits, reddd, blueee, timerr, message1, second, first, wait1, wait2, redd, bluee, timmmer
motor_16.stop()
motor_2.stop()
def onevent_controller_1buttonL2_pressed_0():
global myVariable, a, b, distanceas, waits, reddd, blueee, timerr, message1, second, first, wait1, wait2, redd, bluee, timmmer
motor_2.spin(FORWARD)
motor_16.spin(FORWARD)
def onevent_controller_1buttonR2_pressed_0():
global myVariable, a, b, distanceas, waits, reddd, blueee, timerr, message1, second, first, wait1, wait2, redd, bluee, timmmer
motor_16.spin(REVERSE)
motor_2.spin(FORWARD)
def onevent_controller_1buttonup_released_0():
global stop1
stop1=True
def onevent_controller_1buttondown_released_0():
global stop1
stop1=False
def when_started3():
controller_1.buttonR1.pressed(onevent_controller_1buttonR1_pressed_0)
controller_1.buttonL2.released(onevent_controller_1buttonL2_released_0)
controller_1.buttonR2.released(onevent_controller_1buttonR2_released_0)
controller_1.buttonL1.released(onevent_controller_1buttonL1_released_0)
controller_1.buttonL1.pressed(onevent_controller_1buttonL1_pressed_0)
controller_1.buttonR1.released(onevent_controller_1buttonR1_released_0)
controller_1.buttonL2.pressed(onevent_controller_1buttonL2_pressed_0)
controller_1.buttonR2.pressed(onevent_controller_1buttonR2_pressed_0)
controller_1.buttonUp.released(onevent_controller_1buttonup_released_0)
controller_1.buttonDown.released(onevent_controller_1buttondown_released_0)
#curve upwards throttle
# Set motors to coast so they slow down smoothly
motor_1.set_stopping(COAST)
motor_11.set_stopping(COAST)
motor_10.set_stopping(COAST)
motor_20.set_stopping(COAST)
#axis3
current_speed_left = 0.0
target_speed_left = 0.0
acceleration_rate_left = 100.0
deceleration_rate_left = 1000.0
#axis2
current_speed_right = 0.0
target_speed_right = 0.0
acceleration_rate_right = 100.0
deceleration_rate_right = 1000.0
def smooth_drive_left():
global current_speed_left, target_speed_left, left_motors , current_speed_left, controller_1
while True:#testing if needed may not be needed
controller_input = controller_1.axis3.position()
target_speed_left = controller_input
# code for the smooth speed left
if current_speed_left < target_speed_left:
current_speed_left = current_speed_left + acceleration_rate_left
if current_speed_left > target_speed_left:
current_speed_left = target_speed_left
elif current_speed_left > target_speed_left:
current_speed_left = current_speed_left - deceleration_rate_left
if current_speed_left < target_speed_left:
current_speed_left = target_speed_left
#Spin both
left_motors.spin(FORWARD, current_speed_left, PERCENT)
wait(20, MSEC)
def smooth_drive_right():
global current_speed_right, target_speed_right, controller_1, right_motors, current_speed_right
while True:#testing if needed may not be needed
#brain.screen.print("VEXcode")
#brain.screen.next_row("Axis 2:", controller_1.axis2.position())
controller_input = controller_1.axis2.position()
target_speed_right = controller_input
#code for smooth speed
if current_speed_right < target_speed_right:
current_speed_right = current_speed_right + acceleration_rate_right
if current_speed_right > target_speed_right:
current_speed_right = target_speed_right
elif current_speed_right > target_speed_right:
current_speed_right = current_speed_right - deceleration_rate_right
if current_speed_right < target_speed_right:
current_speed_right = target_speed_right
#Spin both
right_motors.spin(FORWARD, current_speed_right, PERCENT)
wait(20, MSEC)
#start of auton, gps pid control
max_speed = 50
min_speed = 10
turn_speed = 30
stopping_distance = 50
# driving control
def ondriver_drivercontrol_0():
global myVariable
when_started4()
when_started6()
when_started3()
ws1 = Thread(detect_balls)
ws2 = Thread(process_balls)
ws3 = Thread( smooth_drive_left )
ws4 = Thread( smooth_drive_right )
#ws5 = Thread(onevent_controller_1buttonup_pressed_0)
#ws6 = Thread(onevent_controller_1buttonDown_pressed_0)
wait(100, MSEC)
pass
#non manual auton
reference_angle = 0
def drive_to_position_y(y):
global reference_angle
if gps_sensor.y_position(MM) < y:
reference_angle = 0
else:
reference_angle = 180
# before driving along the Y-axis
drivetrain.turn_for(RIGHT, (reference_angle - gps_sensor.heading()), DEGREES)
drivetrain.drive(FORWARD)
while not (gps_sensor.y_position(MM) - y > -20
and gps_sensor.y_position(MM) - y < 20):
wait(5, MSEC)
drivetrain.stop()
def drive_to_position_x(x):
global reference_angle
if gps_sensor.x_position(MM) < x:
reference_angle = 90
else:
reference_angle = 270
drivetrain.turn_for(RIGHT, (reference_angle - gps_sensor.heading()), DEGREES)
drivetrain.drive(FORWARD)
while not (gps_sensor.x_position(MM) - x > -20
and gps_sensor.x_position(MM) - x < 20):
wait(5, MSEC)
drivetrain.stop()
def print_position():
# Print GPS position values to the V5 Brain
brain.screen.print("X: ", gps_sensor.x_position(MM))
brain.screen.print(" Y: ", gps_sensor.y_position(MM))
brain.screen.next_row()
def turn_with_inertial(degrees):
start_heading = inertial_sensor.rotation(DEGREES)
target_heading = start_heading + degrees
while target_heading > 360:
target_heading -= 360
while target_heading < 0:
target_heading += 360
error = target_heading - inertial_sensor.rotation(DEGREES)
if error > 180:
error -= 360
elif error < -180:
error += 360
if error > 0: # Turn right
left_motors.spin(FORWARD)
right_motors.spin(REVERSE)
else: # Turn left
left_motors.spin(REVERSE)
right_motors.spin(FORWARD)
while abs(inertial_sensor.rotation(DEGREES) - target_heading) > 2:
wait(5, MSEC)
left_motors.stop()
right_motors.stop()
# turn_with_inertial(90)
#manual auton
def onauton_autonomous_0():
global myVariable
brain.screen.clear_screen()
####
motor_1 = Motor(Ports.PORT1, GearSetting.RATIO_18_1, False)
motor_11 = Motor(Ports.PORT11, GearSetting.RATIO_18_1, False)
motor_10 = Motor(Ports.PORT10, GearSetting.RATIO_18_1, True)
motor_20 = Motor(Ports.PORT20, GearSetting.RATIO_18_1, True)
right_motors = MotorGroup(motor_10, motor_20)
left_motors = MotorGroup(motor_1, motor_11)
drivetrain = SmartDrive(left_motors, right_motors, gps_sensor, 319.19, 320, 40, MM, 1)
motor_1.set_velocity(50, PERCENT)
motor_11.set_velocity(50, PERCENT)
motor_10.set_velocity(50, PERCENT)
motor_20.set_velocity(50, PERCENT)
####
motor_16.set_velocity(100, PERCENT)
motor_2.set_velocity(100, PERCENT)
# motor_4.spin_for(REVERSE, 71, DEGREES)
# ####
# #the code at start for new parking
# right_motors.spin(FORWARD)
# left_motors.spin(FORWARD)
# wait(1.5,SECONDS)
# right_motors.stop()
# left_motors.stop()
# right_motors.spin(REVERSE)
# left_motors.spin(FORWARD)
# wait(0.5,SECONDS)
# right_motors.stop()
# left_motors.stop()
# right_motors.spin(FORWARD)
# left_motors.spin(FORWARD)
# wait(0.55,SECONDS)
# right_motors.stop()
# left_motors.stop()
# right_motors.spin(FORWARD)
# left_motors.spin(REVERSE)
# wait(0.48,SECONDS)
# right_motors.stop()
# left_motors.stop()
# motor_16.spin(FORWARD)
# right_motors.spin(FORWARD)
# left_motors.spin(FORWARD)
# wait(1.8,SECONDS)
# right_motors.stop()
# left_motors.stop()
# motor_16.stop()
# right_motors.spin(REVERSE)
# left_motors.spin(REVERSE)
# wait(0.8,SECONDS)
# right_motors.stop()
# left_motors.stop()
# right_motors.spin(FORWARD)
# left_motors.spin(REVERSE)
# wait(0.30,SECONDS)
# right_motors.stop()
# left_motors.stop()
# right_motors.spin(FORWARD)
# left_motors.spin(FORWARD)
# wait(0.9,SECONDS)
# right_motors.stop()
# left_motors.stop()
# motor_16.spin(REVERSE)
# wait(2.3,SECONDS)
# motor_16.stop()
# motor_4.spin_for(FORWARD, 67, DEGREES)
inertial_sensor.calibrate()
while inertial_sensor.is_calibrating():
wait(25, MSEC)
gps_sensor.calibrate()
while gps_sensor.is_calibrating():
sleep(25, MSEC)
drivetrain.set_turn_velocity(25, PERCENT)
print_position()
#where to go
drive_to_position_x(0)
drive_to_position_y(0)
print_position()
#turning
turn_with_inertial(90)
pass
# # create a function for handling the starting and stopping of all autonomous tasks
# def vexcode_auton_function():
# # Start the autonomous control tasks
# auton_task_0 = Thread( onauton_autonomous_0 )
# # wait for the driver control period to end
# while( competition.is_autonomous() and competition.is_enabled() ):
# # wait 10 milliseconds before checking again
# wait( 10, MSEC )
# # Stop the autonomous control tasks
# auton_task_0.stop()
# def vexcode_driver_function():
# # Start the driver control tasks
# driver_control_task_0 = Thread( ondriver_drivercontrol_0 )
# # wait for the driver control period to end
# while( competition.is_driver_control() and competition.is_enabled() ):
# # wait 10 milliseconds before checking again
# wait( 10, MSEC )
# # Stop the driver control tasks
# driver_control_task_0.stop()
# # register the competition functions
# competition = Competition( vexcode_driver_function, vexcode_auton_function )
def pre_autonomous():
# actions to do when the program starts
brain.screen.clear_screen()
brain.screen.print("pre auton code")
wait(1, SECONDS)
def autonomous():
brain.screen.clear_screen()
brain.screen.print("autonomous code")
# place automonous code here
auton_task_0 = Thread( onauton_autonomous_0 )
def user_control():
brain.screen.clear_screen()
# place driver control in this while loop
while True:
driver_control_task_0 = Thread( ondriver_drivercontrol_0 )
wait(20, MSEC)
# create competition instance
comp = Competition(user_control, autonomous)
pre_autonomous()