Double PID wont move

VEX V5 Technical Support UNOFFICIAL Tech Support
, , ,
1

So recently I made a couple of PIDs and they worked great but I wanted a little more adaptability for movement and better accuracy. I made what I think is a double pid where I just put a driving pid and a turning pid together.

The issue currently with the PID is that the thing won’t even move even though when I had these PIDs separate they worked flawlessly.

Here is the code

    global error, integral, derivative, previous_error, error2, integral2, derivative2, previous_error2

    # Set sensor rotation to 0 degrees
    LeftDrive1111.set_position(0, DEGREES)
    inertial_10.set_rotation(0, DEGREES)

    # Reset PID variables
    error = 0
    integral = 0
    derivative = 0
    previous_error = 0
    consecutive_stable_iterations = 0
    error2 = 0
    integral2 = 0
    derivative2 = 0
    previous_error2 = 0
    TimeStop = 50

    # Main PID loop
    while True:
        current_distance = ((3.14159265 * Wheel_Diameter) / 360) * (LeftDrive1111.position(DEGREES)) * (Drive_Gear_Ratio) 
        error = (target_distance * 1) - current_distance
        integral = integral + error
        if  error < 0 :
            integral = 0
        derivative = error - previous_error
        previous_error = error
        output = kp * error + ki * integral + kd * derivative
        speed = abs(output)

        current_angle = inertial_10.rotation(DEGREES)
        error2 = target_angle - current_angle
        integral2 = integral2 + error2
        if  error2 < 0 :
            integral2 = 0
        derivative2 = error2 - previous_error2
        previous_error2 = error2
        output2 = kp2 * error2 + ki2 * integral2 + kd2 * derivative2
        speed2 = abs(output2)

        error3 = error + error2


        if error > 1:
            left_speed3 = (1 * speed)
            right_speed3 = (1 * speed)
        if error < -1:
            left_speed3 = (-1 * speed)
            right_speed3 = (-1 * speed)

        if error2 > 1:
            left_speed4 = 1.25 * speed2 * 2 + 10
            right_speed4 = -1.1 * speed2 * 2 + 10
        if error2 < -1:
            left_speed4 = -1.35 * speed2 * 2
            right_speed4 = 1.1 * speed2 * 2

        left_speed5 = left_speed4 + left_speed3
        right_speed5 = right_speed4 + right_speed3

        if abs(error3) < 1.5:
            consecutive_stable_iterations += 1
        else:
            consecutive_stable_iterations = 0\

        # Check for stability
        if consecutive_stable_iterations >=Time:
            LeftDrive1111.stop()
            LeftDrive1155.stop()
            RightDrive1111.stop()
            RightDrive1155.stop()          
            break

        if TimeStop == 1:
            wait(3, SECONDS)
            break

        # Set motor velocities
        LeftDrive1111.spin(FORWARD, left_speed5, VOLT)
        LeftDrive1155.spin(FORWARD, left_speed5, VOLT)
        RightDrive1111.spin(FORWARD, right_speed5, VOLT)
        RightDrive1155.spin(FORWARD, right_speed5, VOLT) 


        # Sleep for a short time to avoid excessive loop execution
        wait(20, MSEC)

The idea was that if I wanted to drive straight forward I could set the turn to zero and the drive distance to whatever and the second pid will keep the robot straight and vice versa for turning.

I also wanted to see if I could have a target distance and turn angle so that I could drive in arcs or curves accurately without pure pursuit/odometry.

Also, I want to mention that this is literally two pids put together with the outputs added together.

1 Like
2

This is very similar to an algorithm I’ve coded before. In my set up, I would feed in an array of coordinates and it would drive through each one. It is like a very simple version of pure pursuit and if tuned right it can make some pretty nice curves.

In my algorithm, I didn’t activate the distance based PD until the last movement so that it didn’t slow down between points. I also don’t use the I term in my drive functions but that’s personal preference.

I have the turn PID spit out a value based on the derivative and proportional error of direction. I subtract this value from 1, and use it as a multiplier on the input voltage of the appropriate drive train side, while the opposite side I give a full 12 volts.

Your code looks very similar to what I have, but I use C++. Just by scanning over it I didn’t see where your problem is, but know that the idea for the algorithm you have is legit

4 Likes
3

Your individual turn and drive PIDs seem fine (assuming your PID is tuned right), the only thing I noticed about them is integral gets set to zero if error is negative, but you probably want it to be set to zero only if error is close to zero, because otherwise the integral won’t work if error is negative from the robot turning counterclockwise or driving backwards.

I did notice that your “time stop” exit condition has a long wait in it which stops the PID from being able to run, but as long as the time stop variable isn’t 1 it shouldn’t break anything.

I think the problem is with how you combine your turn and drive PID outputs. If you know how arcade control works, you can combine them the exact same way, replacing the drive input with the drive pid and the same for turning. You also don’t need to change the sign of output with the absolute value function and the if statements after the PID, the outputs of the PID already have the correct signs.

One last thing, my team found that when combining our turn and drive PID the PID’s output values can often add up to more then the max speed of the motors, meaning that the motors are being told to go a lot faster then they can. We fixed this by putting a max speed on the outputs before we combined them, then scaled the left and right speeds so that the ratio of turn speed to drive speed was the same, but it fit in the max speed of the motor. If you want I can give you some pseudo code of it as an example

4 Likes
4

I don’t understand how my combination of the outputs would make it not work. I use arcade control and I understand where you’re getting at but wouldn’t it not matter because the PID outputs would already be negative when they need to be?

What would be the issue with telling the motor to go faster than it already can? Unless the limiter uses percentages or something wouldn’t it not matter? I would like to see some pseudo-code of that if you are still willing to though.

Also, I don’t think the time stop does anything because time stop is equal to 50 and my consecutive stable iterations code should be the thing breaking the loop.

Thank you for your help

5

Yeah ignore the bit about arcade control, I just got confused because your variable names are all similar. My point about the signs thing still stands though. Right now you have these lines:


if error > 1:
    left_speed3 = (1 * speed)
    right_speed3 = (1 * speed)
if error < -1:
    left_speed3 = (-1 * speed)
    right_speed3 = (-1 * speed)

if error2 > 1:
    left_speed4 = 1.25 * speed2 * 2 + 10
    right_speed4 = -1.1 * speed2 * 2 + 10
if error2 < -1:
    left_speed4 = -1.35 * speed2 * 2
    right_speed4 = 1.1 * speed2 * 2

Which shouldn’t be necessary if you just don’t use the absolute value function on the output variables higher up. You then subtract instead of add the right turn speed on these two lines and it should work:

left_speed5 = left_speed4 + left_speed3
right_speed5 = right_speed4 + right_speed3

Right now there is nothing telling the PIDs that they can’t power the motors more than 12 volts. This means if the drive PID outputs a big number like 20 and the turn PID outputs a relatively small number like 4, the speeds given to the motors on both sides will be > 12, meaning both sides go max speed, and the robot won’t turn. Here’s the pseudocode for how my team fixed this:

var drive_pid_output = drive_proportional * drive_kp + // etc...
var turn_pid_output = turn_proportional * turn_kp + // etc...

// Stop the PIDs from outputing values > 12 or < -12
drive_pid_output = clamp(drive_pid_output , -12, 12)
turn_pid_output = clamp(turn_pid_output , -12, 12)

// Get what percentage of the combined speed the individual drive and turn speeds are
var total_speed = abs(drive_pid_output) + abs(turn_pid_output)
var drive_percent = drive_pid_output / total_speed
var turn_percent = turn_pid_output / total_speed

// Scale down the drive and turn speed so they are the same relative to each other, but "fit" in the range of the max speed
var scaled_drive = min(12, total_speed) * drive_percent
var scaled_turn = min(12, total_speed) * turn_percent

// Get left and right speeds from drive and turn speeds
var left_speed = scaled_drive + scaled_turn
var right_speed = scaled_drive - scaled_turn

However, I don’t think any of this should be stopping it from working at all, they should just be making it less accurate. The problem is most likely caused by another part of your code

1 Like
6

When a system is misbehaving, print stuff to the screen whose values you should know, or you know should change over time, and see what they’re actually doing. You might redesign what you’re printing several times until you find what is staying still that should be changing. Given that the robot isn’t moving, printing to the screen should be fine.

2 Likes
7

Thank you i will test this when I can get it moving. I looked through my code again though and didn’t see anything that would stop it from moving. Its also especially confusing because I still have my other two pids in the code and they work fine. would you be willing to look at my code and see if you can help?

here is all of the code

#region VEXcode Generated Robot Configuration
from vex import *
import urandom

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

# Robot configuration code
LeftDrive1111_motor_a = Motor(Ports.PORT20, GearSetting.RATIO_6_1, True)
LeftDrive1111_motor_b = Motor(Ports.PORT6, GearSetting.RATIO_6_1, True)
LeftDrive1111 = MotorGroup(LeftDrive1111_motor_a, LeftDrive1111_motor_b)
LeftDrive1155_motor_a = Motor(Ports.PORT3, GearSetting.RATIO_6_1, True)
LeftDrive1155_motor_b = Motor(Ports.PORT9, GearSetting.RATIO_6_1, False)
LeftDrive1155 = MotorGroup(LeftDrive1155_motor_a, LeftDrive1155_motor_b)
RightDrive1111_motor_a = Motor(Ports.PORT5, GearSetting.RATIO_6_1, False)
RightDrive1111_motor_b = Motor(Ports.PORT11, GearSetting.RATIO_6_1, False)
RightDrive1111 = MotorGroup(RightDrive1111_motor_a, RightDrive1111_motor_b)
RightDrive1155_motor_a = Motor(Ports.PORT7, GearSetting.RATIO_6_1, False)
RightDrive1155_motor_b = Motor(Ports.PORT2, GearSetting.RATIO_6_1, True)
RightDrive1155 = MotorGroup(RightDrive1155_motor_a, RightDrive1155_motor_b)
CataIntake1 = Motor(Ports.PORT13, GearSetting.RATIO_18_1, False)
inertial_10 = Inertial(Ports.PORT10)
controller_1 = Controller(PRIMARY)
IntakeACT = DigitalOut(brain.three_wire_port.b)
Wings = DigitalOut(brain.three_wire_port.c)
CataIntake_motor_a = Motor(Ports.PORT1, GearSetting.RATIO_18_1, False)
CataIntake_motor_b = Motor(Ports.PORT4, GearSetting.RATIO_18_1, True)
CataIntake = MotorGroup(CataIntake_motor_a, CataIntake_motor_b)


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


def play_vexcode_sound(sound_name):
    # Helper to make playing sounds from the V5 in VEXcode easier and
    # keeps the code cleaner by making it clear what is happening.
    print("VEXPlaySound:" + sound_name)
    wait(5, MSEC)

# add a small delay to make sure we don't print in the middle of the REPL header
wait(200, MSEC)
# clear the console to make sure we don't have the REPL in the console
print("\033[2J")

#endregion VEXcode Generated Robot Configuration

vexcode_brain_precision = 0
vexcode_console_precision = 0
vexcode_controller_1_precision = 0
message1 = Event()
CataShoot = Event()
intakeACT = Event()
WingsOpen = Event()
WingsClose = Event()
BlockerUp = Event()
Intake2 = Event()
Test = [[0 for y in range(10)] for x in range(10)]
DriverControl = False
Autonomous = False
Started = False
IntakeAUCT = False
wings = False
Intake = False
Cata = False
IntakeR = False
Auton = False
ButtonUp = False
Button_Up = False
myVariable = 0
RightDrive = 0
LeftDrive = 0
ErrorDS = 0
OutputDS = 0
Auton1 = 0
CataStop = 0
armupdown = 0
LeftRot = 0
RightRot = 0
TurnOSC = 0
Heading = 0
consecutive_stable_iterations = 0
inertial_10.set_heading(0, DEGREES)

Wheel_Diameter = 3.25
Drive_Gear_Ratio = (36/48)

max_speed = 100
min_speed = 50

error = 0
integral = 0
derivative = 0
previous_error = 0

kp = .564
ki = .000013
kd = 0

kp2 = .762
ki2 = .0006
kd2 = 4.4

def drive_distance(target_distance, Time, TimeStop):
    global error, integral, derivative, previous_error

    # Set initial rotation to 0 degrees
    LeftDrive1111.set_position(0, DEGREES)

    # Reset PID variables
    error = 0
    integral = 0
    derivative = 0
    previous_error = 0
    consecutive_stable_iterations = 0

    # Main PID loop for turning
    while True:
        current_distance = ((3.14159265 * Wheel_Diameter) / 360) * (LeftDrive1111.position(DEGREES)) * (Drive_Gear_Ratio) 
        error = (target_distance * 1) - current_distance
        integral = integral + error
        if  error < 0 :
            intergral = 0
        derivative = error - previous_error
        previous_error = error
        output = kp * error + ki * integral + kd * derivative
        speed = abs(output)

        if error > 1:
            left_speed = 1 * speed
            right_speed = 1 * speed
        if error < -1:
            left_speed = -1 * speed
            right_speed = -1 * speed

        if abs(error) < 1.5:
            consecutive_stable_iterations += 1
        else:
            consecutive_stable_iterations = 0\

        # Check for stability
        if consecutive_stable_iterations >=Time:
            LeftDrive1111.stop()
            LeftDrive1155.stop()
            RightDrive1111.stop()
            RightDrive1155.stop() 
            break

        if TimeStop == 1:
            wait(3, SECONDS)
            break

        # Set motor velocities
        LeftDrive1111.spin(FORWARD, left_speed, VOLT)
        LeftDrive1155.spin(FORWARD, left_speed, VOLT)
        RightDrive1111.spin(FORWARD, right_speed, VOLT)
        RightDrive1155.spin(FORWARD, right_speed, VOLT) 


        # Sleep for a short time to avoid excessive loop execution
        wait(20, MSEC)

def drive_turn(target_distance, target_angle, Time):
    global error, integral, derivative, previous_error, error2, integral2, derivative2, previous_error2

    # Set sensor rotation to 0 degrees
    LeftDrive1111.set_position(0, DEGREES)
    inertial_10.set_rotation(0, DEGREES)

    # Reset PID variables
    error = 0
    integral = 0
    derivative = 0
    previous_error = 0
    consecutive_stable_iterations = 0
    error2 = 0
    integral2 = 0
    derivative2 = 0
    previous_error2 = 0
    TimeStop = 50

    # Main PID loop
    while True:
        current_distance = ((3.14159265 * Wheel_Diameter) / 360) * (LeftDrive1111.position(DEGREES)) * (Drive_Gear_Ratio) 
        error = (target_distance * 1) - current_distance
        integral = integral + error
        if  error < 0 :
            integral = 0
        derivative = error - previous_error
        previous_error = error
        output = kp * error + ki * integral + kd * derivative
        speed = abs(output)

        current_angle = inertial_10.rotation(DEGREES)
        error2 = target_angle - current_angle
        integral2 = integral2 + error2
        if  error2 < 0 :
            integral2 = 0
        derivative2 = error2 - previous_error2
        previous_error2 = error2
        output2 = kp2 * error2 + ki2 * integral2 + kd2 * derivative2
        speed2 = abs(output2)

        error3 = error + error2


        if error > 1:
            left_speed3 = (1 * speed)
            right_speed3 = (1 * speed)
        if error < -1:
            left_speed3 = (-1 * speed)
            right_speed3 = (-1 * speed)

        if error2 > 1:
            left_speed4 = 1.25 * speed2 * 2 + 10
            right_speed4 = -1.1 * speed2 * 2 + 10
        if error2 < -1:
            left_speed4 = -1.35 * speed2 * 2
            right_speed4 = 1.1 * speed2 * 2

        left_speed5 = left_speed4 + left_speed3
        right_speed5 = right_speed4 + right_speed3

        if abs(error3) < 1.5:
            consecutive_stable_iterations += 1
        else:
            consecutive_stable_iterations = 0\

        # Check for stability
        if consecutive_stable_iterations >=Time:
            LeftDrive1111.stop()
            LeftDrive1155.stop()
            RightDrive1111.stop()
            RightDrive1155.stop()          
            break

        if TimeStop == 1:
            wait(3, SECONDS)
            break

        # Set motor velocities
        LeftDrive1111.spin(FORWARD, left_speed5, VOLT)
        LeftDrive1155.spin(FORWARD, left_speed5, VOLT)
        RightDrive1111.spin(FORWARD, right_speed5, VOLT)
        RightDrive1155.spin(FORWARD, right_speed5, VOLT) 


        # Sleep for a short time to avoid excessive loop execution
        wait(20, MSEC)

def turn_to_angle(target_angle, Time, TimeStop):
    global error, integral, derivative, previous_error

    inertial_10.set_rotation(0, DEGREES)

    # Reset PID variables
    error = 0
    integral = 0
    derivative = 0
    previous_error = 0
    consecutive_stable_iterations = 0

    # Main PID loop for turning
    while True:
        current_angle = inertial_10.rotation(DEGREES)
        error = target_angle - current_angle
        integral = integral + error
        if  error < 0 :
            intergral = 0
        derivative = error - previous_error
        previous_error = error
        output = kp2 * error + ki2 * integral + kd2 * derivative
        speed = abs(output)

        if error > 1:
            left_speed2 = 1.25 * speed * 2
            right_speed2 = -1.1 * speed * 2
        if error < -1:
            left_speed2 = -1.35 * speed * 2
            right_speed2 = 1.1 * speed * 2

        if abs(error) < 1.5:
            consecutive_stable_iterations += 1
        else:
            consecutive_stable_iterations = 0\

        # Check for stability
        if consecutive_stable_iterations >=Time:
            LeftDrive1111.stop()
            LeftDrive1155.stop()
            RightDrive1111.stop()
            RightDrive1155.stop()          
            break
        if TimeStop == 1:
            wait(3, SECONDS)
            break

        # Set motor velocities
        LeftDrive1111.spin(FORWARD, left_speed2, VOLT)
        LeftDrive1155.spin(FORWARD, left_speed2, VOLT)
        RightDrive1111.spin(FORWARD, right_speed2, VOLT)
        RightDrive1155.spin(FORWARD, right_speed2, VOLT) 


        # Sleep for a short time to avoid excessive loop execution
        wait(20, MSEC)



def Intake_Time_Direction(Intake_Time_Direction__Time, Intake_Time_Direction__Direction):
    global message1, CataShoot, intakeACT, WingsOpen, WingsClose, BlockerUp, Intake2, Test, DriverControl, Autonomous, Started, IntakeAUCT, wings, Intake, Cata, IntakeR, Auton, myVariable, RightDrive, LeftDrive, ErrorDS, OutputDS, Auton1, CataStop, armupdown, LeftRot, RightRot, TurnOSC, Heading, vexcode_brain_precision, vexcode_console_precision, vexcode_controller_1_precision
    CataIntake.spin(FORWARD)
    CataIntake.set_velocity((Intake_Time_Direction__Direction * 100), PERCENT)
    wait(Intake_Time_Direction__Time, SECONDS)
    CataIntake.stop()
    CataIntake.set_velocity(100, PERCENT)

def ondriver_drivercontrol_0():
    global Button_Up
    controller_1.buttonUp.pressed(Button_Up)
    Buttonup = False
    CataIntake.set_position(0, DEGREES)
    global message1, CataShoot, intakeACT, WingsOpen, WingsClose, BlockerUp, Intake2, Test, DriverControl, Autonomous, Started, IntakeAUCT, wings, Intake, Cata, IntakeR, Auton, myVariable, RightDrive, LeftDrive, ErrorDS, OutputDS, Auton1, CataStop, armupdown, LeftRot, RightRot, TurnOSC, Heading, vexcode_brain_precision, vexcode_console_precision, vexcode_controller_1_precision, ButtonUp
    if Button_Up:
        if ButtonUp:
            CataIntake.spin_for(FORWARD, 360,DEGREES)
            wait(.2, SECONDS)
            CataIntake.spin_for(FORWARD, 360,DEGREES)
            wait(1,SECONDS)
            ButtonUp = False
        else:
            CataIntake.spin_to_position(0,DEGREES)
            wait(5,MSEC)
            ButtonUp = True


def onauton_autonomous_0():
    global message1, CataShoot, intakeACT, WingsOpen, WingsClose, BlockerUp, Intake2, Test, DriverControl, Autonomous, Started, IntakeAUCT, wings, Intake, Cata, IntakeR, Auton, myVariable, RightDrive, LeftDrive, ErrorDS, OutputDS, Auton1, CataStop, armupdown, LeftRot, RightRot, TurnOSC, Heading, vexcode_brain_precision, vexcode_console_precision, vexcode_controller_1_precision
    Autonomous = True
    DriverControl = False
    Started = False
    while True:
        controller_1.screen.set_cursor(1,1)
        controller_1.screen.print(inertial_10.heading(DEGREES))
        controller_1.screen.clear_row(1)
        controller_1.screen.set_cursor(2,1)
        controller_1.screen.print(((3.14159265 * Wheel_Diameter) / 360) * (LeftDrive1111.position(DEGREES)) * (Drive_Gear_Ratio))
        controller_1.screen.clear_row(2)
        controller_1.screen.set_cursor(3,1)
        controller_1.screen.print()
        controller_1.screen.clear_row(3)


def when_started1():
    global message1, CataShoot, intakeACT, WingsOpen, WingsClose, BlockerUp, Intake2, Test, DriverControl, Autonomous, Started, IntakeAUCT, wings, Intake, Cata, IntakeR, Auton, myVariable, RightDrive, LeftDrive, ErrorDS, OutputDS, Auton1, CataStop, armupdown, LeftRot, RightRot, TurnOSC, Heading, vexcode_brain_precision, vexcode_console_precision, vexcode_controller_1_precision
    # Devices setup
    LeftDrive1111.set_position(0, DEGREES)
    LeftDrive1111.set_stopping(COAST)
    LeftDrive1155.set_stopping(COAST)
    RightDrive1111.set_stopping(COAST)
    RightDrive1155.set_stopping(COAST)
    LeftDrive1111.set_max_torque(100, PERCENT)
    RightDrive1111.set_max_torque(100, PERCENT)
    LeftDrive1155.set_max_torque(100, PERCENT)
    RightDrive1155.set_max_torque(100, PERCENT)
    CataIntake.set_max_torque(100, PERCENT)
    CataIntake.set_velocity(100, PERCENT)
    inertial_10.set_heading(0, DEGREES)
    inertial_10.calibrate()
    while inertial_10.is_calibrating():
        sleep(50)
    CataIntake.set_stopping(HOLD)
    while True:
        if DriverControl or Started:
            LeftDrive1111.set_velocity(LeftDrive, PERCENT)
            LeftDrive1155.set_velocity(LeftDrive, PERCENT)
            RightDrive1111.set_velocity(RightDrive, PERCENT)
            RightDrive1155.set_velocity(RightDrive, PERCENT)
            LeftDrive1111.spin(FORWARD)
            LeftDrive1155.spin(FORWARD)
            RightDrive1111.spin(FORWARD)
            RightDrive1155.spin(FORWARD)
        wait(5, MSEC)
    wait(5, MSEC)

def when_started2():
    global message1, CataShoot, intakeACT, WingsOpen, WingsClose, BlockerUp, Intake2, Test, DriverControl, Autonomous, Started, IntakeAUCT, wings, Intake, Cata, IntakeR, Auton, myVariable, RightDrive, LeftDrive, ErrorDS, OutputDS, Auton1, CataStop, armupdown, LeftRot, RightRot, TurnOSC, Heading, vexcode_brain_precision, vexcode_console_precision, vexcode_controller_1_precision
    Started = True
    DriverControl = False
    Autonomous = False

def ondriver_drivercontrol_1():
    global message1, CataShoot, intakeACT, WingsOpen, WingsClose, BlockerUp, Intake2, Test, DriverControl, Autonomous, Started, IntakeAUCT, wings, Intake, Cata, IntakeR, Auton, myVariable, RightDrive, LeftDrive, ErrorDS, OutputDS, Auton1, CataStop, armupdown, LeftRot, RightRot, TurnOSC, Heading, vexcode_brain_precision, vexcode_console_precision, vexcode_controller_1_precision
    # Controller Controls
    while True:
        if DriverControl or Started:
            LeftDrive = controller_1.axis3.position() + 0.01 * (controller_1.axis1.position() * math.fabs(controller_1.axis1.position()))
            RightDrive = controller_1.axis3.position() + -0.01 * (controller_1.axis1.position() * math.fabs(controller_1.axis1.position()))
        if controller_1.buttonL2.pressing():
            if IntakeAUCT:
                IntakeACT.set(True)
                IntakeAUCT = False
                wait(0.5, SECONDS)
            else:
                IntakeACT.set(False)
                IntakeAUCT = True
                wait(0.5, SECONDS)
            intakeACT.broadcast()
        if controller_1.buttonL1.pressing():
            if wings:
                Wings.set(True)
                wings = False
                wait(0.5, SECONDS)
            else:
                Wings.set(False)
                wings = True
                wait(0.5, SECONDS)
            BlockerUp.broadcast()
        wait(5, MSEC)

def ondriver_drivercontrol_2():
    global message1, CataShoot, intakeACT, WingsOpen, WingsClose, BlockerUp, Intake2, Test, DriverControl, Autonomous, Started, IntakeAUCT, wings, Intake, Cata, IntakeR, Auton, myVariable, RightDrive, LeftDrive, ErrorDS, OutputDS, Auton1, CataStop, armupdown, LeftRot, RightRot, TurnOSC, Heading, vexcode_brain_precision, vexcode_console_precision, vexcode_controller_1_precision
    while True:
        for repeat_count in range(10):
            if IntakeAUCT:
                brain.screen.set_cursor(1, 1)
                controller_1.screen.print("Arm UP")
            else:
                controller_1.screen.print("Arm DOWN")
            wait(0.5, SECONDS)
            controller_1.screen.clear_row(1)
            controller_1.screen.set_cursor(controller_1.screen.row(), 1)
            wait(5, MSEC)
        wait(5, MSEC)

def when_started3():
    global message1, CataShoot, intakeACT, WingsOpen, WingsClose, BlockerUp, Intake2, Test, DriverControl, Autonomous, Started, IntakeAUCT, wings, Intake, Cata, IntakeR, Auton, myVariable, RightDrive, LeftDrive, ErrorDS, OutputDS, Auton1, CataStop, armupdown, LeftRot, RightRot, TurnOSC, Heading, vexcode_brain_precision, vexcode_console_precision, vexcode_controller_1_precision
    while True:
        if DriverControl or Started:
            for repeat_count2 in range(10):
                if controller_1.buttonR2.pressing():
                    CataIntake.set_velocity(100, PERCENT)
                    CataIntake.spin(FORWARD)
                else:
                    if not (Cata and controller_1.buttonR1.pressing()):
                        wait(0.01, SECONDS)
                        CataIntake.stop()
                    if controller_1.buttonR1.pressing():
                        CataIntake.set_velocity(50, PERCENT)
                        CataIntake.spin(REVERSE)
                    else:
                        if not (controller_1.buttonR2.pressing() and Cata):
                            wait(0.01, SECONDS)
                            CataIntake.stop()
                wait(5, MSEC)
        wait(5, MSEC)

def onauton_autonomous_1():
    global message1, CataShoot, intakeACT, WingsOpen, WingsClose, BlockerUp, Intake2, Test, DriverControl, Autonomous, Started, IntakeAUCT, wings, Intake, Cata, IntakeR, Auton, myVariable, RightDrive, LeftDrive, ErrorDS, OutputDS, Auton1, CataStop, armupdown, LeftRot, RightRot, TurnOSC, Heading, vexcode_brain_precision, vexcode_console_precision, vexcode_controller_1_precision
    inertial_10.set_heading(0, DEGREES)
    inertial_10.calibrate()
    while inertial_10.is_calibrating():
        sleep(65)
    drive_turn(24, 1, 50)


# 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 )
    auton_task_1 = Thread( onauton_autonomous_1 )
    # 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()
    auton_task_1.stop()

def vexcode_driver_function():
    # Start the driver control tasks
    driver_control_task_0 = Thread( ondriver_drivercontrol_0 )
    driver_control_task_1 = Thread( ondriver_drivercontrol_1 )
    driver_control_task_2 = Thread( ondriver_drivercontrol_2 )

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


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

ws2 = Thread( when_started2 )
ws3 = Thread( when_started3 )
when_started1()

[details="Summary"]
[spoiler]This text will be hidden[/spoiler]
[/details]
8

My best guess is that the loop in onauton_autonomous_0 has no wait in it, so the task scheduler never gets a chance to run the other tasks. My only other ideas are that maybe the inertial isn’t calibrating or the random backslash in the if-else for increasing consecutive_stable_iterations is messing something up. Other then that I don’t know what could be causing it, if none of this fixes it I would recommend doing what @u89djt said and log values you think you know to the console and see if it’s doing what you think it should be doing, which should help narrow down the problem.

1 Like
9

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.