JPII Robotics - COCO BEST 2012

Main.c

/*
 * JPII Robotics 2012
 * Main.C - 10/19/12
 * (c) 2012 Max Kirkby & Thomas Gaubert
 *
 * Licensed under GNU General Public License v3.0 <http://www.gnu.org/licenses/gpl.html>.
 */
int motorArmUpDown = port2; // Moves arm up and down.
int motorVertical = port3; 	// Moves robot up and down.
int motorArmBase = port6; 	// Base rotation
int servoArmCW = port8; 	// The servo controlling the claw
int motorArmCenter = port9; // Moves the arm center motor up and down.

int speedLimit = 55;        // How fast the base rotates

// Controller Channel Reference
// Ch1 = X axis right
// Ch2 = Y axis right
// Ch3 = Y axis left
// Ch4 = X axis left

void RestMotor(int mid);
int GetDirectionRight();
void OpenMotor(int mid, int amount);
void OpenMotorWithLimit(int mid, int amount, int speedLimit);
void OpenMotor(int mid, int power, bool forward);

task main() {
	while(true) {
		// Move it up and down based on left analog.
		OpenMotor(motorArmUpDown, vexRT[Ch2]);
		OpenMotor(motorVertical, -vexRT[Ch3] - 20);

		// Move the arm around.
		OpenMotorWithLimit(motorArmBase, vexRT[Ch1], speedLimit);

		// Move the arm up and down.
		int lop = 32;
		if (vexRT[Btn7R] == 1)
			lop = 127;
		if (vexRT[Btn7L] == 1)
			lop = 16;

		if (vexRT[Btn8U] == 1) {
			motor[motorArmCenter] = 127;
			Sleep(50);
			motor[motorArmCenter] = 0;
		}
		
		if (vexRT[Btn8D] == 1) {
			motor[motorArmCenter] = -127;
			Sleep(50);
			motor[motorArmCenter] = 0;
		}

		if (vexRT[Btn6D] == 1) {
			motor[servoArmCW] = 127;
			// Extend the arm out.
		}
		
		if (vexRT[Btn5D] == 1) {
			motor[servoArmCW] = -127;
			// Retracts the arm.
		}
	}
}

void OpenMotor(int mid, int power, bool forward) {
	int factor = 1;
	if (!forward) {
		factor = -1;
	}
	motor[mid] = (power * 127) / 100 * factor;
}

void OpenMotor(int mid, int amount) {
	motor[mid] = amount;
}

void OpenMotorWithLimit(int mid, int amount, int speedLimit) {
	if(amount > speedLimit || amount < speedLimit * -1) {
		if(amount < 0) {
			amount = speedLimit * -1;
		} else {
			amount = speedLimit;
		}
	}
	motor[mid] = amount;
}

void RestMotor(int mid) {
	motor[mid] = 0;
}

int GetDirectionRight() {
	if (vexRT[Ch2] >= -5 && vexRT[Ch2] <= 5 && vexRT[Ch1] >= -5 && vexRT[Ch1] <= 5)
		return 0;
	else
		return (int)(atan2(vexRT[Ch2],vexRT[Ch1])*(180/3.14159265358979));
}