Hello! We tried adding PID to our autonomous but it does not stop the robot. The motor returns a proper encoder value, but the error is always only calculated once and never again.
Here is my code.
Auton Function:
int moveBackward(int rotation, int velocity) {
target=rotation;
sensor=0;
vex::task PID(calculation);
while(error>5)
{
sensor=RFDrive.rotation(rotationUnits::deg);
LFDrive.spin(vex::directionType::fwd,speed, vex::velocityUnits::pct);
RFDrive.spin(vex::directionType::fwd,speed, vex::velocityUnits::pct);
LBDrive.spin(vex::directionType::fwd,speed, vex::velocityUnits::pct);
RBDrive.spin(vex::directionType::fwd,speed, vex::velocityUnits::pct);
}
LFDrive.stop(brakeType::coast);
RFDrive.stop(brakeType::coast);
LBDrive.stop(brakeType::coast);
RBDrive.stop(brakeType::coast);
vex::task::stop(PID);
return 0;
}
PID:
#include "cmath"
#include"vex.h"
//the target value
int targetValue;
int targetSpeed;
//the value that is received by the sensor
double sensorValue;
//the integral
float integral;
//the big three constants
double kp = 0.5;
double ki = 0.2;
double kd = 0.1;
//sets the speed
float speed;
float error;
float previousError;
float derivative1;
int calculus()
{
while (targetValue - sensorValue >5)
{
//error calculation
error = targetValue - sensorValue;
//integral calculation
integral += error;
//safety check
if (error == 0 || error > targetValue)
{
integral = 0;
}
//safety check
if (error > 1000)
{
integral = 0;
}
//derivative calculation
derivative1 = error - previousError;
//previous error calculation
previousError = error;
//final speed calculation
speed = (kp * error) + (ki * integral) + (kd * derivative1);
if(speed>targetSpeed)
{
speed=targetSpeed;
}
vex::task::sleep(15);
}
return 0;
}