We’ve been running this code for a bit now, and inconsistently it will skip random lines.
The spot in which it skips is not identical each run, and we’ve run certain autons 50-100 times and have seen it skip in every turn. Previously, the PID Drive was skipping as well but this was fixed by changing the initial speed of the PID Drive. This quick fix was not successful with the PID Turn.
If anybody has any recommendations or has experienced this please let us know of any fixes.
Thank you!
Code attached below:
void pidTurn(double angle, int maxpower=6) {
//ENDPOINT defines the final value which the gyro should
//report after it turns through the desired angle
const double ENDPOINTTURN = angle;
double currentValueTurn = inertial1.rotation(degrees);
double currentErrorTurn = ENDPOINTTURN - currentValueTurn;
double previousErrorTurn = 0.00;
double totalErrorTurn = 0.00;
const double INTEGRAL_LIMIT = 100.0;
if(currentErrorTurn > 0) {
LF.spin(directionType::fwd, 3, percentUnits::pct);
RF.spin(directionType::rev, 3, percentUnits::pct);
RB.spin(directionType::rev, 3, percentUnits::pct);
LB.spin(directionType::fwd, 3, percentUnits::pct);
} else {
LF.spin(directionType::fwd, 3, percentUnits::pct);
RF.spin(directionType::rev, 3, percentUnits::pct);
RB.spin(directionType::rev, 3, percentUnits::pct);
LB.spin(directionType::fwd, 3, percentUnits::pct);
}
task::sleep(200);
//While loop ensures that the robot will keep
//turning until it reaches the endpoint
while(fabs(currentErrorTurn) > 5) {
if(fabs(currentErrorTurn) < INTEGRAL_LIMIT)
totalErrorTurn += currentErrorTurn;
else
totalErrorTurn = 0;
if(previousErrorTurn * currentErrorTurn <= 0) totalErrorTurn = 0;
double kP = .135; //0.1350
double kI = 0.0; //0.03
double kD = 0.0; //0.74
double p = kP * currentErrorTurn;
double i = kI * totalErrorTurn;
double d = kD * (currentErrorTurn - previousErrorTurn);
double power = (p+i+d);
int finalpower;
if(power > 0)
{
if(power>maxpower){
finalpower=maxpower;
}else{
finalpower = power;
}
}
else
{
if((fabs(power))>maxpower){
finalpower=-maxpower;
}else{
finalpower = power;
}
}
LF.spin(directionType::fwd, (finalpower), voltageUnits::volt);
RF.spin(directionType::rev, (finalpower), voltageUnits::volt);
LB.spin(directionType::fwd, (finalpower), voltageUnits::volt);
RB.spin(directionType::rev, (finalpower), voltageUnits::volt);
currentValueTurn = inertial1.rotation(degrees);
previousErrorTurn = currentErrorTurn;
currentErrorTurn = ENDPOINTTURN - currentValueTurn;
task::sleep(20);
if((fabs(LB.velocity(velocityUnits::pct))) * (fabs(RB.velocity(velocityUnits::pct))) < 1) break;
}
drivestop();
task::sleep(50);
drivestopcoast();
}