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.

5 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

Hey, I’ve got something that works! I’ve still got a few issues though. It has oscillation issues once it reaches near the speed, (nothing major, but I’m afraid it will overheat the motors). The second issue is that it’ll get to a specific rpm, but it will never hit the target. The third issue is that at different target RPMs, it requires different factors to oscillate less (Ex: tbh = 0.03 * (tbh + error)). Changing the factor helps a little with the first and second issues but doesn’t fix it. Posted below is the loop snippet, along with the whole program with the loop in it.

def pid_loop():
    global tbh, error, output_voltage, current_rpm, target_rpm
    # Run the TBH 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
        
        # Update the TBH value
        tbh = 0.03 * (tbh + error)
        
        # Calculate the output voltage based on the TBH value
        output_voltage = tbh
        
        
        # Set the flywheel motor speeds based on the output voltage
        Flywheel_Motors.spin(FORWARD, output_voltage, VOLT)

Whole code:

#region VEXcode Generated Robot Configuration
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)
IndexerLeft = Line(brain.three_wire_port.e)
IndexerRight = 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
max_voltage = 12
min_voltage = 0
integral = .0000000001
Ki = .0000000001
integral_limit = .0000000001 
max_integral= .0000000001 
target_rpm = 2700
tbh = 0

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(45, PERCENT)  
    drivetrain.set_drive_velocity(100, PERCENT)
    drivetrain.set_turn_velocity(100, PERCENT)
    #Velocity Set ^

    drivetrain.drive_for(FORWARD, 5, INCHES)
    drivetrain.turn_for(LEFT, 90, DEGREES)
    Flywheel_Motors.spin(FORWARD, 7.25, VOLT)
    wait(4,SECONDS)
    IntakeMotor.spin(FORWARD)
    ExpansionMotor.spin(FORWARD)
    wait(6,SECONDS)
    ExpansionMotor.stop()
    IntakeMotor.stop()                      

def when_started1():
    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)
    drivetrain.set_turn_velocity(10, PERCENT)
    drivetrain.set_drive_velocity(10, 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)
    drivetrain.set_turn_velocity(10, PERCENT)
    drivetrain.set_drive_velocity(10, PERCENT)
    Flywheel_Motors.set_stopping(COAST)

def pid_loop():
    global tbh, error, output_voltage, current_rpm, target_rpm
    # Run the TBH 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
        
        # Update the TBH value
        tbh = 0.03 * (tbh + error)
        
        # Calculate the output voltage based on the TBH value
        output_voltage = tbh
        
        
        # Set the flywheel motor speeds based on the output voltage
        Flywheel_Motors.spin(FORWARD, output_voltage, VOLT)



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
    target_rpm = 2600



#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, vexcode_controller_2_precision
   # left_drive_smart.set_velocity(100, PERCENT)
   # right_drive_smart.set_velocity(100, PERCENT)



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.stop()

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(IndexerLeft.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(IndexerRight.reflectivity(PERCENT), precision=6 if vexcode_brain_precision is None else vexcode_brain_precision)
        brain.screen.set_cursor(7, 1)
        brain.screen.print(error)
        brain.screen.set_cursor(8, 1)
        brain.screen.print(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()

Any Ideas? @Foster

Now seriously I wouldn’t care about anything this forums says, but it irks me when some is so blatantly wrong yet think they are right.

Look, there’s something called common sense. Why would you assume I’m talking about 1 line of code. I mean seriously. I said code, not line. If u need help with PID, send everything that pertains to that. Sending your driver control or the configured devices does not help in the slightest.

I seriously don’t understand what the problem with this, this is a forum, not a college essay. Don’t get why u are not picking at this.

Sorry my first comment got mixed messages, but it’s best to ask and not assume, or even better, just ignore it.

In my experience, using voltage for anything less than 100% velocity on a flywheel motor is the way to go. See Voltage vs Velocity control to see why I think this. If your code works good, however, no need to change it, but keep this in mind as a backup if you ever need it.

To use voltage instead, you could try making a function with the velocity input and have the function multiply 12 by the velocity divided by 10 and spin the motor at the outputted integer. Here’s that in V5 Blocks:

I’ve actually tried voltage control, and technically that’s what I’m using for the TBH loop (It targets a specific velocity, but I have full voltage control). The problem with voltage control is that it is very slow getting up to that velocity, I can never hit a very specific rpm, which makes programming more difficult, and it recovers slowly after a disk fires. That’s why I’m trying to get the TBH loop to work. I’ve actually got really got the loop working and it has a more stable rpm than pure voltage control, it just never hits the target (E.G. Target is 2400 and it will settle at 2100). That along with the slight issues with the motor flipping back and forth really fast which potentially heats it up faster.

Question, I’m trying to figure out how to stop the oscillation. My idea so far is the figure out what voltage the flywheel goes around when it’s oscillating and set a sort of “deadzone” so that it stays at a constant voltage. Would something like this work / be feasible? Also, I still have no idea how to fix the thing with target being off. Does anyone see anything off with my math in the loop?

It looks like your TBH isn’t implemented correctly. There isn’t any check for when error crosses 0, and you update the tbh value every time the while loop is ran. I’d go back up to 46x’s post at the top of this thread to double check your logic.

3 Likes

Alright, I’ve rewritten the code. here is the loop:

def tbh_loop():
    global tbh, error, output_voltage, current_rpm, target_rpm, gain, prev_error, gain, output
    # Run the TBH 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

        output += gain * error

        if (error > 0 and prev_error < 0) or (error < 0 and prev_error > 0):
            output = 0.5 * (output + tbh)
            tbh = output
            prev_error = error

        # Set the flywheel motor speeds based on the output voltage
        Flywheel_Motors.spin(FORWARD, output, VOLT)

And here is the whole code:

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
max_voltage = 12
min_voltage = 0
tbh = 0
error= 0
target_rpm = 0
tbh = .5
output_voltage = 0
prev_error=0
gain = .00025
output = 0
#def encoder_X():


#def encoder_Y():



#def Interial():


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(50, PERCENT)  
    drivetrain.set_drive_velocity(75, PERCENT)
    drivetrain.set_turn_velocity(50, PERCENT)
    #Velocity Set ^
    Flywheel_Motors.spin(FORWARD, 11.4, VOLT)
    drivetrain.drive_for(FORWARD, 20, INCHES)
    drivetrain.turn_for(RIGHT, 90, DEGREES)
    drivetrain.drive_for(FORWARD, 90, INCHES)
    drivetrain.turn_for(LEFT, 90, DEGREES)
    left_drive_smart.spin(REVERSE, 3.5, VOLT)
    right_drive_smart.spin(REVERSE, 3.5, VOLT)
    wait(1.5,SECONDS)
    left_drive_smart.stop()
    right_drive_smart.stop()
    ExpansionMotor.spin(FORWARD)
    wait(.25,SECONDS)
    ExpansionMotor.stop()
    IntakeMotor.stop()
    drivetrain.drive_for(FORWARD, 10, INCHES) 
    IntakeMotor.spin_for(FORWARD, 5000, DEGREES, wait=False)
    ExpansionMotor.spin_for(FORWARD, 5000, DEGREES, wait=False)
    wait(2, SECONDS)
    IntakeMotor.spin_for(FORWARD, 5000, DEGREES, wait=False)
    ExpansionMotor.spin_for(FORWARD, 5000, DEGREES, wait=False)


def when_started1():
    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)
    drivetrain.set_turn_velocity(10, PERCENT)
    drivetrain.set_drive_velocity(10, 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)
    drivetrain.set_turn_velocity(10, PERCENT)
    drivetrain.set_drive_velocity(10, PERCENT)
    Flywheel_Motors.set_stopping(COAST)

def tbh_loop():
    global tbh, error, output_voltage, current_rpm, target_rpm, gain, prev_error, gain, output
    # Run the TBH 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

        output += gain * error

        if (error > 0 and prev_error < 0) or (error < 0 and prev_error > 0):
            output = 0.5 * (output + tbh)
            tbh = output
            prev_error = error

        # Set the flywheel motor speeds based on the output voltage
        Flywheel_Motors.spin(FORWARD, output, VOLT)


#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, vexcode_controller_2_precision
   # left_drive_smart.set_velocity(100, PERCENT)
   # right_drive_smart.set_velocity(100, PERCENT)



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, tbh, error, output_voltage, current_rpm, target_rpm 
    target_rpm = 2600

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, tbh, error, output_voltage, current_rpm, target_rpm 
    Flywheel_Motors.stop()

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, tbh, error, output_voltage, current_rpm, target_rpm 
    target_rpm = 2300

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, tbh, error, output_voltage, current_rpm, target_rpm, RealTBH

def when_started2():
    global myVariable, vexcode_brain_precision, vexcode_console_precision, vexcode_controller_1_precision, vexcode_visionleft_objects, vexcode_visionright_objects, vexcode_rollervision_objects, output_voltage
    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(IndexerLeft.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(IndexerRight.reflectivity(PERCENT), precision=6 if vexcode_brain_precision is None else vexcode_brain_precision)
        brain.screen.set_cursor(7, 1)
        brain.screen.print(error)
        brain.screen.set_cursor(8, 1)
        brain.screen.print(tbh)
        brain.screen.set_cursor(9, 1)
        brain.screen.print(output_voltage)

        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(tbh_loop)
ws2 = Thread( when_started2 )
when_started1()
tbh_loop()

It speeds up fast, but then it goes way off target, and then slows down rapidly. It repeats the cycle. I’ve tried altering the gain, but it doesn’t seem to affect anything. Any suggestions on how to fix this?

Finally! Finally! Finally! Sucess! I’ve got the code working. I cleaned up my program a bit and adjusted the gain. Now the only issue I have is that the flywheel spins up a bit slow. If I alter the gain to be larger, it has more issues with overshooting, which is annoying as the main point of it is to get to the desired speed as fast as possible. If I modify the gain to be smaller, it just goes up to that speed slower. Is there anything that I can change that can give an initial burst of speed, and some more speed after I fire a disc without that overshoot?

2 Likes

Hi there. My knowledge of python is pretty limited. I made some code for our bot and implemented your tbh loop but the program keeps crashing. Any insight as to what I have done wrong?

# VEX V5 Python Project with Competition Template
import sys
import vex
from vex import *
import motor_group
import drivetrain
import smartdrive

#region config
brain                = vex.Brain()
left1                = vex.Motor(vex.Ports.PORT1, vex.GearSetting.RATIO18_1, False)
left2                = vex.Motor(vex.Ports.PORT2, vex.GearSetting.RATIO18_1, False)
left_drive_smart     = motor_group(left1, left2)
right1               = vex.Motor(vex.Ports.PORT3, vex.GearSetting.RATIO18_1, False)
right2               = vex.Motor(vex.Ports.PORT4, vex.GearSetting.RATIO18_1, False)
right_drive_smart    = motor_group(right1, right2)
drivetrain_inertial  = Inertial_21(Ports.PORT21)
drivetrain           = SmartDrive(left_drive_smart, right_drive_smart, drivetrain_inertial, 299.24, 320, 40, MM, 1.6666666666666667)
flywheel1            = vex.Motor(vex.Ports.PORT5, vex.GearSetting.RATIO18_1, False)
flywheel2            = vex.Motor(vex.Ports.PORT6, vex.GearSetting.RATIO18_1, False)
Flywheel_group       = motor_group(Flywheel1,Flywheel2)
intake               = vex.Motor(vex.Ports.PORT8, vex.GearSetting.RATIO18_1, False)
indexer              = vex.Motor(vex.Ports.PORT9, vex.GearSetting.RATIO18_1, False)
inertial             = vex.Inertial(vex.Ports.PORT21)
controller_1         = vex.Controller(vex.ControllerType.PRIMARY)
#endregion config


int( max_voltage = 12)
int(min_voltage = 0)
int(tbh = 0)
int(error= 0)
int(target_rpm = 0)
int(tbh = .5)
int(output_voltage = 0)
int(prev_error=0)
int(gain = .00025)
int(output = 0)

# Creates a competition object that allows access to Competition methods.
competition = vex.Competition()



def pre_auton():
    # All activities that occur before competition start
    # Example: setting initial positions
    pass

def autonomous():
    # Place autonomous code here
    pass

def drivercontrol():
    
     def tbh_loop():
        global tbh, error, output_voltage, current_rpm, target_rpm, gain, prev_error, gain, output
        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

        output += gain * error

        if (error > 0 and prev_error < 0) or (error < 0 and prev_error > 0):
            output = 0.5 * (output + tbh)
            tbh = output
            prev_error = error
         # Set the flywheel motor speeds based on the output voltage    
        Flywheel_group.spin(FORWARD, output, VOLT)    
        pass
###################################################################################
###################################################################################
     while true:   
        left_drive_smart.spin(vex.DirectionType.FWD, (controller.axis2.position(vex.PercentUnits.PCT)), vex.VelocityUnits.PCT)
        right_drive_smart.spin(vex.DirectionType.REV, (controller.axis3.position(vex.PercentUnits.PCT)), vex.VelocityUnits.PCT)
        
        Flywheel_group.spin(FORWARD, 8.7, VOLT)
       
        if controller.buttonL2.pressing():
            indexer.spin(DirectionType.FWD, 100, vex.VelocityUnits.PCT)
            
        else:
            indexer.spin(vex.DirectionType.FWD, 0, vex.VelocityUnits.PCT)
        
        if controller.buttonR2.pressing():
            intake.spin(vex.DirectionType.FWD, -100, vex.VelocityUnits.PCT)
            intake.set_max_torque_percent(100, PercentUnits.PCT)
        elif controller.buttonR1.pressing():
            intake.spin(vex.DirectionType.FWD, 100, vex.VelocityUnits.PCT)
            intake.set_max_torque_percent(100, PercentUnits.PCT)
        else:
            intake.spin(vex.DirectionType.FWD, 0, vex.VelocityUnits.PCT)
        
            
        if controller.buttonRight.pressing():
            bob.set(True)
        elif controller.buttonLeft.pressing():
            bob.set(False)   
        else:
            bob.set(False)
            
        if controller.buttonY.pressing():
            bob1.set(True)
        elif controller.buttonA.pressing():
            bob1.set(False)   
        else:
            bob1.set(False)
     pass

# Do not adjust the lines below

# Set up (but don't start) callbacks for autonomous and driver control periods.
competition.autonomous(autonomous)
competition.drivercontrol(drivercontrol)

# Run the pre-autonomous function.
pre_auton()

# Robot Mesh Studio runtime continues to run until all threads and
# competition callbacks are finished.

I am using Robotmesh Studio to do my coding.
Thanks,
George | 2114B

1 Like

Hey George,
I looked over your code and I think that I have a few ideas of what might be wrong. First, you are declaring all of the variables as ints, where the ones that have decimals need to be a float. I don’t do this is Vexcode, but you might need to in RobotMesh Studio. Next, you might want to isolate the TBH loop to a thread. Start small, then when you get it working, try to integrate it into your main code. Hope this helps!

1 Like