TBH Controller Question

Hello,
I am trying to code a TBH (Take Back Half) controller for my flywheel in order to get a more accurate RPM, and to ramp up my flywheel faster. (Side note, will this give a more accurate RPM as compared to pure voltage control?) However, I’m encountering some problems. The code I found for this controller is very old, and is in C++, a language which I am unfamiliar with. I was wondering if anyone had some example code for this in python, or if someone could make some pseudo code that I can adapt to python. I know @jpearman made the original topic and has a large amount of knowledge on this topic, so I’m tagging him. Any help would be appreciated!

Slight update. I’ve got a hold of a cortex microcontroller and ran the code with an old motor and all of that. As far as I can understand (and from what I’ve read), it is kind of like a basic i loop? Can anyone else help me understand what’s going on with a TBH? Grateful for all the help I could get.

As you said, a TBH controller is sort of an I loop.
Basically, the motor velocity is just increased by the error and a gain value:

velocity += error * gain

but, when the error goes from negative to positive, or vice-versa, the new velocity is just the average of the tbh value and the last velocity, and the tbh is set to the new velocity:

when (signof(error) != signof(lastError):
    velocity = (lastVelocity + tbh) / 2
    tbh = velocity

To answer your other question, a tbh controller will give more accurate flywheel speed than voltage control

Another helpful resource about tbh controllers is the sigbots wiki’s entry on it

5 Likes

Hey! Thank you for your help. I’ve developed some code, and I see that it does affect the flywheel speed, but it is very jerky, and it seems to only go between 2 volts and 12 volts and some weird inbetween value. I’ve tried changing the integral value to no avail. Here is my code:

def pid_loop():
ToggleBool = 0
Var1 = 0
min_voltage = 2
max_voltage = 12
target_rpm = 2600
integral = 0
Ki = 0.001  
integral_limit = .005 
max_integral= .005 



#    global integral, current_rpm, error, output_voltage 
#    # Run the PID loop continuously
#    while True:
#        # Get the current RPM of the flywheel
#        current_rpm =Flywheel_Sensor.velocity(VelocityUnits.RPM)

        # Calculate the error between the current RPM and the target RPM
#        error = target_rpm - current_rpm

#        if abs(error) > 0:
#            # Update the integral error
#            integral += error
            # Clamp the integral term to the integral limit
#            integral = min(integral_limit, max(-integral_limit, integral))
#        elif abs(error) == 0:
#            integral = 0
        # Update the integral error


#        integral += error


#       integral = min(max_integral, integral)

        
        # Calculate the output voltage based on the error, integral and Ki constant
#        output_voltage = error + (Ki * integral)
        
        # Constrain the output voltage to the minimum and maximum limits
#        output_voltage = min(max_voltage, max(min_voltage, output_voltage))
        
        # Set the flywheel motor speeds based on the output voltage
#        Flywheel_Motors.spin(FORWARD, output_voltage, VOLT)

Ignore the fact that it’s in comments and you can’t see the thread being declared. Also, the variables are being declared outside of the program.

Another update. I’m still having issues with the oscillating, I’ve switched out motors and stuff so I know it isn’t a hardware issue, and I’ve tried messing around with my code, but to no avail. I would appreciate any help I can get on this confusing issue.

Super hard to fix things

# My code - only showing the comments since the code matches

#End of my code - see we should win, but we don't

Please send us the real code, we can’t fix just the comments.

As a tip for the future, only send the code that u need help with. Sending the entire code makes it hard for those tryna help you, help you

1 Like

def pid_loop():
ToggleBool = 0
Var1 = 0
min_voltage = 2
max_voltage = 12
target_rpm = 2600
integral = 0
Ki = 0.001  
integral_limit = .005 
max_integral= .005 



    global integral, current_rpm, error, output_voltage 
    # Run the PID loop continuously
    while True:
        # Get the current RPM of the flywheel
       current_rpm =Flywheel_Sensor.velocity(VelocityUnits.RPM)

        # Calculate the error between the current RPM and the target RPM
        error = target_rpm - current_rpm

        if abs(error) > 0:
            # Update the integral error
            integral += error
            # Clamp the integral term to the integral limit
            integral = min(integral_limit, max(-integral_limit, integral))
        elif abs(error) == 0:
            integral = 0
        # Update the integral error


        integral += error


      integral = min(max_integral, integral)

        
        # Calculate the output voltage based on the error, integral and Ki constant
        output_voltage = error + (Ki * integral)
        
        # Constrain the output voltage to the minimum and maximum limits
        output_voltage = min(max_voltage, max(min_voltage, output_voltage))
        
        Set the flywheel motor speeds based on the output voltage
       Flywheel_Motors.spin(FORWARD, output_voltage, VOLT)

pid_thread = Thread(pid_loop)
pid_loop()

I’m getting mixed messages. More code? Less code? This is the real code, it’s just integrated into my main program.

Actually no it doesn’t make it harder. It makes it worse to get a fragment. And you should know that @JThomas you’ve been here long enough to know that. Lets take a look at code to help a roboteer out:

Hey my turbobolift holo shooter doesn’t work, here is the code:

do_magic_stuff ('win', shooter_direction, Holo_instance_reference);

is pretty worthless.

Sorry to take such a harsh line on this, but VEX has two seasons. Season one is May-June in the 45 days where teams go crazy learning the game. Season two is now: end of Jan to 10 March. About 45 days of people begging for help. More data is always better. @RobotProgrammmer gave the entire program. People can download it, try it on their brain to help the user. A small fragment as @JThomas suggest is useless. I’ve given hearts to thousands of James posts asking for the entire code base.

More info is always better. All the code is always better. Pictures of all the sides of the robot is always better. Videos are always better.
We are trying to fix complex stuff from letters on a screen. Help us help you.

And since I’m clearly on a rant, doing text speak (ie U = You, tryna = trying to help you) etc, doesn’t display and increase in anyone’s brain that you are using your A game. Nouns, verbs, spelling, sentence constructs are all key to communications.

Here is a post from 2020 that will be a huge help to anyone today.

Take a few mins to look and read it. Happy to help you Help us help you.

3 Likes
from vex import *
import urandom

# Brain should be defined by default
brain=Brain()

# Robot configuration code
left_motor_a = Motor(Ports.PORT1, GearSetting.RATIO_6_1, True)
left_motor_b = Motor(Ports.PORT2, GearSetting.RATIO_6_1, True)
left_drive_smart = MotorGroup(left_motor_a, left_motor_b)
right_motor_a = Motor(Ports.PORT3, GearSetting.RATIO_6_1, False)
right_motor_b = Motor(Ports.PORT4, GearSetting.RATIO_6_1, False)
right_drive_smart = MotorGroup(right_motor_a, right_motor_b)
drivetrain_inertial = Inertial(Ports.PORT5)
drivetrain = SmartDrive(left_drive_smart, right_drive_smart, drivetrain_inertial, 299.24, 320, 40, MM, 1.6666666666666667)
Flywheel_Motors_motor_a = Motor(Ports.PORT6, GearSetting.RATIO_6_1, False)
Flywheel_Motors_motor_b = Motor(Ports.PORT7, GearSetting.RATIO_6_1, True)
Flywheel_Motors = MotorGroup(Flywheel_Motors_motor_a, Flywheel_Motors_motor_b)
IntakeMotor = Motor(Ports.PORT8, GearSetting.RATIO_6_1, True)
ExpansionMotor = Motor(Ports.PORT9, GearSetting.RATIO_6_1, True)
# vex-vision-config:begin
VisionLeft__VEX_RED = Signature(1, 6825, 8033, 7430,-1185, -449, -818,5, 0)
VisionLeft__VEX_BLUE = Signature(2, -3245, -2149, -2698,3413, 10031, 6722,1.5, 0)
VisionLeft = Vision(Ports.PORT10, 50, VisionLeft__VEX_RED, VisionLeft__VEX_BLUE)
# vex-vision-config:end
# vex-vision-config:begin
VisionRight__VEXRED = Signature(1, 7279, 8577, 7928,-1215, -431, -822,2.5, 0)
VisionRight__VEXBLUE = Signature(2, -2621, -755, -1688,3953, 9011, 6482,1.5, 0)
VisionRight = Vision(Ports.PORT11, 50, VisionRight__VEXRED, VisionRight__VEXBLUE)
# vex-vision-config:end
# vex-vision-config:begin
VisionRoller__ROLLER_RED = Signature(1, 6897, 8311, 7604,-1197, -369, -784,3, 0)
VisionRoller__ROLLER_BLUE = Signature(2, -3049, -2121, -2586,6207, 9425, 7816,2.5, 0)
VisionRoller = Vision(Ports.PORT12, 50, VisionRoller__ROLLER_RED, VisionRoller__ROLLER_BLUE)
# vex-vision-config:end
DistanceLeft = Distance(Ports.PORT13)
DistanceRight = Distance(Ports.PORT14)
BackupDistance = Sonar(brain.three_wire_port.a)
LeftGPS = Gps(Ports.PORT15, 0.00, 0.00, MM, 180)
RightGPS = Gps(Ports.PORT16, 0.00, 0.00, MM, 180)
Flywheel_Sensor = Rotation(Ports.PORT17, False)
Roller_Sensor = Rotation(Ports.PORT18, False)
Pneumatic = DigitalOut(brain.three_wire_port.c)
Pneumatic2 = DigitalOut(brain.three_wire_port.d)
SensorLeft = Line(brain.three_wire_port.e)
SensorRight = Line(brain.three_wire_port.f)
controller_1 = Controller(PRIMARY)
controller_2 = Controller(PARTNER)


# wait for rotation sensor to fully initialize
wait(30, MSEC)

def calibrate_drivetrain():
    # Calibrate the Drivetrain Inertial
    sleep(200, MSEC)
    brain.screen.print("Calibrating")
    brain.screen.next_row()
    brain.screen.print("Inertial")
    drivetrain_inertial.calibrate()
    while drivetrain_inertial.is_calibrating():
        sleep(25, MSEC)
    brain.screen.clear_screen()
    brain.screen.set_cursor(1, 1)



# define variables used for controlling motors based on controller inputs
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, 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:
            # stop the motors if the brain is calibrating
            if drivetrain_inertial.is_calibrating():
                left_drive_smart.stop()
                right_drive_smart.stop()
                while drivetrain_inertial.is_calibrating():
                    sleep(25, MSEC)
            
            # 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)
        # wait before repeating the process
        wait(20, MSEC)

# define variables used for controlling motors based on controller inputs
controller_2_left_shoulder_control_motors_stopped = True
controller_2_right_shoulder_control_motors_stopped = True

# define a task that will handle monitoring inputs from controller_2
def rc_auto_loop_function_controller_2():
    global controller_2_left_shoulder_control_motors_stopped, controller_2_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:
            # check the buttonL1/buttonL2 status
            # to control ExpansionMotor
            if controller_2.buttonL1.pressing():
                ExpansionMotor.spin(FORWARD)
                controller_2_left_shoulder_control_motors_stopped = False
            elif controller_2.buttonL2.pressing():
                ExpansionMotor.spin(REVERSE)
                controller_2_left_shoulder_control_motors_stopped = False
            elif not controller_2_left_shoulder_control_motors_stopped:
                ExpansionMotor.stop()
                # set the toggle so that we don't constantly tell the motor to stop when
                # the buttons are released
                controller_2_left_shoulder_control_motors_stopped = True
            # check the buttonR1/buttonR2 status
            # to control IntakeMotor
            if controller_2.buttonR1.pressing():
                IntakeMotor.spin(FORWARD)
                controller_2_right_shoulder_control_motors_stopped = False
            elif controller_2.buttonR2.pressing():
                IntakeMotor.spin(REVERSE)
                controller_2_right_shoulder_control_motors_stopped = False
            elif not controller_2_right_shoulder_control_motors_stopped:
                IntakeMotor.stop()
                # set the toggle so that we don't constantly tell the motor to stop when
                # the buttons are released
                controller_2_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)
rc_auto_loop_thread_controller_2 = Thread(rc_auto_loop_function_controller_2)
#endregion VEXcode Generated Robot Configuration

vexcode_brain_precision = 0
vexcode_console_precision = 0
vexcode_controller_1_precision = 0
vexcode_controller_2_precision = 0
vexcode_visionleft_objects = None
vexcode_visionright_objects = None
vexcode_rollervision_objects = None
ToggleBool = 0
Var1 = 0
min_voltage = 2
max_voltage = 12
target_rpm = 2600
integral = 0
Ki = 0.001  
integral_limit = .005 
max_integral= .005 

def onauton_autonomous_0():
    global myVariable, vexcode_brain_precision, vexcode_console_precision, vexcode_controller_1_precision, vexcode_visionleft_objects, vexcode_visionright_objects, vexcode_rollervision_objects
    IntakeMotor.set_velocity(100, PERCENT)
    ExpansionMotor.set_velocity(100, PERCENT)  
    drivetrain.set_drive_velocity(75, PERCENT)
    drivetrain.set_turn_velocity(75, PERCENT)
    Flywheel_Motors.spin(FORWARD, 9.5, VOLT)
    #Velocity Set ^
    left_drive_smart.spin(REVERSE, 3, VOLT)
    right_drive_smart.spin(REVERSE, 3, VOLT)
    ExpansionMotor.spin_for(FORWARD, 90, DEGREES)
    wait(.2, SECONDS)
    drivetrain.drive_for(FORWARD, 10, INCHES)
    drivetrain.turn_for(RIGHT, 10, DEGREES)
    ExpansionMotor.spin_for(FORWARD, 45, TURNS)
    IntakeMotor.spin_for(FORWARD, 45, TURNS)
    wait(4, SECONDS)
    drivetrain.turn_for(LEFT, 10, DEGREES)
    drivetrain.drive_for(FORWARD, 48, INCHES)
    drivetrain.turn_for(LEFT, 90, DEGREES)
    left_drive_smart.spin(REVERSE, 3, VOLT)
    right_drive_smart.spin(REVERSE, 3, VOLT)
    wait(.5, SECONDS)
    ExpansionMotor.spin_for(FORWARD, 90, DEGREES)
    wait(.1, SECONDS)
    ExpansionMotor.spin_for(FORWARD, 60, TURNS)
    drivetrain.drive_for(FORWARD, 36, INCHES)
    drivetrain.turn_for(LEFT, 10, DEGREES)
    ExpansionMotor.spin_for(FORWARD, 45, TURNS)
    IntakeMotor.spin_for(FORWARD, 45, TURNS)
    wait(4, SECONDS)
    drivetrain.turn_for(RIGHT, 10, DEGREES)

def pid_loop():
    global integral, current_rpm, error, output_voltage 
#    # Run the PID loop continuously
    while True:
#        # Get the current RPM of the flywheel
        current_rpm =Flywheel_Sensor.velocity(VelocityUnits.RPM)

        # Calculate the error between the current RPM and the target RPM
        error = target_rpm - current_rpm

        if abs(error) > 0:
#            # Update the integral error
            integral += error
            # Clamp the integral term to the integral limit
            integral = min(integral_limit, max(-integral_limit, integral))
        elif abs(error) == 0:
            integral = 0
        # Update the integral error


        integral += error


       integral = min(max_integral, integral)

        
        # Calculate the output voltage based on the error, integral and Ki constant
        output_voltage = error + (Ki * integral)
        
        # Constrain the output voltage to the minimum and maximum limits
        output_voltage = min(max_voltage, max(min_voltage, output_voltage))
        
        # Set the flywheel motor speeds based on the output voltage
        Flywheel_Motors.spin(FORWARD, output_voltage, VOLT)






def when_started1():
    global myVariable, vexcode_brain_precision, vexcode_console_precision, vexcode_controller_1_precision, vexcode_visionleft_objects, vexcode_visionright_objects, vexcode_rollervision_objects
    drivetrain.set_drive_velocity(75, PERCENT)
    drivetrain.set_turn_velocity(75, PERCENT)
    IntakeMotor.set_velocity(100, PERCENT)
    ExpansionMotor.set_velocity(100, PERCENT)
    Flywheel_Motors.set_velocity(100, PERCENT)
    Flywheel_Motors.set_stopping(COAST)

def ondriver_drivercontrol_0():
    global myVariable, vexcode_brain_precision, vexcode_console_precision, vexcode_controller_1_precision, vexcode_visionleft_objects, vexcode_visionright_objects, vexcode_rollervision_objects
    IntakeMotor.set_velocity(100, PERCENT)
    ExpansionMotor.set_velocity(100, PERCENT)
    Flywheel_Motors.set_velocity(100, PERCENT)
    Flywheel_Motors.set_stopping(COAST)

def onevent_controller_1buttonR1_pressed_0():
    global myVariable, vexcode_brain_precision, vexcode_console_precision, vexcode_controller_1_precision, vexcode_visionleft_objects, vexcode_visionright_objects, vexcode_rollervision_objects
    Flywheel_Motors.spin(FORWARD, 9.75, VOLT)
    #Testing Middle line aiming. Start at 9.35V




def onevent_controller_1buttonLeft_pressed_0():
    global myVariable, vexcode_brain_precision, vexcode_console_precision, vexcode_controller_1_precision, vexcode_visionleft_objects, vexcode_visionright_objects, vexcode_rollervision_objects, vexcode_controller_2_precision
    left_drive_smart.spin(REVERSE, 2.5, VOLT)
    right_drive_smart.spin(REVERSE, 2.5, VOLT)
    ExpansionMotor.spin(FORWARD)
    #Get Roller
    wait(.25, SECONDS)
    ExpansionMotor.stop()
    left_drive_smart.stop()
    right_drive_smart.stop()
    drivetrain.drive_for(FORWARD, 75, INCHES)
    drivetrain.turn_for(RIGHT, 90, DEGREES)
    wait(.5, SECONDS)
    drivetrain.drive_for(REVERSE, 75, INCHES)
    left_drive_smart.spin(REVERSE, 3, VOLT)
    right_drive_smart.spin(REVERSE, 3, VOLT)
    wait(2, SECONDS)
    left_drive_smart.stop()
    right_drive_smart.stop()
    ExpansionMotor.spin(FORWARD)
    wait(.25, SECONDS)
    ExpansionMotor.stop()



def onevent_controller_1buttonUp_pressed_0():
    global myVariable, vexcode_brain_precision, vexcode_console_precision, vexcode_controller_1_precision, vexcode_visionleft_objects, vexcode_visionright_objects, vexcode_rollervision_objects, vexcode_controller_2_precision
    left_drive_smart.spin(REVERSE, 2.5, VOLT)
    right_drive_smart.spin(REVERSE, 2.5, VOLT)
    ExpansionMotor.spin(FORWARD)
    #Get Roller
    wait(.4, SECONDS)
    ExpansionMotor.stop()
    left_drive_smart.stop()
    right_drive_smart.stop()
    
def onevent_controller_1buttonR2_pressed_0():
    global myVariable, vexcode_brain_precision, vexcode_console_precision, vexcode_controller_1_precision, vexcode_visionleft_objects, vexcode_visionright_objects, vexcode_rollervision_objects
    Flywheel_Motors.stop()

def onevent_controller_1buttonL1_pressed_0():
    global myVariable, vexcode_brain_precision, vexcode_console_precision, vexcode_controller_1_precision, vexcode_visionleft_objects, vexcode_visionright_objects, vexcode_rollervision_objects
    Pneumatic2.set(True)    
    Pneumatic.set(True)


def onevent_controller_1buttonL2_pressed_0():
    global myVariable, vexcode_brain_precision, vexcode_console_precision, vexcode_controller_1_precision, vexcode_visionleft_objects, vexcode_visionright_objects, vexcode_rollervision_objects
    Pneumatic2.set(False)    
    Pneumatic.set(False)


#def onevent_controller_1buttonL1_released_0():
 #   global myVariable, vexcode_brain_precision, vexcode_console_precision, vexcode_controller_1_precision, vexcode_visionleft_objects, vexcode_visionright_objects, vexcode_rollervision_objects
  #  Pneumatic2.set(True)
  #  wait(.1, SECONDS)
  #  Pneumatic2.set(False)

#def onevent_controller_1buttonLeft_pressed_0():
   # global myVariable, vexcode_brain_precision, vexcode_console_precision, vexcode_controller_1_precision, vexcode_visionleft_objects, vexcode_visionright_objects, vexcode_rollervision_objects
  #  drivetrain.set_drive_velocity(50, PERCENT)
  #  drivetrain.set_turn_velocity(50, PERCENT)
  #  drivetrain.drive(FORWARD)

 
#def onevent_controller_1buttonLeft_released_0():
  #  global myVariable, vexcode_brain_precision, vexcode_console_precision, vexcode_controller_1_precision, vexcode_visionleft_objects, vexcode_visionright_objects, vexcode_rollervision_objects


def onevent_controller_2buttonX_pressed_0():
    global myVariable, vexcode_brain_precision, vexcode_console_precision, vexcode_controller_1_precision, vexcode_visionleft_objects, vexcode_visionright_objects, vexcode_rollervision_objects, vexcode_controller_2_precision
    Flywheel_Motors.spin(FORWARD, 9.75, VOLT)

def onevent_controller_2buttonY_pressed_0():
    global myVariable, vexcode_brain_precision, vexcode_console_precision, vexcode_controller_1_precision, vexcode_visionleft_objects, vexcode_visionright_objects, vexcode_rollervision_objects, vexcode_controller_2_precision
    Flywheel_Motors.spin(FORWARD, 8.8, VOLT)

def onevent_controller_2buttonA_pressed_0():
    global myVariable, vexcode_brain_precision, vexcode_console_precision, vexcode_controller_1_precision, vexcode_visionleft_objects, vexcode_visionright_objects, vexcode_rollervision_objects, vexcode_controller_2_precision
    Flywheel_Motors.spin(FORWARD, 8.8, VOLT)

def onevent_controller_2buttonB_pressed_0():
    global myVariable, vexcode_brain_precision, vexcode_console_precision, vexcode_controller_1_precision, vexcode_visionleft_objects, vexcode_visionright_objects, vexcode_rollervision_objects, vexcode_controller_2_precision
    Flywheel_Motors.spin(FORWARD, 7.6, VOLT)

def when_started2():
    global myVariable, vexcode_brain_precision, vexcode_console_precision, vexcode_controller_1_precision, vexcode_visionleft_objects, vexcode_visionright_objects, vexcode_rollervision_objects
    brain.screen.set_font(FontType.PROP20)
    LeftGPS.calibrate()
    RightGPS.calibrate()

    while LeftGPS.is_calibrating():
        sleep(25) #change me 50
    while RightGPS.is_calibrating():
        sleep(25) #Change me 
    while True:
        brain.screen.set_cursor(1, 1)
        brain.screen.print(DistanceLeft.object_distance(INCHES), precision=6 if vexcode_brain_precision is None else vexcode_brain_precision)
        brain.screen.set_cursor(1, 3)
        brain.screen.print("<--DistL  DistR-->")
        brain.screen.set_cursor(1, 16)
        brain.screen.print(DistanceRight.object_distance(INCHES), precision=6 if vexcode_brain_precision is None else vexcode_brain_precision)
        brain.screen.set_cursor(2, 1)
        brain.screen.print("BACKUP DIST:")
        brain.screen.set_cursor(2, 11)
        brain.screen.print(BackupDistance.distance(INCHES), precision=6 if vexcode_brain_precision is None else vexcode_brain_precision)
        # Distance sensor end
        # No Vision Sensor signature selected
        brain.screen.set_cursor(3, 1)
        brain.screen.print("VisionLeft:")
        brain.screen.set_cursor(3, 12)
        brain.screen.print((vexcode_visionleft_objects), precision=6 if vexcode_brain_precision is None else vexcode_brain_precision)
        brain.screen.set_cursor(3, 15)
        brain.screen.print("VisionRight:")
        brain.screen.set_cursor(3, 28)
        brain.screen.print((vexcode_visionright_objects), precision=6 if vexcode_brain_precision is None else vexcode_brain_precision)
        brain.screen.set_cursor(3, 31)
        brain.screen.print("RollerVision:")
        brain.screen.print((vexcode_rollervision_objects), precision=6 if vexcode_brain_precision is None else vexcode_brain_precision)
        brain.screen.set_cursor(3, 44)
        brain.screen.print((vexcode_rollervision_objects), precision=6 if vexcode_brain_precision is None else vexcode_brain_precision)
        # Vision sensor end
        # Vision sensor end
        # Rotation Sensor begin
        brain.screen.set_cursor(4, 1)
        brain.screen.print("Roller Degrees:")
        brain.screen.set_cursor(4, 16)
        brain.screen.print(Roller_Sensor.position(DEGREES), precision=6 if vexcode_brain_precision is None else vexcode_brain_precision)
        brain.screen.set_cursor(4, 18)
        brain.screen.print("Flywheel RPM:")
        brain.screen.set_cursor(4, 31)
        brain.screen.print(Flywheel_Sensor.velocity(RPM), precision=6 if vexcode_brain_precision is None else vexcode_brain_precision)
        # Rotation Sensor End
        # GPS Sensor begin
        brain.screen.set_cursor(5, 1)
        brain.screen.print("GPS_L X:")
        brain.screen.set_cursor(5, 10)
        brain.screen.print(LeftGPS.x_position(INCHES), precision=6 if vexcode_brain_precision is None else vexcode_brain_precision)
        brain.screen.set_cursor(5, 13)
        brain.screen.print("GPS_L Y")
        brain.screen.set_cursor(5, 21)
        brain.screen.print(LeftGPS.y_position(INCHES), precision=6 if vexcode_brain_precision is None else vexcode_brain_precision)
        brain.screen.set_cursor(5, 24)
        brain.screen.print("GPS_R X:")
        brain.screen.set_cursor(5, 33)
        brain.screen.print(RightGPS.x_position(INCHES), precision=6 if vexcode_brain_precision is None else vexcode_brain_precision)
        brain.screen.set_cursor(5, 36)
        brain.screen.print("GPS_R Y:")
        brain.screen.set_cursor(5, 45)
        brain.screen.print(RightGPS.y_position(INCHES), precision=6 if vexcode_brain_precision is None else vexcode_brain_precision)
        # GPS sensor end
        # Indexer begin
        brain.screen.set_cursor(6, 1)
        brain.screen.print("Indexer Left:")
        brain.screen.set_cursor(6, 15)
        brain.screen.print(SensorLeft.reflectivity(PERCENT), precision=6 if vexcode_brain_precision is None else vexcode_brain_precision)
        brain.screen.set_cursor(6, 18)
        brain.screen.print("Indexer Right:")
        brain.screen.set_cursor(6, 33)
        brain.screen.print(SensorRight.reflectivity(PERCENT), precision=6 if vexcode_brain_precision is None else vexcode_brain_precision)
        #brain.screen.set_cursor(7, 1)
        #brain.screen.print("Current RPM: ", current_rpm)
        #brain.screen.set_cursor(8, 1)
        #brain.screen.print("Error: ", error)
        #brain.screen.set_cursor(9, 1)
        #brain.screen.print("Output Voltage: ", output_voltage)
        #brain.screen.set_cursor(10, 1)
        #brain.screen.print("Integral: ", integral)
        wait(.16666666, SECONDS)
        brain.screen.clear_screen()

    while True:
        controller_1.screen.set_cursor(1, 1)
        controller_1.screen.print("Drive_L Temp:")
        controller_1.screen.set_cursor(1, 15)
        controller_1.screen.print(left_drive_smart.temperature(PERCENT))
        controller_1.screen.set_cursor(2, 1)
        controller_1.screen.print("Drive_R Temp:")
        controller_1.screen.set_cursor(2, 15)
        controller_1.screen.print(right_drive_smart.temperature(PERCENT))
        controller_1.screen.set_cursor(3, 4)
        controller_1.screen.print("DANGER AT 55%")
        wait(.1, SECONDS)

    while True:
        controller_2.screen.set_cursor(1, 1)
        controller_2.screen.print("Actual RPM:")
        controller_2.screen.set_cursor(1, 12)
        controller_1.screen.print(Flywheel_Sensor.velocity(RPM))  
        controller_2.screen.set_cursor(2, 5)
        controller_2.screen.print("TARGET RPM")
        controller_2.screen.set_cursor(3, 1)
        controller_2.screen.print("S:4200 M:3440 F:5654")
        wait(.1, SECONDS)

    while True:
        VisionLeft.take_snapshot(VisionLeft__VEX_BLUE)
        VisionRight.take_snapshot(VisionRight__VEXBLUE)
        VisionRoller.take_snapshot(VisionRoller__ROLLER_BLUE)
        wait(.2, SECONDS)

#def onevent_controller_1buttonL2_pressed_0():
 #   global myVariable, vexcode_brain_precision, vexcode_console_precision, vexcode_controller_1_precision, vexcode_visionleft_objects, vexcode_visionright_objects, vexcode_rollervision_objects


#def onevent_controller_1buttonL1_pressed_0():
 #   global myVariable, vexcode_brain_precision, vexcode_console_precision, vexcode_controller_1_precision, vexcode_visionleft_objects, vexcode_visionright_objects, vexcode_rollervision_objects


#def onevent_controller_1buttonRight_pressed_0():
  #  global myVariable, vexcode_brain_precision, vexcode_console_precision, vexcode_controller_1_precision, vexcode_visionleft_objects, vexcode_visionright_objects, vexcode_rollervision_objects



# 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 )

# Calibrate the Drivetrain
calibrate_drivetrain()

# system event handlers


controller_1.buttonUp.pressed(onevent_controller_1buttonUp_pressed_0)
#controller_1.buttonUp.released(onevent_controller_1buttonUp_released_0)
controller_1.buttonLeft.pressed(onevent_controller_1buttonLeft_pressed_0)
#controller_1.buttonLeft.released(onevent_controller_1buttonLeft_released_0)
#controller_1.buttonRight.pressed(onevent_controller_1buttonRight_pressed_0)
#controller_1.buttonRight.released(onevent_controller_1buttonRight_released_0)
#controller_1.buttonDown.pressed(onevent_controller_1buttonDown_pressed_0)
#controller_1.buttonDown.released(onevent_controller_1buttonDown_released_0)

#controller_1.buttonX.pressed(onevent_controller_1buttonX_pressed_0)
#controller_1.buttonX.released(onevent_controller_1buttonX_released_0)
#controller_1.buttonY.pressed(onevent_controller_1buttonY_pressed_0)
#controller_1.buttonY.released(onevent_controller_1buttonY_released_0)
#controller_1.buttonA.pressed(onevent_controller_1buttonA_pressed_0)
#controller_1.buttonA.released(onevent_controller_1buttonA_released_0)
#controller_1.buttonB.pressed(onevent_controller_1buttonB_pressed_0)
#controller_1.buttonB.pressed(onevent_controller_1buttonB_released_0)

controller_1.buttonL1.pressed(onevent_controller_1buttonL1_pressed_0)
#controller_1.buttonL1.released(onevent_controller_1buttonL1_released_0)
controller_1.buttonL2.pressed(onevent_controller_1buttonL2_pressed_0)
#controller_1.buttonL2.released(onevent_controller_1buttonL2_released_0)
controller_1.buttonR1.pressed(onevent_controller_1buttonR1_pressed_0)
#controller_1.buttonR1.released(onevent_controller_1buttonR1_released_0)
controller_1.buttonR2.pressed(onevent_controller_1buttonR2_pressed_0)
#controller_1.buttonR2.released(onevent_controller_1buttonR2_released_0)




#controller_2.buttonUp.pressed(onevent_controller_2buttonUp_pressed_0)
#controller_2.buttonUp.released(onevent_controller_2buttonUp_released_0)
#controller_2.buttonLeft.pressed(onevent_controller_2buttonLeft_pressed_0)
#controller_2.buttonLeft.released(onevent_controller_2buttonLeft_released_0)
#controller_2.buttonRight.pressed(onevent_controller_2buttonRight_pressed_0)
#controller_2.buttonRight.released(onevent_controller_2buttonRight_released_0)
#controller_2.buttonDown.pressed(onevent_controller_2buttonDown_pressed_0)
#controller_2.buttonDown.released(onevent_controller_2buttonDown_released_0)

controller_2.buttonX.pressed(onevent_controller_2buttonX_pressed_0)
#controller_2.buttonX.released(onevent_controller_2buttonX_released_0)
controller_2.buttonY.pressed(onevent_controller_2buttonY_pressed_0)
#controller_2.buttonY.released(onevent_controller_2buttonY_released_0)
controller_2.buttonA.pressed(onevent_controller_2buttonA_pressed_0)
#controller_2.buttonA.released(onevent_controller_2buttonA_released_0)
controller_2.buttonB.pressed(onevent_controller_2buttonB_pressed_0)
#controller_2.buttonB.pressed(onevent_controller_2buttonB_released_0)


# add 15ms delay to make sure events are registered correctly.
wait(.015, SECONDS)
pid_thread = Thread(pid_loop)
ws2 = Thread( when_started2 )
when_started1()
pid_loop()

# register the competition functions
competition = Competition( vexcode_driver_function, vexcode_auton_function )

# Calibrate the Drivetrain
calibrate_drivetrain()

# system event handlers


controller_1.buttonUp.pressed(onevent_controller_1buttonUp_pressed_0)
#controller_1.buttonUp.released(onevent_controller_1buttonUp_released_0)
controller_1.buttonLeft.pressed(onevent_controller_1buttonLeft_pressed_0)
#controller_1.buttonLeft.released(onevent_controller_1buttonLeft_released_0)
#controller_1.buttonRight.pressed(onevent_controller_1buttonRight_pressed_0)
#controller_1.buttonRight.released(onevent_controller_1buttonRight_released_0)
#controller_1.buttonDown.pressed(onevent_controller_1buttonDown_pressed_0)
#controller_1.buttonDown.released(onevent_controller_1buttonDown_released_0)

#controller_1.buttonX.pressed(onevent_controller_1buttonX_pressed_0)
#controller_1.buttonX.released(onevent_controller_1buttonX_released_0)
#controller_1.buttonY.pressed(onevent_controller_1buttonY_pressed_0)
#controller_1.buttonY.released(onevent_controller_1buttonY_released_0)
#controller_1.buttonA.pressed(onevent_controller_1buttonA_pressed_0)
#controller_1.buttonA.released(onevent_controller_1buttonA_released_0)
#controller_1.buttonB.pressed(onevent_controller_1buttonB_pressed_0)
#controller_1.buttonB.pressed(onevent_controller_1buttonB_released_0)

controller_1.buttonL1.pressed(onevent_controller_1buttonL1_pressed_0)
#controller_1.buttonL1.released(onevent_controller_1buttonL1_released_0)
controller_1.buttonL2.pressed(onevent_controller_1buttonL2_pressed_0)
#controller_1.buttonL2.released(onevent_controller_1buttonL2_released_0)
controller_1.buttonR1.pressed(onevent_controller_1buttonR1_pressed_0)
#controller_1.buttonR1.released(onevent_controller_1buttonR1_released_0)
controller_1.buttonR2.pressed(onevent_controller_1buttonR2_pressed_0)
#controller_1.buttonR2.released(onevent_controller_1buttonR2_released_0)




#controller_2.buttonUp.pressed(onevent_controller_2buttonUp_pressed_0)
#controller_2.buttonUp.released(onevent_controller_2buttonUp_released_0)
#controller_2.buttonLeft.pressed(onevent_controller_2buttonLeft_pressed_0)
#controller_2.buttonLeft.released(onevent_controller_2buttonLeft_released_0)
#controller_2.buttonRight.pressed(onevent_controller_2buttonRight_pressed_0)
#controller_2.buttonRight.released(onevent_controller_2buttonRight_released_0)
#controller_2.buttonDown.pressed(onevent_controller_2buttonDown_pressed_0)
#controller_2.buttonDown.released(onevent_controller_2buttonDown_released_0)

controller_2.buttonX.pressed(onevent_controller_2buttonX_pressed_0)
#controller_2.buttonX.released(onevent_controller_2buttonX_released_0)
controller_2.buttonY.pressed(onevent_controller_2buttonY_pressed_0)
#controller_2.buttonY.released(onevent_controller_2buttonY_released_0)
controller_2.buttonA.pressed(onevent_controller_2buttonA_pressed_0)
#controller_2.buttonA.released(onevent_controller_2buttonA_released_0)
controller_2.buttonB.pressed(onevent_controller_2buttonB_pressed_0)
#controller_2.buttonB.pressed(onevent_controller_2buttonB_released_0)


# add 15ms delay to make sure events are registered correctly.
wait(.015, SECONDS)
pid_thread = Thread(pid_loop)
ws2 = Thread( when_started2 )
when_started1()
pid_loop()

Here is the full code again. Sorry Mods…
Also, this is in Vexcode V5 Python