Hey everybody, I just made a few PID’s for my robot and I was wondering if there were any blatant issues with them that I need to fix or if there are some improvements to make. I am running the arm off a PID as well, but if that turns our to be overkill then I may just use waitUntil(potentiometer = whatever);

//Drive PID Function

// Positional Tuners

double latKp = 2;

double rotKp = 2;

//Integral Tuners

double latKi = 0.005;

double rotKi = 0.005;

//Derivative Tuners

double latKd = 0.1;

double rotKd = 0.1;

//Motor Speed Variables

double latSpeed;

double rotSpeed;

//Potential Variables

double latError;

double rotError;

//Previous Potential Variables

double latPrevError = 0;

double rotPrevError = 0;

//Integral Variables

double latIntegral = 0;

double rotIntegral = 0;

//Derivative Variables

double latDerivative;

double rotDerivative;

//Manual Toggle

bool LateralPID_ON = false;

bool RotationalPID_ON = false;

//Reset Rotational Sensors

void resetSensors(){

RightSensor.setPosition(0,degrees);

LeftSensor.setPosition(0,degrees);

}

//Lateral PID

void LateralPID(int distance_cm){

resetSensors();

while (LateralPID_ON){

//Print Error Values

Brain.Screen.printAt(10,50,“Lateral Error %6.2f”, latError);

Brain.Screen.printAt(10,50,“Rotational Error %6.2f”, rotError);

```
//Get Left and Right Values in cm
double LeftValue = (LeftSensor.position(degrees)/360) * 8.255;
double RightValue = (RightSensor.position(degrees)/360) * 8.255;
//Get Error Values
latError = distance_cm - ((LeftValue - RightValue)/2);
rotError = LeftValue - RightValue;
//Get Integral Values with Boundaries
if ( (sqrt(latError * latError)) < 2 && latError != 0){
latIntegral = latIntegral + latError;
}
else{
latIntegral = 0;
}
if ((sqrt(rotError * rotError)) < 60 && rotError != 0){
rotIntegral = rotIntegral + rotError;
}
else{
rotIntegral = 0;
}
//Get Derivative Value
latDerivative = latError - latPrevError;
rotDerivative = rotError - rotPrevError;
//Update Previous Error
latPrevError = latError;
rotPrevError = rotError;
//Set Motor Speeds
latSpeed = (latError * latKp) + (latIntegral * latKi) + (latDerivative * latKd);
rotSpeed = (rotError * rotKp) + (rotIntegral * rotKi) + (rotDerivative * rotKd);
//Spin Motor
LeftBottom.spin(forward, (latSpeed - rotSpeed), voltageUnits::volt);
LeftTop.spin(forward, (latSpeed - rotSpeed), voltageUnits::volt);
RightBottom.spin(forward, (latSpeed + rotSpeed), voltageUnits::volt);
RightTop.spin(forward, (latSpeed + rotSpeed), voltageUnits::volt);
//Provide Exist Condition
if (latError < 1 && latError > -1){
LeftBottom.stop();
LeftTop.stop();
RightBottom.stop();
RightTop.stop();
break;
}
//wait 20 msec
vex::task::sleep(20);
```

}

}

void Rotational_PID (int bot_angle){

while (RotationalPID_ON){

//Print Rotational Error

Brain.Screen.printAt(10,50,“Rotational Error %6.2f”, rotError);

```
//Get Sensor Values
double LeftValue = LeftSensor.position(degrees);
double RightValue = RightSensor.position(degrees);
//Establish Rotational Error
rotError = bot_angle - (LeftValue - RightValue);
//Get Rotational Integral with Boundaries
if ((sqrt(rotError * rotError)) < 60 && rotError != 0){
rotIntegral = rotIntegral + rotError;
}
else{
rotIntegral = 0;
}
//Get Rotational Derivative
rotDerivative = rotError - rotPrevError;
//Update Rotational Previous Error
rotPrevError = rotError;
//Set Rotational Motor Speed
rotSpeed = (rotError * rotKp) + (rotIntegral * rotKi) + (rotDerivative * rotKd);
//Spin Motors
LeftBottom.spin(reverse, (rotSpeed), voltageUnits::volt);
LeftTop.spin(reverse, (rotSpeed), voltageUnits::volt);
RightBottom.spin(forward, (rotSpeed), voltageUnits::volt);
RightTop.spin(forward, (rotSpeed), voltageUnits::volt);
//Provide Exist Condition
if (rotError < 1 && rotError > -1){
LeftBottom.stop();
LeftTop.stop();
RightBottom.stop();
RightTop.stop();
break;
}
//Wait 20 msec
vex::task::sleep(20);
```

}

}

//Arm PID Function

//Positional Tuner

double ArmKp = 2;

//Integral Tuner

double ArmKi = 0.005;

//Derivative Tuner

double ArmKd = 0.1;

//Arm Speed

double ArmSpeed;

//Potential Variable

int ArmError;

//Previous Potential Variable

int PrevArmError = 0;

//Integral Variable

int ArmIntegral = 0;

//Derivative Variable

int ArmDerivative;

//Manual Toggle

bool ArmPID_ON = false;

//Arm PID

void ArmPID (int Arm_Angle){

while(ArmPID_ON){

//Print Arm Error

Brain.Screen.printAt(10,50,“Arm Error %6.2f”, ArmError);

```
//Get Sensor Value
int Arm_Position = TowerSensor.angle(degrees);
//Get Potential Value
ArmError = Arm_Angle - Arm_Position;
//Get Integral Value with Contraints
if (abs(ArmError) < 5 && ArmError != 0 ){
ArmIntegral = ArmIntegral + ArmError;
}
else{
ArmIntegral = 0;
}
//Get Derivative Value
ArmDerivative = ArmError - PrevArmError;
//Update Previous Error
PrevArmError = ArmError;
//Set Motor Speed
ArmSpeed = (ArmError * ArmKp) + (ArmIntegral * ArmKi) + (ArmDerivative * ArmKd);
//Spin Arm
Tower.spin(forward, ArmSpeed, voltageUnits::volt);
//Provide Exit Condition
if(ArmError < 1 && ArmError > -1){
Tower.stop();
break;
}
//Wait 20 msec
vex::task::sleep(20);
```

}

}

For reference, lat means lateral, rot means rotational.