Im currently working on a project where the claw bot I have built finds and places a yellow cone onto a red mobile base.
My current code is as followed;
#region VEXcode Generated Robot Configuration
from vex import *
import urandom
# Brain should be defined by default
brain=Brain()
# Robot configuration code
left_drive_smart = Motor(Ports.PORT10, GearSetting.RATIO_18_1, False)
right_drive_smart = Motor(Ports.PORT1, GearSetting.RATIO_18_1, True)
drivetrain = DriveTrain(left_drive_smart, right_drive_smart, 319.19, 295, 40, MM, 1)
Arm = Motor(Ports.PORT8, GearSetting.RATIO_18_1, False)
Claw = Motor(Ports.PORT4, GearSetting.RATIO_18_1, False)
controller_1 = Controller(PRIMARY)
optical_11 = Optical(Ports.PORT11)
# vex-vision-config:begin
vision_5__RED_CONE = Signature(1, 8061, 8981, 8521,-1137, -677, -907,2.5, 0)
vision_5__YELLOW_CONE = Signature(2, 2163, 3641, 2902,-4369, -3627, -3998,2.5, 0)
vision_5 = Vision(Ports.PORT5, 50, vision_5__RED_CONE, vision_5__YELLOW_CONE)
# vex-vision-config:end
distance_2 = Distance(Ports.PORT2)
# wait for rotation sensor to fully initialize
wait(30, MSEC)
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")
# define variables used for controlling motors based on controller inputs
controller_1_left_shoulder_control_motors_stopped = True
controller_1_right_shoulder_control_motors_stopped = True
drivetrain_l_needs_to_be_stopped_controller_1 = False
drivetrain_r_needs_to_be_stopped_controller_1 = False
# define a task that will handle monitoring inputs from controller_1
def rc_auto_loop_function_controller_1():
global drivetrain_l_needs_to_be_stopped_controller_1, drivetrain_r_needs_to_be_stopped_controller_1, controller_1_left_shoulder_control_motors_stopped, controller_1_right_shoulder_control_motors_stopped, remote_control_code_enabled
# process the controller input every 20 milliseconds
# update the motors based on the input values
while True:
if remote_control_code_enabled:
# calculate the drivetrain motor velocities from the controller joystick axies
# left = axis3
# right = axis2
drivetrain_left_side_speed = controller_1.axis3.position()
drivetrain_right_side_speed = controller_1.axis2.position()
# check if the value is inside of the deadband range
if drivetrain_left_side_speed < 5 and drivetrain_left_side_speed > -5:
# check if the left motor has already been stopped
if drivetrain_l_needs_to_be_stopped_controller_1:
# stop the left drive motor
left_drive_smart.stop()
# tell the code that the left motor has been stopped
drivetrain_l_needs_to_be_stopped_controller_1 = False
else:
# reset the toggle so that the deadband code knows to stop the left motor next
# time the input is in the deadband range
drivetrain_l_needs_to_be_stopped_controller_1 = True
# check if the value is inside of the deadband range
if drivetrain_right_side_speed < 5 and drivetrain_right_side_speed > -5:
# check if the right motor has already been stopped
if drivetrain_r_needs_to_be_stopped_controller_1:
# stop the right drive motor
right_drive_smart.stop()
# tell the code that the right motor has been stopped
drivetrain_r_needs_to_be_stopped_controller_1 = False
else:
# reset the toggle so that the deadband code knows to stop the right motor next
# time the input is in the deadband range
drivetrain_r_needs_to_be_stopped_controller_1 = True
# only tell the left drive motor to spin if the values are not in the deadband range
if drivetrain_l_needs_to_be_stopped_controller_1:
left_drive_smart.set_velocity(drivetrain_left_side_speed, PERCENT)
left_drive_smart.spin(FORWARD)
# only tell the right drive motor to spin if the values are not in the deadband range
if drivetrain_r_needs_to_be_stopped_controller_1:
right_drive_smart.set_velocity(drivetrain_right_side_speed, PERCENT)
right_drive_smart.spin(FORWARD)
# check the buttonL1/buttonL2 status
# to control Arm
if controller_1.buttonL1.pressing():
Arm.spin(FORWARD)
controller_1_left_shoulder_control_motors_stopped = False
elif controller_1.buttonL2.pressing():
Arm.spin(REVERSE)
controller_1_left_shoulder_control_motors_stopped = False
elif not controller_1_left_shoulder_control_motors_stopped:
Arm.stop()
# set the toggle so that we don't constantly tell the motor to stop when
# the buttons are released
controller_1_left_shoulder_control_motors_stopped = True
# check the buttonR1/buttonR2 status
# to control Claw
if controller_1.buttonR1.pressing():
Claw.spin(FORWARD)
controller_1_right_shoulder_control_motors_stopped = False
elif controller_1.buttonR2.pressing():
Claw.spin(REVERSE)
controller_1_right_shoulder_control_motors_stopped = False
elif not controller_1_right_shoulder_control_motors_stopped:
Claw.stop()
# set the toggle so that we don't constantly tell the motor to stop when
# the buttons are released
controller_1_right_shoulder_control_motors_stopped = True
# wait before repeating the process
wait(20, MSEC)
# define variable for remote controller enable/disable
remote_control_code_enabled = True
rc_auto_loop_thread_controller_1 = Thread(rc_auto_loop_function_controller_1)
#endregion VEXcode Generated Robot Configuration
vexcode_vision_5_objects = None
vexcode_vision_5_object_index = 0
message1 = Event()
myVariable = 0
opem = 0
close = 0
grabbed = 0
cone_in_air = 0
def Find_Base():
global message1, myVariable, opem, close, grabbed, cone_in_air, vexcode_vision_5_objects, vexcode_vision_5_object_index
while True:
vexcode_vision_5_objects = vision_5.take_snapshot(vision_5__RED_CONE)
if vexcode_vision_5_objects and len(vexcode_vision_5_objects) > 0:
drivetrain.turn_for(RIGHT, 20, DEGREES)
wait(1, SECONDS)
break
else:
drivetrain.turn_for(RIGHT, 4, DEGREES)
wait(5, MSEC)
while True:
vexcode_vision_5_objects = vision_5.take_snapshot(vision_5__RED_CONE)
if vexcode_vision_5_objects[vexcode_vision_5_object_index].centerX < 95 and vexcode_vision_5_objects and len(vexcode_vision_5_objects) > 0:
wait(0.25, SECONDS)
drivetrain.turn_for(LEFT, 7, DEGREES)
wait(0.25, SECONDS)
drivetrain.drive_for(FORWARD, 100, MM)
if vexcode_vision_5_objects[vexcode_vision_5_object_index].centerX > 100 and vexcode_vision_5_objects and len(vexcode_vision_5_objects) > 0:
wait(0.25, SECONDS)
drivetrain.turn_for(RIGHT, 7, DEGREES)
wait(0.25, SECONDS)
drivetrain.drive_for(FORWARD, 100, MM)
if distance_2.object_distance(MM) < 270:
wait(0.25, SECONDS)
break
wait(5, MSEC)
def Drop():
global message1, myVariable, opem, close, grabbed, cone_in_air, vexcode_vision_5_objects, vexcode_vision_5_object_index
wait(0.25, SECONDS)
Arm.spin_to_position(500, DEGREES)
wait(0.25, SECONDS)
drivetrain.drive_for(FORWARD, 300, MM)
wait(0.25, SECONDS)
Claw.spin_for(REVERSE, 360, DEGREES)
wait(0.25, SECONDS)
drivetrain.drive_for(REVERSE, 300, MM)
wait(0.25, SECONDS)
Arm.spin_to_position(0, DEGREES)
wait(0.25, SECONDS)
drivetrain.turn_for(RIGHT, 90, DEGREES)
wait(0.25, SECONDS)
Claw.spin_to_position(0, DEGREES)
def Start():
global message1, myVariable, opem, close, grabbed, cone_in_air, vexcode_vision_5_objects, vexcode_vision_5_object_index
Arm.spin_for(FORWARD, 45, DEGREES)
Claw.spin_for(REVERSE, 360, DEGREES)
close = 0
opem = 0
grabbed = 0
cone_in_air = 0
drivetrain.set_turn_velocity(30, PERCENT)
drivetrain.set_drive_velocity(25, PERCENT)
Arm.set_velocity(25, PERCENT)
Claw.set_stopping(HOLD)
Arm.set_stopping(HOLD)
while True:
vexcode_vision_5_objects = vision_5.take_snapshot(vision_5__YELLOW_CONE)
if vexcode_vision_5_objects and len(vexcode_vision_5_objects) > 0:
wait(0.25, SECONDS)
drivetrain.turn_for(RIGHT, 25, DEGREES)
wait(0.25, SECONDS)
break
else:
wait(0.25, SECONDS)
drivetrain.turn_for(RIGHT, 4, DEGREES)
wait(5, MSEC)
while True:
vexcode_vision_5_objects = vision_5.take_snapshot(vision_5__YELLOW_CONE)
if vexcode_vision_5_objects[vexcode_vision_5_object_index].centerX < 95 and vexcode_vision_5_objects and len(vexcode_vision_5_objects) > 0:
wait(0.25, SECONDS)
drivetrain.turn_for(LEFT, 7, DEGREES)
wait(0.25, SECONDS)
drivetrain.drive_for(FORWARD, 100, MM)
if vexcode_vision_5_objects[vexcode_vision_5_object_index].centerX > 100 and vexcode_vision_5_objects and len(vexcode_vision_5_objects) > 0:
wait(0.25, SECONDS)
drivetrain.turn_for(RIGHT, 7, DEGREES)
wait(0.25, SECONDS)
drivetrain.drive_for(FORWARD, 100, MM)
if distance_2.object_distance(MM) < 200:
break
wait(5, MSEC)
def pick_up():
global message1, myVariable, opem, close, grabbed, cone_in_air, vexcode_vision_5_objects, vexcode_vision_5_object_index
while True:
if distance_2.object_distance(MM) < 175 and distance_2.object_distance(MM) > 160:
wait(0.25, SECONDS)
Claw.spin_for(FORWARD, 152, DEGREES)
wait(0.25, SECONDS)
grabbed = grabbed + 1
if not (distance_2.object_distance(MM) < 175 and distance_2.object_distance(MM) > 160):
wait(0.25, SECONDS)
drivetrain.drive_for(FORWARD, 200, MM)
if grabbed == 1:
wait(0.25, SECONDS)
Arm.spin_for(FORWARD, 150, DEGREES)
wait(0.25, SECONDS)
cone_in_air = cone_in_air + 1
wait(0.25, SECONDS)
break
wait(5, MSEC)
def when_started1():
global message1, myVariable, opem, close, grabbed, cone_in_air, vexcode_vision_5_objects, vexcode_vision_5_object_index
Start()
pick_up()
Find_Base()
Drop()
when_started1()