The turn function doesn’t work at all. after looking at it for a long time i can’t find any bugs. Another issue with the pid loop is the drive function doesn’t correct overshooting. when it crosses the encoder target, it just stops and drifts instead of oscillating(we haven’t tuned it yet). What should i use instead of a while(abs(error)<3) for the loop?
const double kp = 0.4;
const double ki = 0.0;
const double kd = 0;
//turn costants
const double tp = 0.5;
const double ti = 0.0005
const double td = 0;
//double proportion;
double totalError;
double rtotalError;
double integral;
double rintegral;
double integralActiveZone = 200;//adjust later
double integralPowerLimit = 50;//adjust later
double derivative;
double rderivative;
double leftPower;
double rightPower;
double terror; //turn error >:D
double gyroValue;
double gyroerror;
double turn_TotalError;
double tderivative;
int side;
task gyroTask() //stolen from backwards crew :/
{
long rate;
long angle, lastAngle;
lastAngle=0.0;
gyroError=0.0;
// Change sensitivity, this allows the rate reading to be higher
setGyroSensitivity(gyro, gyroNormalSensitivity);
//Reset the gyro sensor to remove any previous data.
resetGyro(gyro);
wait1Msec(1000);
while (true) {
rate = getGyroRate(gyro);
angle = getGyroDegrees(gyro);
// If big rate then ignore gyro changes
if( abs( rate ) < 2 )
{
if( angle != lastAngle )
gyroError += lastAngle - angle;
}
lastAngle = angle;
gyroValue = angle + gyroError;
wait1Msec(10);
}
}
void Drive(double target) {
double error = target;
double lastError = target;
double rError = target;
double rLastError = target;
wait1Msec(20);
while(abs(error) > 3){
error = target - getMotorEncoder(left);
rError = target - getMotorEncoder(right);
if(error == 0){
break;
}
//left integral
if(abs(error) < integralActiveZone && error != 0){
totalError += error;
}
else totalError = 0.0;
if(totalError > integralPowerLimit){
totalError = integralPowerLimit;
}
if(totalError < -integralPowerLimit){
totalError = -integralPowerLimit;
}
integral = ki * totalError;
///////
//right integral
if(abs(rError) < integralActiveZone && rError != 0){
rtotalError += rerror;
}
else rtotalError = 0.0;
if(rtotalError > integralPowerLimit){
rtotalError = integralPowerLimit;
}
if(rtotalError < -integralPowerLimit){
rtotalError = -integralPowerLimit;
}
rintegral = ki * rtotalError;
///////
//derivative
derivative = kd * (error - lastError);
lastError = error;
rderivative = kd * (rerror - rlastError);
rlastError = rerror;
if(error == 0){
derivative = 0;
rderivative = 0;
}
//terror = turn error lol
terror = gyroValue*tp
LeftPower = 0.5 * (kp * error + integral + derivative);
RightPower = 0.5 * (kp * rError + rintegral + rderivative);
setMotorSpeed(Left,LeftPower+terror);
setMotorSpeed(Right,RightPower-terror);
wait1Msec(15);
}
StopMultipleMotors(Left,Right);
}
void PID_turn(heading,speed,diff){ //currently a PI loop. Might add D later
terror = heading - gyroValue;
tlasterror = heading - gyrovalue;
if (heading > gyroValue){
side = -1;
}
else{
side = 1;
}
while(abs(terror)<1.5){
terror = heading - gyroValue;
if(abs(error) < diff/3 && error != 0){
turn_totalError += terror;
}
tderivative = td * (terror - tlastError);
tlastError = error;
if(error == 0){
tderivative = 0;
}
LeftPower = tp * terror + ti * turn_TotalError + tderivative;
rightPower = tp * terror + ti * turn_TotalError + tderivative;
setMotorSpeed(left,side*LeftPower);
setMotorSpeed(right,-1*side*rightPower);
wait1Msec(15);
}
/*
if (heading > 0){
while(terror<2){
terror = (heading - gyroValue)*tp;
if(abs(error) < diff/3 && error != 0){
turn_totalError += terror;
}
setMotorSpeed(left,-speed-(terror+ti*turn_totalerror));
setMotorSpeed(right,speed+terror+ti*turn_totalerror);
}
}
else if (heading < 0){
while(terror>-2){
terror = (heading - gyroValue)*tp;
if(abs(error) < diff/3 && error != 0){
turn_totalError += terror;
}
setMotorSpeed(left,speed-(terror+ti*turn_totalerror));
setMotorSpeed(right,-speed+terror+ti*turn_totalerror);
wait1Msec(15);
}
}
*/
stopMotor(left);
stopMotor(right);
}