def driver():
"""
| This is the Teleop mode where the driver has the control over the robot.
| The fucntion runs the claw, base and wrist in different threads
"""
print("TELEOP running")
sys.run_in_thread(CLAW_DRIVE)
sys.run_in_thread(BASE_DRIVE)
sys.run_in_thread(WRIST_DRIVE)
lower_or_raise_arm_by_controller()
def grab_or_release_object_by_controller():
if CONTROLLER.buttonRDown.pressing():
CLAW_MOTOR.spin(
DirectionType.REV, # dir
CLAW_MOTOR_VELOCITY_PERCENT, # velocity
VelocityUnits.PCT # velocityUnit
)
elif CONTROLLER.buttonRUp.pressing():
CLAW_MOTOR.spin(
DirectionType.FWD, # dir
CLAW_MOTOR_VELOCITY_PERCENT, # velocity
VelocityUnits.PCT # velocityUnit
)
else:
CLAW_MOTOR.stop(BrakeType.HOLD)
def keep_grabbing_or_releasing_objects_by_controller():
while True:
grab_or_release_object_by_controller()
run_in_thread(keep_lowering_or_raising_arm_by_controller)
run_in_thread(keep_grabbing_or_releasing_objects_by_controller)
keep_driving_by_controller()
def control_claw_by_controller_right_buttons():
# open claw
if CONTROLLER.buttonRDown.pressing():
CLAW_MOTOR.spin(
DirectionType.REV, # dir
CLAW_MOTOR_VELOCITY_PERCENT, # velocity
VelocityUnits.PCT # velocityUnit
)
# close claw
elif CONTROLLER.buttonRUp.pressing():
CLAW_MOTOR.spin(
DirectionType.FWD, # dir
CLAW_MOTOR_VELOCITY_PERCENT, # velocity
VelocityUnits.PCT # velocityUnit
)
else:
CLAW_MOTOR.stop(BrakeType.HOLD)
def keep_controlling_claw_by_controller_right_buttons():
while True:
control_claw_by_controller_right_buttons()
run_in_thread(keep_controlling_shoulder_by_controller_axis_d)
run_in_thread(keep_controlling_elbow_by_controller_axis_a)
run_in_thread(keep_controlling_claw_by_controller_right_buttons)
keep_pivoting_base_by_controller_left_buttons()
motor_left = vex.Motor(vex.Ports.PORT20, vex.GearSetting.RATIO18_1, False)
dt = vex.Drivetrain(motor_left, motor_right, 319.1764, 292.1,
vex.DistanceUnits.MM)
arm = vex.Motor(vex.Ports.PORT2, vex.GearSetting.RATIO18_1, False)
arm.reset_rotation()
arm.rotate_to(30, RotationUnits.DEG, 30, vex.VelocityUnits.PCT, True)
arm.stop()
#encoder_right = vex.Encoder(brain.three_wire_port.e)
#encoder_left = vex.Encoder(brain.three_wire_port.b)
#encd = vex.Encoder(brain.three_wire_port.d)
#encd.reset_rotation()
#encoder_right.reset_rotation()
#encoder_left.reset_rotation()
'''
pid_right = pidmotor(motor_right, encoder_right)
pid_left = pidmotor(motor_left, encoder_left)
sys.run_in_thread(pid_right.run())
sys.run_in_thread(pid_left.run())
'''
while True:
direction = vex.DirectionType.FWD
y_axis = con.axis3.position()
x_axis = con.axis4.position()
angle_rad = math.atan2(y_axis, x_axis)
angle_deg = math.degrees(angle_rad)
motor_11 = vexiq.Motor(11) motor_12 = vexiq.Motor(12, True) # Reverse Polarity import drivetrain dt = drivetrain.Drivetrain(motor_7, motor_12, 200, 176) #endregion config item = None def thread2(): global item sys.wait_for(lambda: item == 1) motor_11.run_until_time(100, 1.12, True) sys.wait_for(lambda: item == 0) motor_11.run_until_time(-(100), 0.25, True) sys.run_in_thread(thread2) # main thread item = 0 dt.turn_until(100, 52.5) dt.drive_until(100, 435) item = 1 motor_8.run_until_time(-(100), 1.12, True) dt.turn_until(50, 92) dt.drive_until(100, 100) item = 0 motor_8.run_until_time(100, 0.25, True) dt.drive_until(-(50), 200)