Vex Code PID Not Working

Programming Support VEXcode Pro V5 Text Tech Support
1

We are working on a PID for our autonomous and for some reason when we try to make it go straight it ends up spinning the robot so here is the code and were wondering if you are able to fix it.

 if Drive_straight_Distance_Heading_Velocity_KP__Velocity > 0:
        while FLW.position(DEGREES) < Drive_straight_Distance_Heading_Velocity_KP__Distance:
            Error = Drive_straight_Distance_Heading_Velocity_KP__Heading + Inertial_.rotation(DEGREES)
            Output = Error * Drive_straight_Distance_Heading_Velocity_KP__KP
            FLW.set_velocity((Drive_straight_Distance_Heading_Velocity_KP__Velocity - Output), PERCENT)
            FRW.set_velocity((Drive_straight_Distance_Heading_Velocity_KP__Velocity + Output), PERCENT)
            MLW.set_velocity((Drive_straight_Distance_Heading_Velocity_KP__Velocity - Output), PERCENT)
            MRW.set_velocity((Drive_straight_Distance_Heading_Velocity_KP__Velocity + Output), PERCENT)
            BLW.set_velocity((Drive_straight_Distance_Heading_Velocity_KP__Velocity - Output), PERCENT)
            BRW.set_velocity((Drive_straight_Distance_Heading_Velocity_KP__Velocity + Output), PERCENT)
            FLW.spin(FORWARD)
            FRW.spin(FORWARD)
            MLW.spin(FORWARD)
            MRW.spin(FORWARD)
            BLW.spin(FORWARD)
            BRW.spin(FORWARD)
            wait(5, MSEC)
    else:
        while FLW.position(DEGREES) > Drive_straight_Distance_Heading_Velocity_KP__Distance:
            Error = Drive_straight_Distance_Heading_Velocity_KP__Heading - Inertial_.rotation(DEGREES)
            Error = Error * Drive_straight_Distance_Heading_Velocity_KP__KP
            FLW.set_velocity((Drive_straight_Distance_Heading_Velocity_KP__Velocity - Output), PERCENT)
            MLW.set_velocity((Drive_straight_Distance_Heading_Velocity_KP__Velocity - Output), PERCENT)
            BLW.set_velocity((Drive_straight_Distance_Heading_Velocity_KP__Velocity - Output), PERCENT)
            FRW.set_velocity((Drive_straight_Distance_Heading_Velocity_KP__Velocity + Output), PERCENT)
            MRW.set_velocity((Drive_straight_Distance_Heading_Velocity_KP__Velocity + Output), PERCENT)
            BRW.set_velocity((Drive_straight_Distance_Heading_Velocity_KP__Velocity + Output), PERCENT)
            FLW.spin(FORWARD)
            MLW.spin(FORWARD)
            BLW.spin(FORWARD)
            FRW.spin(FORWARD)
            MRW.spin(FORWARD)
            BRW.spin(FORWARD)
            wait(5, MSEC)
    FLW.stop()
    MLW.stop()
    BLW.stop()
    FRW.stop()
    MRW.stop()
    BRW.stop()
2

Why do left/right have different signs?

I would display my variables/etc to the brain/controller screen and watch them. Should be able to narrow this down pretty quickly.

3

I am not the most qualified in PID loops, but the only places I can see that might cause an error are in the error definitions. maybe try playing around with those lines, the “+” and “-” signs.