my program just randomly started to fail at everything. I havent changed anything since the last time it worked but for some reason everything isnt working. Its acting like the deadband doesnt exist and just keeps looping over and over. My program wont even compile when i try and download it via bluetooth. Somone please help me i have no idea why it just decided to stop working
void move(double leftSpeed, double rightSpeed) {
Motoright.setVelocity(rightSpeed, percent);
Motorleft.setVelocity(leftSpeed, percent);
Motoright.spin(forward);
Motorleft.spin(forward);
}
void gyro_turn(double targetDeg, double Kp, double Ki, double Kd, double minPower, double maxPower, bool resetGyro, bool useHeading, bool stopMotor) {
if (resetGyro){
BrainInertial.setHeading(0, degrees);
}
if (useHeading){
currentPosition = BrainInertial.heading(degrees);
} else {
currentPosition = BrainInertial.rotation(degrees);
}
double deadband = 1;
double turnTotalError = 0;
double turnPrevError = 0;
while (true) {
double error = targetDeg - currentPosition;
// degrees of freedom
if (fabs(error) <= deadband)
{Brain.Screen.print("deadband");
Brain.Screen.print("%lf", BrainInertial.heading(degrees));
break;}
// pid
double output = error \* Kp;
output += (error - turnPrevError) \* Kd;
turnTotalError += error;
output += turnTotalError \* Ki;
// min and max
if (output > maxPower) output = maxPower;
if (output < -maxPower) output = -maxPower;
// if neg keep neg
if (fabs(output) < minPower){
output = (output >= 0)? minPower : -minPower;
}
turnPrevError = error;
move(output, -output);
printf("\\n rotation: %.3lf speed: %.3lf integral: %.3lf", BrainInertial.rotation(degrees), output, turnTotalError);
wait(10, msec);
}
if(stopMotor){
printf("\\n rotation: %.3lf", BrainInertial.heading(degrees));
printf("jfisjfsk");
Motorleft.stop();
Motoright.stop();
}
}