So I made this PID loop, and I’m fairly confident that it will work on its own. However, I have incorporated what I believe to be a ‘straight line assist’ feature, and I am not sure if it will work. I do not have the robot right now, so I can’t really test it out. This is my first time making a PID loop from scratch, and I would really appreciate it if someone could point out things I did wrong, redundancies that could be taken out, and ways to make it more accurate. Any help or constructive criticism would be appreciated. Thanks in advance!

```
//pid loop starts
void drive(float target, float speed, float timeout){
Brain.Timer.reset();
FL.setPosition(0,degrees);
FR.setPosition(0,degrees);
BL.setPosition(0,degrees);
BR.setPosition(0,degrees);
Inertial.calibrate();
float kp = 0.06;
float ki = 0.01;
float kd = 0.9;
float error;
float prevError;
float totalError = 0;
float proportion;
float integral;
float derivative;
float rawPow;
float powerLeft;
float powerRight;
float turnl;
float turnr;
while(1){
//setting the error values
error = target;
prevError = error;
totalError += error;
//proportion value
proportion = kp*error;
//integral value
integral = totalError * ki;
//derivative value
derivative = (error - prevError) * kd;
//rawPow is the motor power without the straight line assist
rawPow = proportion + integral + derivative;
//this will make turnl equal to the inertial heading
if(Inertial.heading(degrees)>0){
turnl = Inertial.heading(degrees);
}
//this will make turnl equal to 0 if inertial heading is below zero
else{
turnl = 0;
}
//ditto
if(Inertial.heading(degrees)<0){
turnr = Inertial.heading(degrees);
}
//ditto
else{
turnr = 0;
}
//this is the pid power value plus the inertial heading, which corrects itself if it goes off course
powerLeft = rawPow + turnl;
//ditto
powerRight = rawPow + turnr;
//this is the speed cap
if(powerLeft>speed){
powerLeft = speed;
}
//ditto
if(powerRight>speed){
powerRight = speed;
}
//this is the timeout
if(Brain.Timer.value()>=timeout){
break;
}
if(error<4){
break;
}
//spinning the motors at the right voltages
if(rawPow>0){
FL.spin(fwd, powerLeft, vex::voltageUnits::volt);
FR.spin(fwd, powerRight, vex::voltageUnits::volt);
BL.spin(fwd, powerLeft, vex::voltageUnits::volt);
BR.spin(fwd, powerRight, vex::voltageUnits::volt);
}
if(rawPow<0){
FL.spin(reverse, powerLeft, vex::voltageUnits::volt);
FR.spin(reverse, powerRight, vex::voltageUnits::volt);
BL.spin(reverse, powerLeft, vex::voltageUnits::volt);
BR.spin(reverse, powerRight, vex::voltageUnits::volt);
}
wait(20,msec);
}
wait(20,msec);
}
void autonomous(void) {
drive(1500,50,1000);
}
```