ClawBotCompetition.c

#pragma config(Motor,  port1,           rightMotor,    tmotorVex393_HBridge, openLoop, reversed)
#pragma config(Motor,  port6,           clawMotor,     tmotorVex269_MC29, openLoop, reversed)
#pragma config(Motor,  port7,           armMotor,      tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port10,          leftMotor,     tmotorVex393_HBridge, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//short movingForward = false; //Should be zero
short holdArm = false; //Should be zero
short movingArm = false;
// double speedMultiplier = 1;

void resetMotor()
{
	motor[rightMotor] = 0;
	motor[leftMotor] = 0;
	if (holdArm == false)
	{
		motor[armMotor] = 0;
	}
}

void moveArmUp(int speed)
{
	if (movingArm == false) {
	motor[armMotor] = speed;
}
}

void freezeArm() {
	holdArm = true;
	motor[armMotor] = 10;
}

void unfreezeArm() {

	holdArm = false;
	resetMotor();

}


void holdArmAt(int speed, int waitTime)
{
	holdArm = true;
	moveArmUp(speed);
	wait1Msec(waitTime);
	// motor[armMotor] = 20;
}

void turnLeft(int speed, int waitTime)
{
	//movingForward = 1;
	wait1Msec(1000);
	motor[rightMotor] = speed * -1;
	motor[leftMotor] = speed;
	wait1Msec(waitTime);
	resetMotor();
}

void turnRight(int speed, int waitTime)
{
	//movingForward = 1;
	wait1Msec(1000);
	motor[rightMotor] = speed;
	motor[leftMotor] = speed * -1;
	wait1Msec(waitTime);
	resetMotor();
}

void moveArmDown(int speed)
{
	if (movingArm == false) {
		motor[armMotor] = -1 * speed;
	}
}

void closeClawFor()
{

	wait1Msec(1000);
	motor[clawMotor] = 70;
}

void openClawFor()
{
	wait1Msec(750);
	motor[clawMotor] = -70;
	wait1Msec(800);
	resetMotor();
}

void closeClaw(int speed) {
	motor[clawMotor] = 70;
}

void openClaw(int speed) {
	motor[clawMotor] = -70;
}

void stopArm() {
	motor[armMotor] = 0;
}

void stopClaw() {
	motor[clawMotor] = 0;
}

void moveForward(int speed, int waitTime)
{
	//movingForward = 1;
	wait1Msec(3000);
	motor[rightMotor] = speed;
	motor[leftMotor] = speed;
	wait1Msec(waitTime);
	resetMotor();
	//movingForward = 0;
}

void driveControl(int leftStick, int rightStick) {

	//movingForward = true;
	motor[rightMotor] = rightStick;
	motor[leftMotor] = leftStick;

}

//These are multiplied, put in seconds not milliseconds
void autonomous1() {
	moveForward(40, 10000);
}

void autonomous2() {
	moveForward(40, 10000);
}


task main {
	while() {

		if (vexRT[Btn7U] && vexRT[Btn8U]) {

//			autonomous1();

		}

		if (vexRT[Btn7D] && vexRT[Btn8D]) {

	//		autonomous2();

		}


		driveControl(vexRT[Ch2], vexRT[Ch3]);

		//motor[rightMotor] = vexRT[Ch2];
		//motor[leftMotor] = vexRT[Ch3];

		//Move Arm
		if (vexRT[Btn5D]) {
		//unfreezeArm();
		moveArmDown(30);
			//
			//movingArm = true;
			} else if (vexRT[Btn5U]) {
			//unfreezeArm();
			moveArmUp(50);
			//movingArm = true;

		} else if (vexRT[Btn5U] == false) {
			stopArm();
	}	else if (vexRT[Btn5D] == false) {
		stopArm();
}


		//Move Claw
		if (vexRT[Btn6D])
		{
			closeClaw(30);
		}
		else if (vexRT[Btn6U])
		{
			openClaw(30);
		} else if (vexRT[Btn6D] == false) {
		stopClaw();
	} else if (vexRT[Btn6U] == false) {
	stopClaw();
}

	}
}