Hi,
I recently wrote a pid code. An issue I am encountering is that it will consistently drive to far and then reverse when the integral value increases. Here is a look at the code that makes the robot drive forward, turn around, and then drive back. Thanks for the help.
// ---- START VEXCODE CONFIGURED DEVICES ----
// ---- END VEXCODE CONFIGURED DEVICES ----
#include "vex.h"
using namespace vex;
motor leftFront = motor(PORT1, ratio18_1,false);
motor leftBack = motor(PORT11, ratio18_1,false);
motor rightFront = motor(PORT10, ratio18_1,true);
motor rightBack = motor(PORT20, ratio18_1,true);
int desiredValue = 0;
int Kp = 100;
int Ki = 10000;
int Kd = 100;
int totalError = 0;
int prevError = 0;
bool enableYPID = false;
bool enableTurnPID = false;
int main() {
// Initializing Robot Configuration. DO NOT REMOVE!
vexcodeInit();
leftFront.setStopping(hold); //tell the drivebase to not let the wheels spin unless told too.
rightFront.setStopping(hold);
leftBack.setStopping(hold);
rightBack.setStopping(hold);
enableYPID = true;
int desiredValue = 1800;
while(enableYPID) {
int averagePosition = (leftFront.position(degrees) + leftBack.position(degrees ) + rightFront.position(degrees) + rightBack.position(degrees ))/2;
int error = averagePosition - desiredValue;
totalError += error;
int diffrenceError = error - prevError;
int Speed = error / Kp + totalError / Ki + diffrenceError / Kd;
leftFront.spin(reverse, Speed, voltageUnits::volt);
rightFront.spin(reverse, Speed, voltageUnits::volt);
leftBack.spin(reverse, Speed, voltageUnits::volt);
rightBack.spin(reverse, Speed, voltageUnits::volt);
prevError = error;
wait(0.02, sec);
if( error < 3 and error > -3 ){
enableYPID = false;
}
}
enableYPID = false;
leftFront.spin(reverse, 0, voltageUnits::volt);
rightFront.spin(reverse, 0, voltageUnits::volt);
leftBack.spin(reverse, 0, voltageUnits::volt);
rightBack.spin(reverse, 0, voltageUnits::volt);
Brain.Screen.print("stuff");
wait(1, sec);
enableTurnPID = true;
int desiredTurnValue = 1800;
while(enableTurnPID) {
int averagePosition = (leftFront.position(degrees) + leftBack.position(degrees ) - rightFront.position(degrees) - rightBack.position(degrees ))/2;
int error = averagePosition - desiredTurnValue;
totalError += error;
int diffrenceError = error - prevError;
int Speed = error / Kp + totalError / Ki + diffrenceError / Kd;
leftFront.spin(reverse, Speed, voltageUnits::volt);
rightFront.spin(forward, Speed, voltageUnits::volt);
leftBack.spin(reverse, Speed, voltageUnits::volt);
rightBack.spin(forward, Speed, voltageUnits::volt);
prevError = error;
wait(0.02, sec);
if( error < 3 and error > -3 ){
enableTurnPID = false;
}
}
enableTurnPID = false;
leftFront.spin(reverse, 0, voltageUnits::volt);
rightFront.spin(reverse, 0, voltageUnits::volt);
leftBack.spin(reverse, 0, voltageUnits::volt);
rightBack.spin(reverse, 0, voltageUnits::volt);
Brain.Screen.print(" more stuff");
wait(1, sec);
enableYPID = true;
desiredValue = 3600;
while(enableYPID) {
int averagePosition = (leftFront.position(degrees) + leftBack.position(degrees ) + rightFront.position(degrees) + rightBack.position(degrees ))/2;
int error = averagePosition - desiredValue;
totalError += error;
int diffrenceError = error - prevError;
int Speed = error / Kp + totalError / Ki + diffrenceError / Kd;
leftFront.spin(reverse, Speed, voltageUnits::volt);
rightFront.spin(reverse, Speed, voltageUnits::volt);
leftBack.spin(reverse, Speed, voltageUnits::volt);
rightBack.spin(reverse, Speed, voltageUnits::volt);
prevError = error;
wait(0.02, sec);
if( error < 3 and error > -3 ){
enableYPID = false;
}
}
enableYPID = false;
leftFront.spin(reverse, 0, voltageUnits::volt);
rightFront.spin(reverse, 0, voltageUnits::volt);
leftBack.spin(reverse, 0, voltageUnits::volt);
rightBack.spin(reverse, 0, voltageUnits::volt);
Brain.Screen.print(" even more stuff");
}