Hello everyone, we’re a first-year team that recently qualified for states through skills, but our autonomous code is still a major weak point. It’s very inconsistent: the turn values don’t always behave as expected, and sometimes the robot drives strangely (one side moves more than the other, or it turns the wrong way). I’ve pasted our current autonomous code below if anyone could help us diagnose what’s going wrong or point out anything obviously incorrect, it would be a huge help. We’ve been trying to fix this for a long time, but none of our programmers have been able to figure it out yet.
PS: we used python in vexcode v5
#region VEXcode Generated Robot Configuration
from vex import *
import urandom
import math
# Brain should be defined by default
brain=Brain()
# Robot configuration code
# 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()
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
#region VEXcode Generated Robot Configuration
from vex import *
import urandom
import math
# Brain should be defined by default
brain=Brain()
# Robot configuration code
Right_motor_a = Motor(Ports.PORT1, GearSetting.RATIO_6_1, True)
Right_motor_b = Motor(Ports.PORT2, GearSetting.RATIO_6_1, True)
Right = MotorGroup(Right_motor_a, Right_motor_b)
RightExZ = Motor(Ports.PORT3, GearSetting.RATIO_6_1, True)
Left_motor_a = Motor(Ports.PORT11, GearSetting.RATIO_6_1, False)
Left_motor_b = Motor(Ports.PORT12, GearSetting.RATIO_6_1, False)
Left = MotorGroup(Left_motor_a, Left_motor_b)
LeftExZ = Motor(Ports.PORT13, GearSetting.RATIO_6_1, False)
controller_1 = Controller(PRIMARY)
IMU = Inertial(Ports.PORT10)
Intake_Motor_Top = Motor(Ports.PORT4, GearSetting.RATIO_18_1, False)
Outtake = Motor(Ports.PORT5, GearSetting.RATIO_6_1, False)
digital_out_a = DigitalOut(brain.three_wire_port.a)
digital_out_b = DigitalOut(brain.three_wire_port.b)
# wait for rotation sensor to fully initialize
wait(30, MSEC)
# Make random actually random
# Set random seed
initializeRandomSeed()
# 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")
# -------------------------------
# Constants
# -------------------------------
AUTO_VELOCITY = 15 # percent
AUTO_TORQUE = 100 # percent
TRACK_CIRCUMFERENCE = 3.14159 * 9
AUTON_RIGHT_TRIM = 5 # try 5, adjust up/down as needed
# -------------------------------
# Drive Helper Functions
# -------------------------------
def LeftDrive_Power(power):
Left.set_velocity(power, PERCENT)
LeftExZ.set_velocity(power,PERCENT)
Left.spin(FORWARD)
LeftExZ.spin(FORWARD)
def RightDrive_Power(power):
Right.set_velocity(power, PERCENT)
RightExZ.set_velocity(power, PERCENT)
Right.spin(FORWARD)
RightExZ.spin(FORWARD)
# -------------------------------
# Autonomous Motor Configuration
# -------------------------------
def configure_auton_motors():
Left.set_max_torque(AUTO_TORQUE, PERCENT)
LeftExZ.set_max_torque(AUTO_TORQUE, PERCENT)
Right.set_max_torque(AUTO_TORQUE, PERCENT)
RightExZ.set_max_torque(AUTO_TORQUE, PERCENT)
Left.set_velocity(AUTO_VELOCITY, PERCENT)
LeftExZ.set_velocity(AUTO_VELOCITY, PERCENT)
Right.set_velocity(AUTO_VELOCITY, PERCENT)
RightExZ.set_velocity(AUTO_VELOCITY, PERCENT)
# -------------------------------
# Autonomous Movement Functions
# -------------------------------
def AutonForward(distance_inches: float):
actual_distance = distance_inches * 2
degrees_to_spin = (actual_distance / TRACK_CIRCUMFERENCE) * 360
Left_degrees_to_spin = (degrees_to_spin * 1)
Left.set_velocity(AUTO_VELOCITY, PERCENT)
LeftExZ.set_velocity(AUTO_VELOCITY, PERCENT)
Right.set_velocity(AUTO_VELOCITY, PERCENT)
RightExZ.set_velocity(AUTO_VELOCITY, PERCENT)
Left.spin_for(FORWARD, Left_degrees_to_spin, DEGREES, wait=False)
LeftExZ.spin_for(FORWARD, Left_degrees_to_spin, DEGREES, wait=False)
Right.spin_for(FORWARD, degrees_to_spin, DEGREES, wait=False)
RightExZ.spin_for(FORWARD, degrees_to_spin, DEGREES, wait=True) # Wait for all to finish
def SmoothTurn(target_angle: float, max_speed=50, min_speed=2, kP=0.20, tolerance=2.0):
"""
Smoothly turns the robot to a specified IMU heading (degrees).
Slows as it approaches the target to avoid overshooting.
- target_angle: desired IMU heading (0-360)
- max_speed: maximum motor percent speed
- min_speed: minimum speed to avoid stalling
- kP: proportional gain (tune for your robot)
- tolerance: how close (degrees) to consider 'done'
"""
while True:
current = IMU.heading(DEGREES)
error = (target_angle - current + 540) % 360 - 180 # shortest path
if abs(error) <= tolerance:
Left.stop(BRAKE)
LeftExZ.stop(BRAKE)
Right.stop(BRAKE)
RightExZ.stop(BRAKE)
break
# Proportional speed, clamped
speed = kP * error
speed = max(-max_speed, min(max_speed, speed))
# Apply minimum speed to prevent stalling
if 0 < abs(speed) < min_speed:
speed = min_speed if speed > 0 else -min_speed
Left.spin(FORWARD if speed > 0 else REVERSE, abs(speed), PERCENT)
LeftExZ.spin(FORWARD if speed > 0 else REVERSE, abs(speed), PERCENT)
Right.spin(REVERSE if speed > 0 else FORWARD, abs(speed), PERCENT)
RightExZ.spin(REVERSE if speed > 0 else FORWARD, abs(speed), PERCENT)
wait(10, MSEC)
# -------------------------------
# Driver Control
# -------------------------------
def ondriver_drivercontrol_0():
Left.set_stopping(BRAKE)
LeftExZ.set_stopping(BRAKE)
Right.set_stopping(BRAKE)
RightExZ.set_stopping(BRAKE)
Intake_Motor_Top.set_velocity(100, PERCENT)
Outtake.set_velocity(100, PERCENT)
while True:
left_power = controller_1.axis2.position()
right_power = controller_1.axis3.position()
LeftDrive_Power(left_power)
RightDrive_Power(right_power)
if controller_1.buttonB.pressing():
Intake_Motor_Top.spin(REVERSE)
elif controller_1.buttonX.pressing():
Intake_Motor_Top.spin(FORWARD)
elif controller_1.buttonY.pressing():
Intake_Motor_Top.stop()
if controller_1.buttonR1.pressing():
Outtake.spin(FORWARD)
elif controller_1.buttonR2.pressing():
Outtake.spin(REVERSE)
else:
Outtake.stop()
if controller_1.buttonL1.pressing():
digital_out_a.set(True)
elif controller_1.buttonL2.pressing():
digital_out_a.set(False)
if controller_1.buttonUp.pressing():
digital_out_b.set(False)
elif controller_1.buttonDown.pressing():
digital_out_b.set(True)
wait(10, MSEC)
# -------------------------------
# Autonomous Routine
# -------------------------------
def onauton_autonomous_0():
configure_auton_motors()
# --- IMU Calibration ---
IMU.calibrate()
while IMU.is_calibrating():
wait(20, MSEC)
Right.set_stopping(BRAKE)
RightExZ.set_stopping(BRAKE)
Left.set_stopping(BRAKE)
LeftExZ.set_stopping(BRAKE)
Intake_Motor_Top.set_velocity(100,PERCENT)
Intake_Motor_Top.spin(REVERSE)
Outtake.set_velocity(100,PERCENT)
Outtake.spin(REVERSE)
AutonForward(34)
wait(200, MSEC) # Ensure robot is stopped
current_heading = IMU.heading(DEGREES)
SmoothTurn((current_heading + -45) % 360) # Turn 90 left
AutonForward(34)
wait(200, MSEC)
current_heading = IMU.heading(DEGREES)
SmoothTurn((current_heading - 120) % 360) # Turn 45 right
AutonForward(45)
current_heading = IMU.heading(DEGREES)
SmoothTurn((current_heading - 37) % 360) # Turn 45 right
AutonForward(-30)
Outtake.spin(FORWARD)
# -------------------------------
# Competition Setup
# -------------------------------
competition = Competition(ondriver_drivercontrol_0, onauton_autonomous_0)