PID Loop Not Driving backwards properly

Programming Support
,
1

Hi, I’m from 98709C in and we are trying to use a PID Loop to program our inertial sensor for our autonomous programs. The code works great for driving forwards however, when we use a negative value to drive backwards, it just turns in circles or does some other random thing. We also can’t get it to turn towards the left, and this is super important because our previous programs all rely on turning to the left. Here is of my PID Loop code with a little bit of our autonomous underneath all of it, it basically repeats after a while. Thanks for the help!

#include “vex.h”

using namespace vex;

//Tuning Constants for Distance
double kP = 2.0;
double kI = 0.0;
double kD = 3.6;

//Tuning Constants for Turn
double turnkP = 1.7;
double turnkI = 0.0;
double turnkD = 0.465;

//Establish a Maximum Power; Avoid Going Too Fast
double voltageThreshhold = 8;

//Establish Variables Used in Control Loop
int desiredValue = 0;
int turnDesiredValue = 0;

int error;
int prevError = 0;
int derivative;
int totalError = 0;

int turnError;
int turnPrevError = 0;
int turnDerivative;
int turnTotalError = 0;

//Allows You to Reset Your Loop
bool resetDriveSensors = false;

//Allows You to Turn Off PID Control in Driver Control
bool enableDrivePID = true;

int drivePID()
{
while(enableDrivePID)
{

if(resetDriveSensors)
{
  resetDriveSensors = false;
  BackLeft.setPosition(0,degrees);
  BackRight.setPosition(0,degrees);
  Inertial.setHeading(0,degrees);
}

int leftMotorPosition = BackLeft.position(degrees);
int rightMotorPosition = BackRight.position(degrees);

////////////////////////////////////////////////////
//Drive PID
////////////////////////////////////////////////////
   
int averagePosition = (leftMotorPosition + rightMotorPosition)/2;

error = desiredValue - averagePosition;

derivative = error - prevError;

totalError = totalError + error;

double motorPower = (error * kP + derivative * kD + totalError * kI)/12.0;

if (fabs(motorPower) > voltageThreshhold)
{
  motorPower = voltageThreshhold;
}

///////////////////////////////////////////////////
//Turn PID
///////////////////////////////////////////////////

//int turnDifference = leftMotorPosition - rightMotorPosition;

//turnError = turnDesiredValue - turnDifference;

turnError = turnDesiredValue - Inertial.heading(degrees);
  
turnDerivative = turnError - turnPrevError;

turnTotalError = turnTotalError + turnError;

double turnMotorPower = (turnError * turnkP + turnDerivative * turnkD + turnTotalError * turnkI)/12.0;

if (fabs(turnMotorPower) > voltageThreshhold)
{
  turnMotorPower = voltageThreshhold;
}

///////////////////////////////////////////////////
//Motor Commands
///////////////////////////////////////////////////

double powerScaler = 1.0;

double leftMotorPower = powerScaler * (motorPower + turnMotorPower);
double rightMotorPower = powerScaler * (motorPower - turnMotorPower);


FrontLeft.spin(forward, leftMotorPower, voltageUnits::volt);
FrontRight.spin(forward, rightMotorPower, voltageUnits::volt);
BackLeft.spin(forward, leftMotorPower, voltageUnits::volt);
BackRight.spin(forward, rightMotorPower, voltageUnits::volt);



prevError = error;
turnPrevError = turnError;
vex::task::sleep(20);

}

return 1;
}

int InertialReading()
{
while(true)
{
Brain.Screen.clearLine(1,color::black);
Brain.Screen.print(“Inertial Heading: %f”, Inertial.heading(degrees));
}
}

//////////////////////////////////////////auton starts here/////////////////////////////////////////////////////////
int main()
{
// Initializing Robot Configuration. DO NOT REMOVE!
int fastspeed=100;
int slowspeed=50;
int encoders=0;
int speed=90;
// Initializing Robot Configuration. DO NOT REMOVE!
vexcodeInit();

Inertial.calibrate();

while(Inertial.isCalibrating())
{
wait(100,msec);
}

TopRoller.spin(fwd,fastspeed,vex::velocityUnits::pct); //shooting preload into goal in middle against wall
wait(700,msec);
BottomRoller.spin(fwd,fastspeed,vex::velocityUnits::pct);
TopRoller.spin(fwd,fastspeed,vex::velocityUnits::pct);

vex::task::sleep(2000);

vex::task runDrivePID(drivePID);
vex::task InertialReadingTask(InertialReading);

resetDriveSensors = true;
desiredValue = 1000;
turnDesiredValue= 0;
vex::task::sleep(3000);

resetDriveSensors = true;
desiredValue = 0;
turnDesiredValue= 180;
vex::task::sleep(3000);

2 Likes
2

Welcome to the VEXForum @Heather_Rose!

Just as a suggestion, you can use [code] .... [/code] tags to format your code for easier readability. Here is your code formatted for anyone else reading this topic.

#include “vex.h”

using namespace vex;

//Tuning Constants for Distance
double kP = 2.0;
double kI = 0.0;
double kD = 3.6;

//Tuning Constants for Turn
double turnkP = 1.7;
double turnkI = 0.0;
double turnkD = 0.465;

//Establish a Maximum Power; Avoid Going Too Fast
double voltageThreshhold = 8;

//Establish Variables Used in Control Loop
int desiredValue = 0;
int turnDesiredValue = 0;

int error;
int prevError = 0;
int derivative;
int totalError = 0;

int turnError;
int turnPrevError = 0;
int turnDerivative;
int turnTotalError = 0;

//Allows You to Reset Your Loop
bool resetDriveSensors = false;

//Allows You to Turn Off PID Control in Driver Control
bool enableDrivePID = true;

int drivePID()
{
while(enableDrivePID)
{


if(resetDriveSensors)
{
  resetDriveSensors = false;
  BackLeft.setPosition(0,degrees);
  BackRight.setPosition(0,degrees);
  Inertial.setHeading(0,degrees);
}

int leftMotorPosition = BackLeft.position(degrees);
int rightMotorPosition = BackRight.position(degrees);

////////////////////////////////////////////////////
//Drive PID
////////////////////////////////////////////////////
   
int averagePosition = (leftMotorPosition + rightMotorPosition)/2;

error = desiredValue - averagePosition;

derivative = error - prevError;

totalError = totalError + error;

double motorPower = (error * kP + derivative * kD + totalError * kI)/12.0;

if (fabs(motorPower) > voltageThreshhold)
{
  motorPower = voltageThreshhold;
}

///////////////////////////////////////////////////
//Turn PID
///////////////////////////////////////////////////

//int turnDifference = leftMotorPosition - rightMotorPosition;

//turnError = turnDesiredValue - turnDifference;

turnError = turnDesiredValue - Inertial.heading(degrees);
  
turnDerivative = turnError - turnPrevError;

turnTotalError = turnTotalError + turnError;

double turnMotorPower = (turnError * turnkP + turnDerivative * turnkD + turnTotalError * turnkI)/12.0;

if (fabs(turnMotorPower) > voltageThreshhold)
{
  turnMotorPower = voltageThreshhold;
}

///////////////////////////////////////////////////
//Motor Commands
///////////////////////////////////////////////////

double powerScaler = 1.0;

double leftMotorPower = powerScaler * (motorPower + turnMotorPower);
double rightMotorPower = powerScaler * (motorPower - turnMotorPower);


FrontLeft.spin(forward, leftMotorPower, voltageUnits::volt);
FrontRight.spin(forward, rightMotorPower, voltageUnits::volt);
BackLeft.spin(forward, leftMotorPower, voltageUnits::volt);
BackRight.spin(forward, rightMotorPower, voltageUnits::volt);



prevError = error;
turnPrevError = turnError;
vex::task::sleep(20);


}

return 1;
}

int InertialReading()
{
while(true)
{
Brain.Screen.clearLine(1,color::black);
Brain.Screen.print(“Inertial Heading: %f”, Inertial.heading(degrees));
}
}

//////////////////////////////////////////auton starts here/////////////////////////////////////////////////////////
int main()
{
// Initializing Robot Configuration. DO NOT REMOVE!
int fastspeed=100;
int slowspeed=50;
int encoders=0;
int speed=90;
// Initializing Robot Configuration. DO NOT REMOVE!
vexcodeInit();

Inertial.calibrate();

while(Inertial.isCalibrating())
{
wait(100,msec);
}

TopRoller.spin(fwd,fastspeed,vex::velocityUnits::pct); //shooting preload into goal in middle against wall
wait(700,msec);
BottomRoller.spin(fwd,fastspeed,vex::velocityUnits::pct);
TopRoller.spin(fwd,fastspeed,vex::velocityUnits::pct);

vex::task::sleep(2000);

vex::task runDrivePID(drivePID);
vex::task InertialReadingTask(InertialReading);

resetDriveSensors = true;
desiredValue = 1000;
turnDesiredValue= 0;
vex::task::sleep(3000);

resetDriveSensors = true;
desiredValue = 0;
turnDesiredValue= 180;
vex::task::sleep(3000);

I haven’t managed to fully read through your code, but I did notice this

and

Let’s pretend that turnMotorPower is equal to -10 (just as an example). You stated voltageThreshhold is equal to 8. (just as a spelling note, threshold is spelled with one h, but since it’s code I’m keeping it as threshhold for clarity)

if (fabs(turnMotorPower) > voltageThreshhold) { 
turnMotorPower = voltageThreshhold; 
}

If the absolute value of -10 is greater than 8, which is true. So then it proceeds to do the condition. The turnMotorPower is set to the voltageThreshhold, which you stated is equal to 8.

So turnMotorPower starts this if statement as -10, and ends as 8. As you can see, this would be problematic.

You can fix this by changing the turnMotorPower to be set to voltageThreshhold times the sign of turnMotorPower (+ or - 1). Can you think of a way to do this in code?

Answer 
turnMotorPower = ( voltageThreshhold * (turnMotorPower / fabs(turnMotorPower) ) );

You would do the same for motorPower, since the same issue exists there.

Is this the only issue with the code? Maybe. It’s tough to know since I haven’t looked through the entire code. But I think this is probably an issue.

Well, thank you for reading this entire post. Hopefully this helps, even if it is 2 days after the initial post.

11 Likes
3

Thank you so much! Will try this at robotics practice tomorrow!

6 Likes
4

I’m curious, is there any reason to do /12 when calculating motor power? Does this just help with super tiny numbers in tuning?

5

It just helps with capping how much voltage is given to the motors to help speed up or slow down the motors.

4 Likes
6

Well, dividing by twelve here is not really necessary at all; in fact I’m not even sure why it’s there.

2 Likes
7

Yes, I’ve heard that before. I’ve also seen dividing by twelve in other PID loop, notably the PID with python turtle by @Skynet: https://hdprojects.dev/pid/
I have a feeling it’s to not make the constants so annoying to adjust, but to each his own.

2 Likes
8

Hey! So did this end up being you error?: [quote=“trontech569, post:2, topic:87478”]

if (fabs(turnMotorPower) > voltageThreshhold) { 
turnMotorPower = voltageThreshhold; 
}

[/quote]

9

Please do not revive old threads. @DRow please lock the thread.

3 Likes