1900X/Dumper v2 Auton.c
void driveDistance(int power, int encoderCounts, int direction) {
	//writeDebugStreamLine("nPgmTime,error,nMotorEncoder[lDriveFront], nMotorEncoder[rDriveFront],pTerm,iTerm,dTerm,lPower,rPower");
	//reset encoder values
	SensorValue[lDriveEnc] = 0;
	SensorValue[rDriveEnc] = 0;

	int straighteningError = 0;

	float lPower,
	rPower;

	time1[T1] = 0;
	if (direction == STRAIGHT) { //validate direction
			//limit the values of the power term to only be those that can be taken by the motors
			if (power > 127) {
				power = 127;
			} else if (power < -127) {
				power = -127;
			}


				while(encoderCounts > abs(SensorValue[lDriveEnc] + SensorValue[rDriveEnc])/2.0) {
					//adjust the powers sent to each side if the encoder values don't match
						straighteningError = SensorValue[lDriveEnc] - SensorValue[rDriveEnc];


						if (straighteningError > 0) { //left side is ahead, so slow it down
							lPower = power - straighteningError*straighteningKpLeft;
						} else { //otherwise, just set the right side to the power
							lPower = power;
						}
						if (straighteningError < 0) { //right side is ahead, so slow it down
							rPower = power + straighteningError*straighteningKpRight;
						} else { //otherwise, just set the right side to the power
							rPower = power;
						}
						writeDebugStreamLine("lPower = %d, rPower = %d",lPower,rPower);

					setLeftDtMotors(lPower);
					setRightDtMotors(rPower);
					wait1Msec(25);
			}

			//writeDebugStreamLine("%d,%f,%f,%f,%f,%f,%f,%f,%f",nPgmTime,target,error,nMotorEncoder[lDriveFront], nMotorEncoder[rDriveFront],pTerm,iTerm,dTerm,lPower,rPower);
			setLeftDtMotors(0);
			setRightDtMotors(0);

	} else if (direction == ROTATE_LEFT || direction == ROTATE_RIGHT) {
			//update error terms

			//limit the values of the power term to only be those that can be taken by the motors
			if (power > 127) {
				power = 127;
			} else if (power < -127) {
				power = -127;
			}


			//apply a slew rate to limit acceleration/deceleration


			//adjust the powers sent to each side if the encoder values don't match
			straighteningError = abs(nMotorEncoder[lDriveFront]) - abs(nMotorEncoder[rDriveFront]);

			if (straighteningError > 0) { //left side is ahead, so speed up the right side
				rPower = power + straighteningError*straighteningKpLeftTurn;
			} else { //otherwise, just set the right side to the power
				rPower = power;
			}
			if (straighteningError < 0) { //right side is ahead, so speed up the left side
				lPower = power - straighteningError*straighteningKpRightTurn;
			} else { //otherwise, just set the right side to the power
				lPower = power;
			}

			//lastPower = power; //update the last power
			//writeDebugStreamLine("%d,%f,%f,%f,%f,%f,%f,%f,%f",nPgmTime,error,nMotorEncoder[lDriveFront], nMotorEncoder[rDriveFront],pTerm,iTerm,dTerm,lPower,rPower);
			setLeftDtMotors(lPower * direction); //for a left turn, ROTATE_LEFT = -1 so this moves the left side backwards for a left turn. For a right turn will go forward since ROTATE_RIGHT = 1
			setRightDtMotors(rPower * -1 * direction); //same idea as for a left turn, except this side needs to go the opposite way as the left side in order to turn, hence the * -1 in the calculation
			wait1Msec(25);
		}
	}
//positive powers will go forward or right
//negative powers values go backwards or left
//encoder counts is how many counts to go, always positive
//power is the power to run the motors at before straightening is applied
//time is a maximum time to complete the operation
positive powers will go forward or right
negative powers values go backwards or left
encoder counts is how many counts to go, always positive
power is the power to run the motors at before straightening is applied
time is a maximum time to complete the operation
positive powers will go forward or right negative powers values go backwards or left encoder counts is how many counts to go always positive power is the power to run the motors at before straightening is applied time is a maximum time to complete the operation
void driveDistance(int power, int encoderCounts, int direction) {
	SensorValue[lDriveEnc] = 0;
	SensorValue[rDriveEnc] = 0;
	int straighteningError = 0;
	float lPower,
	rPower;
	time1[T1] = 0;
	if (direction == STRAIGHT) { 
			if (power > 127) {
				power = 127;
			} else if (power < -127) {
				power = -127;
			}
				while(encoderCounts > abs(SensorValue[lDriveEnc] + SensorValue[rDriveEnc])/2.0) {
						straighteningError = SensorValue[lDriveEnc] - SensorValue[rDriveEnc];
						if (straighteningError > 0) { 
							lPower = power - straighteningError*straighteningKpLeft;
						} else { 
							lPower = power;
						}
						if (straighteningError < 0) { 
							rPower = power + straighteningError*straighteningKpRight;
						} else { 
							rPower = power;
						}
						writeDebugStreamLine("lPower = %d, rPower = %d",lPower,rPower);
					setLeftDtMotors(lPower);
					setRightDtMotors(rPower);
					wait1Msec(25);
			}
			setLeftDtMotors(0);
			setRightDtMotors(0);
	} else if (direction == ROTATE_LEFT || direction == ROTATE_RIGHT) {
			if (power > 127) {
				power = 127;
			} else if (power < -127) {
				power = -127;
			}
			straighteningError = abs(nMotorEncoder[lDriveFront]) - abs(nMotorEncoder[rDriveFront]);
			if (straighteningError > 0) { 
				rPower = power + straighteningError*straighteningKpLeftTurn;
			} else { 
				rPower = power;
			}
			if (straighteningError < 0) { 
				lPower = power - straighteningError*straighteningKpRightTurn;
			} else { 
				lPower = power;
			}
			setLeftDtMotors(lPower * direction); 
			setRightDtMotors(rPower * -1 * direction); 
			wait1Msec(25);
		}
	}
void driveDistance ( int power , int encoderCounts , int direction ) { SensorValue [ lDriveEnc ] = 0 ; SensorValue [ rDriveEnc ] = 0 ; int straighteningError = 0 ; float lPower , rPower ; time1 [ T1 ] = 0 ; if ( direction == STRAIGHT ) { if ( power > 127 ) { power = 127 ; } else if ( power < -127 ) { power = -127 ; } while ( encoderCounts > abs ( SensorValue [ lDriveEnc ] + SensorValue [ rDriveEnc ] ) / 2.0 ) { straighteningError = SensorValue [ lDriveEnc ] - SensorValue [ rDriveEnc ] ; if ( straighteningError > 0 ) { lPower = power - straighteningError * straighteningKpLeft ; } else { lPower = power ; } if ( straighteningError < 0 ) { rPower = power + straighteningError * straighteningKpRight ; } else { rPower = power ; } writeDebugStreamLine ( " " , lPower , rPower ) ; setLeftDtMotors ( lPower ) ; setRightDtMotors ( rPower ) ; wait1Msec ( 25 ) ; } setLeftDtMotors ( 0 ) ; setRightDtMotors ( 0 ) ; } else if ( direction == ROTATE_LEFT || direction == ROTATE_RIGHT ) { if ( power > 127 ) { power = 127 ; } else if ( power < -127 ) { power = -127 ; } straighteningError = abs ( nMotorEncoder [ lDriveFront ] ) - abs ( nMotorEncoder [ rDriveFront ] ) ; if ( straighteningError > 0 ) { rPower = power + straighteningError * straighteningKpLeftTurn ; } else { rPower = power ; } if ( straighteningError < 0 ) { lPower = power - straighteningError * straighteningKpRightTurn ; } else { lPower = power ; } setLeftDtMotors ( lPower * direction ) ; setRightDtMotors ( rPower * -1 * direction ) ; wait1Msec ( 25 ) ; } }
positive powers will go forward or right
negative powers values go backwards or left
encoder counts is how many counts to go, always positive
power is the power to run the motors at before straightening is applied
time is a maximum time to complete the operation
positive powers will go forward or right negative powers values go backwards or left encoder counts is how many counts to go always positive power is the power to run the motors at before straightening is applied time is a maximum time to complete the operation
//writeDebugStreamLine("nPgmTime,error,nMotorEncoder[lDriveFront], nMotorEncoder[rDriveFront],pTerm,iTerm,dTerm,lPower,rPower"); //reset encoder values //validate direction //limit the values of the power term to only be those that can be taken by the motors //adjust the powers sent to each side if the encoder values don't match //left side is ahead, so slow it down //otherwise, just set the right side to the power //right side is ahead, so slow it down //otherwise, just set the right side to the power //writeDebugStreamLine("%d,%f,%f,%f,%f,%f,%f,%f,%f",nPgmTime,target,error,nMotorEncoder[lDriveFront], nMotorEncoder[rDriveFront],pTerm,iTerm,dTerm,lPower,rPower); //update error terms //limit the values of the power term to only be those that can be taken by the motors //apply a slew rate to limit acceleration/deceleration //adjust the powers sent to each side if the encoder values don't match //left side is ahead, so speed up the right side //otherwise, just set the right side to the power //right side is ahead, so speed up the left side //otherwise, just set the right side to the power //lastPower = power; //update the last power //writeDebugStreamLine("%d,%f,%f,%f,%f,%f,%f,%f,%f",nPgmTime,error,nMotorEncoder[lDriveFront], nMotorEncoder[rDriveFront],pTerm,iTerm,dTerm,lPower,rPower); //for a left turn, ROTATE_LEFT = -1 so this moves the left side backwards for a left turn. For a right turn will go forward since ROTATE_RIGHT = 1 //same idea as for a left turn, except this side needs to go the opposite way as the left side in order to turn, hence the * -1 in the calculation