PID issues with a tourney this weekend

Programming Support VEXcode V5 Tech Support
,
1

Here is my code

Drive_Gear_Ratio = (36/48)

max_speed = 100
min_speed = 50

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

kp = .5
ki = 0
kd = .01
kf = .3

kp2 = 5
ki2 = 0
kd2 = 0
kf2 = 5

def drive_distance(target_distance):
    global error, integral, derivative, previous_error
    
    # Reset PID variables
    error = 0
    integral = 0
    derivative = 0
    previous_error = 0
    
    # Main PID loop
    while True:
        current_distance = ((3.14159265 * Wheel_Diameter) / 360) * (LeftDrive1111_motor_a.position(DEGREES)) * (Drive_Gear_Ratio)
        error = target_distance - current_distance
        integral = integral + error
        derivative = error - previous_error
        output = kp * error + ki * integral + kd * derivative
        feedforward = kf * target_distance
        speed = max(min_speed, min(max_speed, feedforward + max_speed - abs(output)))
        left_speed = speed
        right_speed = speed
        if error < 0:
            left_speed *= -1
            right_speed *= -1
        LeftDrive1111.set_velocity(left_speed, PERCENT)
        LeftDrive1155.set_velocity(left_speed, PERCENT)
        RightDrive1111.set_velocity(right_speed, PERCENT)
        RightDrive1155.set_velocity(right_speed, PERCENT)
        previous_error = error
        wait(20, MSEC)
        # Break the loop when close enough to the target
        if abs(error) < 1:
            LeftDrive1111_motor_a.set_velocity(0, PERCENT)
            LeftDrive1155_motor_a.set_velocity(0, PERCENT)
            RightDrive1111_motor_a.set_velocity(0, PERCENT)
            RightDrive1155_motor_a.set_velocity(0, PERCENT)
            break

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

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

    # Main PID loop for turning
    while True:
        current_angle = inertial_10.heading(DEGREES)
        error = target_angle - current_angle
        integral = integral + error
        derivative = error - previous_error
        output = kp2 * error + ki2 * integral + kd2 * derivative
        feedforward = kf2 * target_angle
        speed = max(min_speed, min(max_speed, feedforward + max_speed - abs(output)))
        left_speed = -1 * speed
        right_speed = speed
        LeftDrive1111_motor_a.set_velocity(left_speed, PERCENT)
        LeftDrive1155_motor_a.set_velocity(left_speed, PERCENT)
        RightDrive1111_motor_a.set_velocity(right_speed, PERCENT)
        RightDrive1155_motor_a.set_velocity(right_speed, PERCENT)
        previous_error = error
        wait(20, MSEC)
        if abs(error) < 1:
            LeftDrive1111_motor_a.set_velocity(0, PERCENT)
            LeftDrive1155_motor_a.set_velocity(0, PERCENT)
            RightDrive1111_motor_a.set_velocity(0, PERCENT)
            RightDrive1155_motor_a.set_velocity(0, PERCENT)
            break

As far as I know, I think my code is wrong because I have tried to tune these PIDs for 3 days and the drive PID will overshoot by triple the desired value, and the turning PID is so weak it will barely move. I think my code is wrong because I have tried to change every value and tune it with multiple different methods and nothing will get these to work. The worst thing is that I can’t even test the turning PID because I haven’t gotten it to have a high enough velocity to have the torque to move the robot. I also tried to have the PID values the same for both the PIDs to see if I would get a better velocity out of my turning PID and nothing happened still.

Please share your thoughts on this and try to help I really need to get this going for my tournament this weekend.

2

You can try disabling the feedforward control, because my team has never needed feedforward for our PID, and I think it could just be causing problems. Also, you should probably replace feedforward + max_speed - abs(output) on the line where you apply the min and max speed with just abs(output). You should also reset the drivetrain motor encoders in the beginning of the drive function because right now it is looking at the global values, which will make it not always go the right distance after the first drive. You also don’t have it inverse the sign of the speed in the turn PID when error is 0, so it can’t turn to negative angles. Other then that I can’t think of anything else which could be wrong besides your wheel diameter/gear ratio or your tuning.

1 Like
3

I need the feed forward because i drive with my lifted launcher and blocker up and my drive is 450 so it will flip without it. I did all that to the code and the turning didn’t change at all. It still acts like its does not have enough power to move.

On a side note though I think the global variable was messing up my drive fwd PID so thank you.

4

Sorry I didn’t explain why feedforward was causing problems, what I meant was that right now your implementation of feedforward makes the robot go faster when it gets closer to the target, the exact opposite of what PID should be doing, and also . If the target distance is 20 inches and the robot is 2 inches away from the target, meaning you should probably slow down to stop smoothly, feedforward + max_speed - abs(output) evaluates to 105, which is over max speed. If you still want feedforward, this part of the code should be replaced by abs(feedforward + output), with output getting added instead of subtracted and max speed removed.

However, I don’t think feedforward will help prevent you from tipping. All it does it artificially make the robot go faster if it is driving a further distance, which may be useful in some cases but won’t stop you from tipping. A better way to prevent tipping is to apply a max acceleration to prevent the robot from speeding up or down too fast. Here’s some example code which does that:

if abs(output - previous_output) > max_acceleration :
    output = previous_output + max_acceleration  * sign(output - previous_output)

previous_output = output

where the sign function outputs the input divided by it’s absolute value. This code checks if the output of the PID changes by too big of a value, and if it does it limits the change in output to be the maximum change in speed. Be warned though, too high of a max acceleration may make the PID harder to tune and makes your movements take a lot longer.

Also, why are you having the robot drive in the auto period with the blocker and raised shooter up? They don’t have any use during auto, so you can just wait until after auto to deploy them.

As for why your turn PID isn’t working, I’m not sure what could be causing the problem. It may be caused by another part of the code, so can you send the part of your code where you run the turn function? Your inertial sensor could also just not be working, make sure it is plugged into the right port and . You can also try logging the output of the PID into the console or on the brain screen so you can see what the motors are getting told to do.

5

I thought feed-forward got the robot up to speed and then slowed it down at the end of a movement so that it has smooth stops. Is that not what it was doing?

6

i know the inertial is working because the robot will everso slightly move while turning and it will stop at the desired value.

Anyway here is all my code it is a little messy.

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.PORT4, 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.PORT1, GearSetting.RATIO_6_1, True)
RightDrive1155 = MotorGroup(RightDrive1155_motor_a, RightDrive1155_motor_b)
CataIntake = Motor(Ports.PORT12, 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)


# 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

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 = .0000001
ki = 4
kd = .00001
kf = .3

kp2 = 500
ki2 = 500
kd2 = 500
kf2 = 500

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

    LeftDrive1111.set_position(0, DEGREES)
    
    # Reset PID variables
    error = 0
    integral = 0
    derivative = 0
    previous_error = 0
    
    # Main PID loop
    while True:
        current_distance = ((3.14159265 * Wheel_Diameter) / 360) * (LeftDrive1111_motor_a.position(DEGREES)) * (Drive_Gear_Ratio)
        error = target_distance - current_distance
        integral = integral + error
        derivative = error - previous_error
        output = kp * error + ki * integral + kd * derivative
        feedforward = kf * target_distance
        speed = max(min_speed, min(max_speed, abs(output)))
        left_speed = speed
        right_speed = speed
        if error < 0:
            left_speed *= -1
            right_speed *= -1
        LeftDrive1111.set_velocity(left_speed, PERCENT)
        LeftDrive1155.set_velocity(left_speed, PERCENT)
        RightDrive1111.set_velocity(right_speed, PERCENT)
        RightDrive1155.set_velocity(right_speed, PERCENT)
        previous_error = error
        wait(20, MSEC)
        # Break the loop when close enough to the target
        if abs(error) < 1:
            LeftDrive1111_motor_a.set_velocity(0, PERCENT)
            LeftDrive1155_motor_a.set_velocity(0, PERCENT)
            RightDrive1111_motor_a.set_velocity(0, PERCENT)
            RightDrive1155_motor_a.set_velocity(0, PERCENT)
            break

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

    LeftDrive1111.set_position(0, DEGREES)    

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

    # Main PID loop for turning
    while True:
        current_angle = inertial_10.heading(DEGREES)
        error = target_angle - current_angle
        integral = integral + error
        derivative = error - previous_error
        output = kp2 * error + ki2 * integral + kd2 * derivative
        feedforward = kf2 * target_angle
        speed = max(min_speed, min(max_speed, abs(output)))
        left_speed2 = -1 * speed
        right_speed2 = speed
        LeftDrive1111_motor_a.set_velocity(left_speed2, PERCENT)
        LeftDrive1155_motor_a.set_velocity(left_speed2, PERCENT)
        RightDrive1111_motor_a.set_velocity(right_speed2, PERCENT)
        RightDrive1155_motor_a.set_velocity(right_speed2, PERCENT)

        # Update previous error
        previous_error = error

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

        # Break the loop when close enough to the target angle
        if abs(error) < 1:
            LeftDrive1111_motor_a.set_velocity(0, PERCENT)
            LeftDrive1155_motor_a.set_velocity(0, PERCENT)
            RightDrive1111_motor_a.set_velocity(0, PERCENT)
            RightDrive1155_motor_a.set_velocity(0, PERCENT)
            break


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

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(BRAKE)
    LeftDrive1155.set_stopping(BRAKE)
    RightDrive1111.set_stopping(BRAKE)
    RightDrive1155.set_stopping(BRAKE)
    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)
    CataStop = -27
    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)

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:
        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(70, 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(50)
    CataIntake.spin(FORWARD)
    drive_distance(1)
    drive_distance(-12)
    wait(0, SECONDS)
    turn_to_angle(235)
    CataIntake.stop()
    wait(0, SECONDS)
    drive_distance(1)
    turn_to_angle(270)
    drive_distance(5)

    while True:
        LeftDrive1111.spin(FORWARD)
        LeftDrive1155.spin(FORWARD)
        RightDrive1111.spin(FORWARD)
        RightDrive1155.spin(FORWARD)
        wait(5, MSEC)

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

That’s what the P term of PID does, not feedforward. The point of feedforward is to modify the output based on what you expect is about to happen, like if you are using a PID to control the speed of a flywheel feedforward can be used to increase the power going into the flywheel right before you shoot.

The rest of you code doesn’t look like it is messing it up, but I did just notice that you only set the value of the motor_a motors in the turn PID and the stop condition of the drive PID, instead of the whole motor group, which might be messing it up because your motors are physically fighting each other. I can’t think of any other thing it could be, so it is probably that.

8

You were right, I accidentally had them all set to motor a they weren’t fighting each other the motors I had set to spin just weren’t powerful enough to have the robot move without the other motors. Thank you for the explanation of feed forward too I’m going to remove that.