I have an update. for the sake of keeping this as short as possible, Im only going to post what ive done for my turn left pid.
first, in pre auton, I reset the inertial
I’m using two inertial sensors at opposite mounting angles so that if I want to turn left 90 degrees, I say turn until left inertial reads 90, and vice versa for right.
This just makes it so that I dont have to add 180 degrees to every turn I make. (ex, to turn 90 deg left id have to tell my motors to spin until the inertial reads 270.)
void pre_auton(void){
vexcodeInit();
//calibrating left inertial
LeftInertial.calibrate();
LeftInertial.setRotation(0, degrees);
while(LeftInertial.isCalibrating()){
wait(10, msec);
}
//calibrating right inertial
RightInertial.calibrate();
RightInertial.setRotation(0, degrees);
while(RightInertial.isCalibrating()){
wait(10, msec);
}
}
next, I have my turn left pid loop
//Turn left pid
//--------------------------------------------------------------------------------------------------------------------------------------------//
//turn left constants
double turnLeftkP = 0.1;
double turnLeftkI = 0.0;
double turnLeftkD = 0.0;
//Desired value
int desiredTurnLeftValue = 0;
//defining errors
int turnLeftError; //SensorValue - DesiredValue : Positional value
int turnLeftPrevError = 0; //Position 20ms ago
int turnLeftDerivative; // error - prevError : speed
int turnLeftTotalError = 0; //totalError = totalError + error
//variables for settings
bool enableTurnLeftPID = true;
bool resetTurnLeftSensors = false;
//Drive pid
int turnLeftPID(){
if (resetTurnLeftSensors) {
resetTurnLeftSensors = false;
LeftTrack.resetPosition();
RightTrack.resetPosition();
RearTrack.resetPosition();
LeftInertial.setRotation(0, degrees);
}
while(resetTurnLeftSensors){
//Averaging position
int TLCurrentPosition = LeftInertial.heading(degrees);
//Potential
turnLeftError = desiredTurnLeftValue - TLCurrentPosition;
//Integral
turnLeftTotalError += turnLeftError;
//Derivative
turnLeftDerivative = turnLeftError - turnLeftPrevError;
//Lateral PID equation
double turnLeftSpeed = (turnLeftError * turnLeftkP + turnLeftTotalError * turnLeftkI + turnLeftDerivative * turnLeftkD);
FrontLeftDrive.spin(reverse, turnLeftSpeed, voltageUnits::volt);
FrontRightDrive.spin(forward, turnLeftSpeed, voltageUnits::volt);
BackLeftDrive.spin(reverse, turnLeftSpeed, voltageUnits::volt);
BackRightDrive.spin(forward, turnLeftSpeed, voltageUnits::volt);
turnLeftPrevError = turnLeftError;
vex::task::sleep(20);
}
return 1;
}
next, I created a function that basically gives the pid task parameters to run on, and it makes it easier to code in auton.
in this section, I use both of my inertial sensors for more accurate turning. the perameters tell the motors to spin until the left inertial is between 90 and 92 degrees, and the right inertial is between 270 and 272 degrees.
void LTurnPID(int degrees){
resetTurnLeftSensors = true;
desiredTurnLeftValue = degrees;
waitUntil(LInertialAngle > degrees && LInertialAngle < (degrees + 2) && RInertialAngle > (degrees + 180) && RInertialAngle < (degrees + 182));
}
finally, I have the movement testing
vex::task tltest(turnLeftPID);
LTurnPID(90);
vex::task::stop(tltest);
wait(0.5, seconds);