Code Issue

VEX V5 General Discussion Technical Discussion
#1

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;
		
	}
	
}
#2

Have you thought about using one function and specifying the direction with a signed


long inches

(ie: 15 for forward and -15 for backward)?

The math will still work since the encoders count negative when they turn backward.

Pseudo code:

	void DriveTo(long inches)
	{
		int destination = inches * 28 //( 360 per revolution / 4 * pi )
		// Reset encoders to 0
		SensorValue[encoderLeft] = 0;

		while(abs(SensorValue[encoderLeft]) - destination) > 10)
		{
			speed = 	( SensorValue[encoderLeft]) - ticks ) * kp;
			motor[left] = speed;
		}
			motor[left] = 0;
	}
	
	DriveTo(24); //forward 24"
	wait1ms(1000);
	DriveTo(-24); //backward 24"

Add your PID formula within the while statement. You could also use


long left

and


long right

parameters to allow for turns.

#3