Hi everyone,
We are working on an improved autonomous and ran into an odd issue. We were originally using integrated motor encoders on the drive base for driving forwards and backwards. I wanted to switch it to use the quad encoders we recently attached to the robot, but for some reason, only the drive forwards works now. The drive backwards does not ever stop. I have done some debugging and I found out that it never exits the while loop, but when I look at the conditions in the loop, they should become false when the target reaches the target value. The weird part is that the exact same function worked with the IME’s, but when I switched the encoders to quad encoders, only forwards works. Any help is greatly appreciated. Also, if you have any questions about why I have certain things in certain places, I can clarify further.
void drive(char dir,int inches) {
int ticks = (int)((inches / WHEEL_RADIUS ) * ( 180 / PI ));
previousTickTarget = ticks;
SensorValue[rightWheelEnc] = SensorValue[leftWheelEnc] = 0;
if(dir == 'f') {
nMotorEncoder[RightFront] = nMotorEncoder[LeftFront] = 0; //clear encoders
float kp = .7;
float errorR = (SensorValue[rightWheelEnc] * -1) - ticks;
float errorL = SensorValue[leftWheelEnc] - ticks;
int speedR = kp * errorR;
int speedL = kp * errorL;
int tolerance = 25;
while(abs(errorL) > tolerance|| abs(errorR) > tolerance || getMotorVelocity(RightFront) > 10 || getMotorVelocity(LeftFront) > 10) {
if(speedR < 38 && speedR > 0) {
speedR = 38;
}
if(speedL < 38 && speedL > 0) {
speedL = 38;
}
if(speedR < 0 && speedR > -38) {
speedR = -38;
}
if(speedL < 0 && speedL > -38) {
speedL = -38;
}
motor[RightFront] = motor[RightBack] = -1 * speedR;
motor[LeftFront] = motor[LeftBack] = -1 * speedL;
errorR = (SensorValue[rightWheelEnc] * -1) - ticks;
errorL = SensorValue[leftWheelEnc] - ticks;
speedR = kp * errorR;
speedL = kp * errorL;
holdArmPosInAuton(TowerPot);
holdClawShut();
}
motor[RightFront] = motor[RightBack] = 0;
motor[LeftFront] = motor[LeftBack] = 0;
motor[LeftCatapult1] = motor[LeftCatapult2] = 0;
motor[RightCatapult1] = motor[RightCatapult2] = 0;
}
if(dir == 'b') {
nMotorEncoder[RightFront] = nMotorEncoder[LeftFront] = 0; //clear encoders
float kp = .7;
float errorR = (SensorValue[rightWheelEnc] * -1) - ticks;
float errorL = SensorValue[leftWheelEnc] - ticks;
int speedR = kp * errorR;
int speedL = kp * errorL;
int tolerance = 25;
while(abs(errorL) > tolerance|| abs(errorR) > tolerance getMotorVelocity(RightFront) > 10 || getMotorVelocity(LeftFront) > 10 ) {
writeDebugStreamLine("Start while loop");
if(speedR < 10 && speedR > 0) {
speedR = 10;
}
if(speedL < 10 && speedL > 0) {
speedL = 10;
}
if(speedR < 0 && speedR > -10) {
speedR = -10;
}
if(speedL < 0 && speedL > -10) {
speedL = -10;
}
motor[RightFront] = motor[RightBack] = speedR;
motor[LeftFront] = motor[LeftBack] = speedL;
errorR = (SensorValue[rightWheelEnc] * -1) - ticks; // the encoder is mounted backwards, so it needs to use the opposite value
errorL = SensorValue[leftWheelEnc] - ticks;
speedR = kp * errorR;
speedL = kp * errorL;
holdArmPosInAuton(TowerPot);
holdClawShut();
writeDebugStreamLine("End while loop");
}
writeDebugStreamLine("Out of while loop");
motor[RightFront] = motor[RightBack] = 0;
motor[LeftFront] = motor[LeftBack] = 0;
motor[LeftCatapult1] = motor[LeftCatapult2] = 0;
motor[RightCatapult1] = motor[RightCatapult2] = 0;
}
}