Before 12-19/Autonomous/autonomous.c
void driveTranslateStart(int angle, int power) {
	// Calculate, according to our holonomic drive algorithm, the modifications to individual motor powers
	if (angle < 90) {
		// First quadrant (motors 2 and 4 coarse, 1 and 3 fine)
		motor[motorDrive1] += (int) (power * (angle *  HOLONOMIC_DRIVE_CONSTANT - 1));
		motor[motorDrive2] += (int) -power;
		motor[motorDrive3] += (int) (power * (angle * -HOLONOMIC_DRIVE_CONSTANT + 1));
		motor[motorDrive4] += (int) power;
	} else if (angle < 180) {
		// Second quadrant (motors 1 and 3 coarse, 2 and 4 fine)
		motor[motorDrive1] += (int) power;
		motor[motorDrive2] += (int) (power * (angle *  HOLONOMIC_DRIVE_CONSTANT - 3));
		motor[motorDrive3] += (int) -power;
		motor[motorDrive4] += (int) (power * (angle * -HOLONOMIC_DRIVE_CONSTANT + 3));
	} else if (angle < 270) {
		// Third quadrant (motors 2 and 4 coarse, 1 and 3 fine, inverted)
		motor[motorDrive1] += (int) (power * (angle * -HOLONOMIC_DRIVE_CONSTANT + 5));
		motor[motorDrive2] += (int) power;
		motor[motorDrive3] += (int) (power * (angle *  HOLONOMIC_DRIVE_CONSTANT - 5));
		motor[motorDrive4] += (int) -power;
	} else if (angle < 360) {
		// Fourth quadrant (motors 1 and 3 coarse, 2 and 4 fine, inverted)
		motor[motorDrive1] += (int) -power;
		motor[motorDrive2] += (int) (power * (angle * -HOLONOMIC_DRIVE_CONSTANT + 7));
		motor[motorDrive3] += (int) power;
		motor[motorDrive4] += (int) (power * (angle *  HOLONOMIC_DRIVE_CONSTANT - 7));
	}
}
// Calculates and applies individual drive motor powers that result in the given robot drive angle and power
Calculates and applies individual drive motor powers that result in the given robot drive angle and power
Calculates and applies individual drive motor powers that result in the given robot drive angle and power
void driveTranslateStart(int angle, int power) {
	if (angle < 90) {
		motor[motorDrive1] += (int) (power * (angle *  HOLONOMIC_DRIVE_CONSTANT - 1));
		motor[motorDrive2] += (int) -power;
		motor[motorDrive3] += (int) (power * (angle * -HOLONOMIC_DRIVE_CONSTANT + 1));
		motor[motorDrive4] += (int) power;
	} else if (angle < 180) {
		motor[motorDrive1] += (int) power;
		motor[motorDrive2] += (int) (power * (angle *  HOLONOMIC_DRIVE_CONSTANT - 3));
		motor[motorDrive3] += (int) -power;
		motor[motorDrive4] += (int) (power * (angle * -HOLONOMIC_DRIVE_CONSTANT + 3));
	} else if (angle < 270) {
		motor[motorDrive1] += (int) (power * (angle * -HOLONOMIC_DRIVE_CONSTANT + 5));
		motor[motorDrive2] += (int) power;
		motor[motorDrive3] += (int) (power * (angle *  HOLONOMIC_DRIVE_CONSTANT - 5));
		motor[motorDrive4] += (int) -power;
	} else if (angle < 360) {
		motor[motorDrive1] += (int) -power;
		motor[motorDrive2] += (int) (power * (angle * -HOLONOMIC_DRIVE_CONSTANT + 7));
		motor[motorDrive3] += (int) power;
		motor[motorDrive4] += (int) (power * (angle *  HOLONOMIC_DRIVE_CONSTANT - 7));
	}
}
void driveTranslateStart ( int angle , int power ) { if ( angle < 90 ) { motor [ motorDrive1 ] += ( int ) ( power * ( angle * HOLONOMIC_DRIVE_CONSTANT - 1 ) ) ; motor [ motorDrive2 ] += ( int ) - power ; motor [ motorDrive3 ] += ( int ) ( power * ( angle * - HOLONOMIC_DRIVE_CONSTANT + 1 ) ) ; motor [ motorDrive4 ] += ( int ) power ; } else if ( angle < 180 ) { motor [ motorDrive1 ] += ( int ) power ; motor [ motorDrive2 ] += ( int ) ( power * ( angle * HOLONOMIC_DRIVE_CONSTANT - 3 ) ) ; motor [ motorDrive3 ] += ( int ) - power ; motor [ motorDrive4 ] += ( int ) ( power * ( angle * - HOLONOMIC_DRIVE_CONSTANT + 3 ) ) ; } else if ( angle < 270 ) { motor [ motorDrive1 ] += ( int ) ( power * ( angle * - HOLONOMIC_DRIVE_CONSTANT + 5 ) ) ; motor [ motorDrive2 ] += ( int ) power ; motor [ motorDrive3 ] += ( int ) ( power * ( angle * HOLONOMIC_DRIVE_CONSTANT - 5 ) ) ; motor [ motorDrive4 ] += ( int ) - power ; } else if ( angle < 360 ) { motor [ motorDrive1 ] += ( int ) - power ; motor [ motorDrive2 ] += ( int ) ( power * ( angle * - HOLONOMIC_DRIVE_CONSTANT + 7 ) ) ; motor [ motorDrive3 ] += ( int ) power ; motor [ motorDrive4 ] += ( int ) ( power * ( angle * HOLONOMIC_DRIVE_CONSTANT - 7 ) ) ; } }
Calculates and applies individual drive motor powers that result in the given robot drive angle and power
Calculates and applies individual drive motor powers that result in the given robot drive angle and power
// Calculate, according to our holonomic drive algorithm, the modifications to individual motor powers // First quadrant (motors 2 and 4 coarse, 1 and 3 fine) // Second quadrant (motors 1 and 3 coarse, 2 and 4 fine) // Third quadrant (motors 2 and 4 coarse, 1 and 3 fine, inverted) // Fourth quadrant (motors 1 and 3 coarse, 2 and 4 fine, inverted)