```
void pidforward(float x)
{
x=x/(12.56636*(3/2));
initialize;
double error = Fl.rotation(rotationUnits::rev) - Fr.rotation(rotationUnits::rev);
double prevError; //defines a bunch of variables with useless names
double integral;
double derivative;
double adjustment;
double speed;
while(((Fl.rotation(rotationUnits::rev)+Fr.rotation(rotationUnits::rev))/2)>x) //while the encoder ticks are less than x, do this
{
prevError = error; //keeps a history of error (you need two points if you want to have a slope)
error = Fl.rotation(rotationUnits::rev) - Fr.rotation(rotationUnits::rev); //calculate the difference between the two sides' encoder ticks
derivative = error-prevError; //calculates the slope of the change in error
integral = integral+error; //sums the integral of the error over time
adjustment = Kp*error + Ki*integral + Kd*derivative; //sets the adjustment to the PID formula, Kp, Ki, and Kd are constants that need to be fine tuned
speed = (100*(x-((Fl.rotation(rotationUnits::rev)+Fr.rotation(rotationUnits::rev))/2))/x)+20; //determines the base speeds of the motors, P loop in terms of distance
Fl.spin(directionType::fwd, speed + adjustment, velocityUnits::rpm); //adjusts the base speeds of the motors to keep the robot straight
Fr.spin(directionType::fwd, speed - adjustment, velocityUnits::rpm);
Br.spin(directionType::fwd, speed - adjustment, velocityUnits::rpm);
Bl.spin(directionType::fwd, speed + adjustment, velocityUnits::rpm);
task::sleep(5); //don't hog the cpu
} //When the encoder ticks reach the desired value, exit while loop
Fl.stop(); //stop
Fr.stop();
Br.stop();
Bl.stop();
}
void pdGyroTurnRight(float x)
{
float currentVal = Gyro1.value(rotationUnits::deg);
float desired = currentVal+x;
double speed;
float error;
float prevError;
float derivative;
float adjustment;
while(Gyro1.value(rotationUnits::deg)<desired)
{
prevError = error; //keeps a history of error (you need two points if you want to have a slope)
error = Gyro1.value(rotationUnits::deg) - desired; //calculate the difference between the two sides' encoder ticks
derivative = error-prevError; //calculates the slope of the change in error
adjustment = Kp*error + Kd*derivative; //sets the adjustment to the PD formula, Kp and Kd are constants that need to be fine tuned
speed = 20;
Fl.spin(directionType::fwd, speed + adjustment, velocityUnits::rpm); //adjusts the base speeds of the motors to keep the robot straight
Fr.spin(directionType::fwd, -(speed + adjustment), velocityUnits::rpm);
Br.spin(directionType::fwd, -(speed + adjustment), velocityUnits::rpm);
Bl.spin(directionType::fwd, speed + adjustment, velocityUnits::rpm);
task::sleep(5); //don't hog the cpu
}
Fl.stop();
Fr.stop();
Br.stop();
Bl.stop();
}
```

Our coder’s too lazy to post it, so I will. This is our PID code. Instead of working, it causes our robot to spin endlessly in circles. We’ve tried changing signs on some of the motors’ correction commands but it doesn’t work. Is there something else that’s wrong with it?