#region VEXcode Generated Robot Configuration
from vex import *
import urandom
# Brain should be defined by default
brain=Brain()
# Robot configuration code
expander_7 = Triport(Ports.PORT7)
left_motor_a = Motor(Ports.PORT16, GearSetting.RATIO_6_1, True)
left_motor_b = Motor(Ports.PORT9, GearSetting.RATIO_6_1, True)
left_drive_smart = MotorGroup(left_motor_a, left_motor_b)
right_motor_a = Motor(Ports.PORT4, GearSetting.RATIO_6_1, False)
right_motor_b = Motor(Ports.PORT11, GearSetting.RATIO_6_1, False)
right_drive_smart = MotorGroup(right_motor_a, right_motor_b)
drivetrain = DriveTrain(left_drive_smart, right_drive_smart, 319.19, 381, 40, MM, 0.2857142857142857)
controller_1 = Controller(PRIMARY)
goalcarrier = Motor(Ports.PORT10, GearSetting.RATIO_36_1, False)
rearcarrier = Motor(Ports.PORT18, GearSetting.RATIO_36_1, True)
inertial = Inertial(Ports.PORT15)
claw1 = DigitalOut(brain.three_wire_port.a)
limit_switch_b = Limit(brain.three_wire_port.b)
encoder_7a = Encoder(expander_7.a)
rear_switch = Bumper(brain.three_wire_port.c)
encoder_7c = Encoder(brain.three_wire_port.e)
# vex-vision-config:begin
vision_10__NEUTRAL_GOAL = Signature(1, 1121, 1613, 1367,-3973, -3579, -3776,2.4, 0)
vision_10__SIG_2 = Signature(2, 0, 0, 0,0, 0, 0,3, 0)
vision_10__SIG_3 = Signature(3, 0, 0, 0,0, 0, 0,3, 0)
vision_10 = Vision(Ports.PORT17, 50, vision_10__NEUTRAL_GOAL, vision_10__SIG_2, vision_10__SIG_3)
# vex-vision-config:end
gps_6 = Gps(Ports.PORT1, -177.80, 0.00, MM, -90)
potentiometerc = PotentiometerV2(expander_7.c)
rings_motor_a = Motor(Ports.PORT5, GearSetting.RATIO_18_1, False)
rings_motor_b = Motor(Ports.PORT13, GearSetting.RATIO_18_1, True)
rings = MotorGroup(rings_motor_a, rings_motor_b)
platform = Sonar(brain.three_wire_port.g)
puncher = DigitalOut(expander_7.d)
# wait for rotation sensor to fully initialize
wait(30, MSEC)
# 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 + axis1
# right = axis3 - axis1
drivetrain_left_side_speed = controller_1.axis3.position() + controller_1.axis1.position()
drivetrain_right_side_speed = controller_1.axis3.position() - controller_1.axis1.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 rearcarrier
if controller_1.buttonL1.pressing():
rearcarrier.spin(FORWARD)
controller_1_left_shoulder_control_motors_stopped = False
elif controller_1.buttonL2.pressing():
rearcarrier.spin(REVERSE)
controller_1_left_shoulder_control_motors_stopped = False
elif not controller_1_left_shoulder_control_motors_stopped:
rearcarrier.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 goalcarrier
if controller_1.buttonR1.pressing():
goalcarrier.spin(FORWARD)
controller_1_right_shoulder_control_motors_stopped = False
elif controller_1.buttonR2.pressing():
goalcarrier.spin(REVERSE)
controller_1_right_shoulder_control_motors_stopped = False
elif not controller_1_right_shoulder_control_motors_stopped:
goalcarrier.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
# -------------------------------------------#
# #
# Project: VEXcode Project #
# Author: *************** #
# Created: #
# Description:this is a program for live #
# tipping point matches #
# #
# -------------------------------------------#
# Library imports
speed2 = 6
from vex import *
stop_following = False
puncher.set(False)
goalcarrier.set_velocity(100, RPM)
rearcarrier.set_velocity(100, RPM)
rings.set_velocity(600, RPM)
rearcarrier.set_stopping(BRAKE)
# Begin project code
controller_1.screen.print(potentiometerc.angle(DEGREES))
inertial.calibrate()
wait(0.5, SECONDS)
gps_6.calibrate()
inertial.calibrate()
wait(2, SECONDS)
inertial.set_heading(180, DEGREES)
inertial.set_rotation(180, DEGREES)
print(inertial.heading(DEGREES))
rearcarrier.set_velocity(100, PERCENT)
def close_claw():
print("a;lsdhgj;fjsfjdlkgfhkdfslgd")
while not limit_switch_b.pressing():
pass
claw1.set(True)
goalcarrier.spin(FORWARD)
my_event = Event()
my_event(close_claw)
def pre_autonomous():
while True:
drivetrain.set_stopping(BRAKE)
if controller_1.buttonRight.pressing():
claw1.set(True)
if controller_1.buttonB.pressing():
claw1.set(False)
controller_1.screen.clear_screen()
print(potentiometerc.angle(DEGREES))
controller_1.screen.print(potentiometerc.angle(DEGREES))
#adjust vision sensor brightness based on where the screen is touched to make sure it
#works at each field
def autonomous():
def drive(distance):
drivetrain.set_stopping(BRAKE)
#erase previous angle, set variables, and drive forward
encoder_7a.set_position(0, TURNS)
encoder_7c.set_position(0, TURNS)
speed = 100
if distance > 0:
drivetrain.drive(FORWARD)
#wait until the encoder angle passes the threshold value
while encoder_7c.position(TURNS) > distance * -0.8898:
#calculate the speed needed to stop the robot most acurately
error = (distance * 0.8898 - encoder_7c.position(TURNS)) * speed
if error > 150:
error = 150
drivetrain.set_drive_velocity(error, RPM)
if drivetrain.velocity(RPM) < 3.6:
if drivetrain.velocity(RPM) > -3.6:
drivetrain.set_drive_velocity(10, RPM)
if error < 3.6:
if error > -3.6:
break
elif distance < 0:
drivetrain.drive(REVERSE)
#wait until the encoder angle passes the threshold value
while encoder_7c.position(TURNS) < distance * -0.8898:
#calculate the speed needed to stop the robot most acurately
error = (distance * 0.8898 - encoder_7c.position(TURNS)) * speed
if error > 150:
error = 150
drivetrain.set_drive_velocity(error, RPM)
if drivetrain.velocity(RPM) < 3.6:
if drivetrain.velocity(RPM) > -3.6:
drivetrain.set_drive_velocity(10, RPM)
if error < 3.6:
if error > -3.6:
break
drivetrain.stop()
print('position found')
def score_rings():
rearcarrier.set_velocity(25, PERCENT)
if potentiometerc.angle(DEGREES) > 31:
rearcarrier.spin(FORWARD)
while potentiometerc.angle(DEGREES) > 29:
pass
rearcarrier.stop()
rings.set_velocity(100, PERCENT)
rings.spin(FORWARD)
def turn_to_position(degrees):
drivetrain.set_stopping(BRAKE)
brain.timer.clear()
#drivetrain.set_turn_velocity(100, PERCENT)
drivetrain.turn(RIGHT)
error = 1
while not round(error) * 20 == 0:
#set the speed the robot turns based off of
# how close to the target it is
heading = inertial.heading(DEGREES)
#reverse the heading if it is greater than 180 degrees
if degrees > 180:
degrees = (degrees - 180) + -180
if inertial.heading(DEGREES) > 180:
if degrees < 170:
heading = (inertial.heading(DEGREES) - 180) - 180
error = (degrees - heading) / 1
drivetrain.set_turn_velocity(error, RPM)
print(heading)
if error < 1:
if error > -1:
drivetrain.set_turn_velocity(10, RPM)
drivetrain.turn(RIGHT)
if brain.timer.time(SECONDS) > 3:
break
print('yay')
drivetrain.stop()
wait(100, MSEC)
def rear_carrier_down():
rearcarrier.spin(REVERSE)
while potentiometerc.angle(DEGREES) > 67:
print(potentiometerc.angle(DEGREES))
rearcarrier.stop()
def rear_carrier_up():
rearcarrier.spin(FORWARD)
while not potentiometerc.angle(DEGREES) < 40:
pass
rearcarrier.stop()
#************START SEQUENCE************#
#************START SEQUENCE************#
#************START SEQUENCE************#
#************START SEQUENCE************#
#go_to_position(0,0, True)
#wait(100000, SECONDS)
my_event.broadcast()
brain.timer.clear()
drivetrain.set_stopping(BRAKE)
brain.timer.clear()
print('AAAAAAAAHHHHHHHH')
rearcarrier.spin(REVERSE)
wait(1, SECONDS)
goalcarrier.spin_to_position(500, DEGREES, wait=False)
drive(-1.75)
score_rings()
drive(1)
drive(-0.75)
goalcarrier.spin(REVERSE)
turn_to_position(-90)
my_event.broadcast()
drive(7.5)
turn_to_position(0)
rearcarrier.spin(REVERSE)
rearcarrier.set_velocity(100, PERCENT)
turn_to_position(0)
wait(1, SECONDS)
drive(2.5)
turn_to_position(-90)
rearcarrier.spin_to_position(0, DEGREES, wait=False)
drive(1)
goalcarrier.spin(REVERSE)
wait(0.5, SECONDS)
goalcarrier.spin(FORWARD)
wait(0.5, SECONDS)
claw1.set(False)
goalcarrier.spin(REVERSE)
goalcarrier.spin(REVERSE)
goalcarrier.spin(REVERSE)
goalcarrier.spin(REVERSE)
goalcarrier.spin(REVERSE)
goalcarrier.spin(REVERSE)
drive(-1)
turn_to_position(-175)
drivetrain.drive(FORWARD)
my_event.broadcast()
while not limit_switch_b.pressing():
drivetrain.set_drive_velocity(100, PERCENT)
wait(0.5, SECONDS)
drive((gps_6.y_position(INCHES) + 6) / 12)
turn_to_position(-90)
drive(1)
claw1.set(False)
def driver_control():
speed2 = 5.8
while True:
drivetrain.set_stopping(BRAKE)
if controller_1.buttonX.pressing():
claw1.set(True)
if controller_1.buttonB.pressing():
claw1.set(False)
rearcarrier.set_stopping(HOLD)
if controller_1.buttonUp.pressing():
rings.spin(FORWARD)
if controller_1.buttonDown.pressing():
rings.stop()
if gps_6.x_position(INCHES) > 0 and gps_6.y_position(INCHES) < 0:
print("ASGDHGGFDFGHGFDFGHGFGHGFGHG")
if controller_1.buttonLeft.pressing():
speed2 += 0.05
if controller_1.buttonRight.pressing():
speed2 -= 0.05
controller_1.screen.clear_screen()
controller_1.screen.set_cursor(1,1)
controller_1.screen.print(speed2)
competition = Competition(driver_control, autonomous)
I removed the junk I wasn’t using.
edit: mods added code tags, please remember to use them