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

	int straighteningError = 0;

	float lPower,
	rPower;

	time1[T1] = 0;
	if (direction == STRAIGHT || direction == STRAFE_LEFT || direction == STRAFE_RIGHT) { //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;
			}

			if (direction == STRAIGHT) {
				while(encoderCounts > abs(nMotorEncoder[lDriveFront] + nMotorEncoder[rDriveFront])/2.0) {
					//adjust the powers sent to each side if the encoder values don't match
						straighteningError = nMotorEncoder[lDriveFront] - nMotorEncoder[rDriveFront];


						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,lPower);
					setRightDtMotors(rPower,rPower);
					wait1Msec(25);
				}

				setLeftDtMotors(0,0);
				setRightDtMotors(0,0);

			} else if (direction == STRAFE_LEFT) {

					while(encoderCounts > (abs(nMotorEncoder[lDriveFront]) + abs(nMotorEncoder[rDriveFront]))/2.0) {
							//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 slow it down
							lPower = power - straighteningError*straighteningKpLeft*-1;
						} 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*-1;
						} else { //otherwise, just set the right side to the power
							rPower = power;
						}

					setLeftDtMotors(lPower*-1,lPower*1);
					setRightDtMotors(rPower*1,rPower*-1);
					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,0);
			setRightDtMotors(0,0);

	} else if (direction == STRAFE_RIGHT) {

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

						//writeDebugStreamLine("%d,%d,%d,%d",lPower*lfMult,lPower*lbMult,rPower*rfMult,rPower*rbMult);
						//positive powers:
						//  if left side is ahead, straightening error is positive
						// 	lPower will decrease
						//  if right side is ahead, straightening error is negative
						//  rPower will decrease
						//negative powers:
						//  if left side is ahead, straightening error is positive
						//  power is negative, straighteningError is positive
						//  power would go higher (more negative) because power - (positive error)
						//  fix: multiply by sign of power - after this change
						//  power is positive, straighteningError is still positive (as we want)
						//  power is negative, straighteningError is multiplied by -1, so:
						//  (-127) - (12)*(.3)*-1 = -127 + 4 = -123, slower power
						//
						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;
						}

					setLeftDtMotors(lPower*1,lPower*-1);
					setRightDtMotors(rPower*-1,rPower*1);
					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,0);
			setRightDtMotors(0,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, 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,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) {
	nMotorEncoder[rDriveFront] = 0;
	nMotorEncoder[lDriveFront] = 0;
	int straighteningError = 0;
	float lPower,
	rPower;
	time1[T1] = 0;
	if (direction == STRAIGHT || direction == STRAFE_LEFT || direction == STRAFE_RIGHT) { 
			if (power > 127) {
				power = 127;
			} else if (power < -127) {
				power = -127;
			}
			if (direction == STRAIGHT) {
				while(encoderCounts > abs(nMotorEncoder[lDriveFront] + nMotorEncoder[rDriveFront])/2.0) {
						straighteningError = nMotorEncoder[lDriveFront] - nMotorEncoder[rDriveFront];
						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,lPower);
					setRightDtMotors(rPower,rPower);
					wait1Msec(25);
				}
				setLeftDtMotors(0,0);
				setRightDtMotors(0,0);
			} else if (direction == STRAFE_LEFT) {
					while(encoderCounts > (abs(nMotorEncoder[lDriveFront]) + abs(nMotorEncoder[rDriveFront]))/2.0) {
						straighteningError = abs(nMotorEncoder[lDriveFront]) - abs(nMotorEncoder[rDriveFront]);
						if (straighteningError > 0) { 
							lPower = power - straighteningError*straighteningKpLeft*-1;
						} else { 
							lPower = power;
						}
						if (straighteningError < 0) { 
							rPower = power + straighteningError*straighteningKpRight*-1;
						} else { 
							rPower = power;
						}
					setLeftDtMotors(lPower*-1,lPower*1);
					setRightDtMotors(rPower*1,rPower*-1);
					wait1Msec(25);
				}
			setLeftDtMotors(0,0);
			setRightDtMotors(0,0);
	} else if (direction == STRAFE_RIGHT) {
					while(encoderCounts > (abs(nMotorEncoder[lDriveFront]) + abs(nMotorEncoder[rDriveFront]))/2.0) {
						straighteningError = abs(nMotorEncoder[lDriveFront]) - abs(nMotorEncoder[rDriveFront]);
						if (straighteningError > 0) { 
							lPower = power - straighteningError*straighteningKpLeft;
						} else { 
							lPower = power;
						}
						if (straighteningError < 0) { 
							rPower = power + straighteningError*straighteningKpRight;
						} else { 
							rPower = power;
						}
					setLeftDtMotors(lPower*1,lPower*-1);
					setRightDtMotors(rPower*-1,rPower*1);
					wait1Msec(25);
				}
			setLeftDtMotors(0,0);
			setRightDtMotors(0,0);
	} else if (direction == ROTATE_LEFT || direction == ROTATE_RIGHT) {
			update error terms
			if (power > 127) {
				power = 127;
			} else if (power < -127) {
				power = -127;
			}
			apply a slew rate to limit acceleration/deceleration
			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 { 
				rPower = power;
			}
			if (straighteningError < 0) { right side is ahead, so speed up the left side
				lPower = power - straighteningError*straighteningKpRightTurn;
			} else { 
				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, 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,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);
		}
	}
}
void driveDistance ( int power , int encoderCounts , int direction ) { nMotorEncoder [ rDriveFront ] = 0 ; nMotorEncoder [ lDriveFront ] = 0 ; int straighteningError = 0 ; float lPower , rPower ; time1 [ T1 ] = 0 ; if ( direction == STRAIGHT || direction == STRAFE_LEFT || direction == STRAFE_RIGHT ) { if ( power > 127 ) { power = 127 ; } else if ( power < -127 ) { power = -127 ; } if ( direction == STRAIGHT ) { while ( encoderCounts > abs ( nMotorEncoder [ lDriveFront ] + nMotorEncoder [ rDriveFront ] ) / 2.0 ) { straighteningError = nMotorEncoder [ lDriveFront ] - nMotorEncoder [ rDriveFront ] ; 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 , lPower ) ; setRightDtMotors ( rPower , rPower ) ; wait1Msec ( 25 ) ; } setLeftDtMotors ( 0 , 0 ) ; setRightDtMotors ( 0 , 0 ) ; } else if ( direction == STRAFE_LEFT ) { while ( encoderCounts > ( abs ( nMotorEncoder [ lDriveFront ] ) + abs ( nMotorEncoder [ rDriveFront ] ) ) / 2.0 ) { straighteningError = abs ( nMotorEncoder [ lDriveFront ] ) - abs ( nMotorEncoder [ rDriveFront ] ) ; if ( straighteningError > 0 ) { lPower = power - straighteningError * straighteningKpLeft * -1 ; } else { lPower = power ; } if ( straighteningError < 0 ) { rPower = power + straighteningError * straighteningKpRight * -1 ; } else { rPower = power ; } setLeftDtMotors ( lPower * -1 , lPower * 1 ) ; setRightDtMotors ( rPower * 1 , rPower * -1 ) ; wait1Msec ( 25 ) ; } setLeftDtMotors ( 0 , 0 ) ; setRightDtMotors ( 0 , 0 ) ; } else if ( direction == STRAFE_RIGHT ) { while ( encoderCounts > ( abs ( nMotorEncoder [ lDriveFront ] ) + abs ( nMotorEncoder [ rDriveFront ] ) ) / 2.0 ) { straighteningError = abs ( nMotorEncoder [ lDriveFront ] ) - abs ( nMotorEncoder [ rDriveFront ] ) ; if ( straighteningError > 0 ) { lPower = power - straighteningError * straighteningKpLeft ; } else { lPower = power ; } if ( straighteningError < 0 ) { rPower = power + straighteningError * straighteningKpRight ; } else { rPower = power ; } setLeftDtMotors ( lPower * 1 , lPower * -1 ) ; setRightDtMotors ( rPower * -1 , rPower * 1 ) ; wait1Msec ( 25 ) ; } setLeftDtMotors ( 0 , 0 ) ; setRightDtMotors ( 0 , 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 , lPower * direction ) ; setRightDtMotors ( rPower * -1 * direction , 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 //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); //adjust the powers sent to each side if the encoder values don't match //writeDebugStreamLine("%d,%d,%d,%d",lPower*lfMult,lPower*lbMult,rPower*rfMult,rPower*rbMult); //positive powers: //  if left side is ahead, straightening error is positive // 	lPower will decrease //  if right side is ahead, straightening error is negative //  rPower will decrease //negative powers: //  if left side is ahead, straightening error is positive //  power is negative, straighteningError is positive //  power would go higher (more negative) because power - (positive error) //  fix: multiply by sign of power - after this change //  power is positive, straighteningError is still positive (as we want) //  power is negative, straighteningError is multiplied by -1, so: //  (-127) - (12)*(.3)*-1 = -127 + 4 = -123, slower power // //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