RobotCode_AutoOnly_Reversed.c

#pragma config(Motor,  port2,           leftMotor,     tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port3,           rightMotor,    tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port4,           liftMotor,     tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port5,           swivelMotor,   tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port6,           clawMotor,     tmotorVex393_MC29, openLoop)
#pragma config(Sensor, dgtl10,          redLED,        sensorLEDtoVCC)
#pragma config(Sensor, dgtl11,          yellowLED,     sensorLEDtoVCC)
#pragma config(Sensor, dgtl12,          greenLED,      sensorLEDtoVCC)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX2)

/*-----------------------------------------------------------------------------------
Robot code for GHS VEX Team G
2018-19 Competiton Season

Team members are Lucas Ritzdorf, Riker Foster, and Lauryn Vornbrock.

This version of the robot code is intended to be used exclusively for testing
purposes. For that reason, it is not equipped to interact with the competition
control system, and will immediately start the AUTONOMOUS portion of the code.
-----------------------------------------------------------------------------------*/

// Variable Definitions
const float turnTime = 1000;
float clawTime = 500;
int clawDir = -1;
float liftTime = 3000;
int liftDir = 1;

// Task to open or close the claw concurrently
task claw()
{
	setMotor(clawMotor, clawDir*127);
	sleep(clawTime);
	stopMotor(clawMotor);
}

// Task to raise or lower the lift concurrently
task lift()
{
	setMotor(liftMotor, liftDir*127);
	sleep(liftTime);
	stopMotor(liftMotor);
}

task auto()
{
	turnLEDOff(redLED);
	turnLEDOn(yellowLED);
	// Open claw and...
	clawDir = -1; clawTime = 1000; startTask(claw);
	// Drive forwards
	setMultipleMotors(127, leftMotor, rightMotor);
	sleep(1750);
	// Ensure claw is open and...
	while(getTaskState(claw) == taskStateRunning) { sleep(500); }
	// Lift cap
	setMotor(liftMotor, 64);
	setMultipleMotors(32, leftMotor, rightMotor);
	sleep(1000);
	// Set cap down
	setMotor(leftMotor, -32);
	setMotor(rightMotor, 16);
	sleep(1000);
	setMotor(liftMotor, -64);
	sleep(1000);
	stopMotor(liftMotor);
	// Turn slightly right
	setMotor(leftMotor, 127);
	setMotor(rightMotor, -127);
	sleep(500);
	// Back up
	setMultipleMotors(leftMotor, rightMotor, -127);
	sleep(1500);
	// Turn left
	setMotor(leftMotor, -127);
	setMotor(rightMotor, 127);
	sleep(turnTime);
	// Drive forward
	setMultipleMotors(127, leftMotor, rightMotor);
	sleep(1000);
	// Turn right
	setMotor(leftMotor, 127);
	setMotor(rightMotor, -127);
	sleep(turnTime);
	// Drive forward slowly to flip cap
	setMultipleMotors(64, leftMotor, rightMotor);
	sleep(1000);
	setMultipleMotors(32, leftMotor, rightMotor);
	setMotor(liftMotor, 127);
	sleep(2000);
	// Pause
	stopAllMotors();
	sleep(250);
	// Lower lift and...
	liftDir = -1; liftTime = 2000; startTask(lift);
	// Back up
	setMultipleMotors(leftMotor, rightMotor, -127);
	sleep(1500);
	// Turn left
	setMotor(leftMotor, -127);
	setMotor(rightMotor, 127);
	sleep(turnTime);
	// Ensure lift is down and...
	while(getTaskState(lift) == taskStateRunning) { sleep(500); }
	// Drive forward to toggle flag
	setMultipleMotors(127, leftMotor, rightMotor);
	sleep(1000);
	// Done
	stopAllMotors();
	turnLEDOff(greenLED);
	turnLEDOn(yellowLED);
}

// Emergency-stop task
task e_stop()
{
	while(true) {
		if (vexRT[Btn8L] == 1) {
			turnLEDOff(yellowLED);
			turnLEDOn(redLED);
			stopTask(auto);
			stopAllMotors();
		}
		sleep(0.1);
	}
}

task main()
{
	turnLEDOff(redLED);
	turnLEDOff(yellowLED);
	turnLEDOff(greenLED);
	startTask(e_stop);
	while(true) {
		if (vexRT[Btn8R] == 1) { startTask(auto); }
		sleep(100);
	}
}