RobotCode_ForTesting.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 user-control portion of the code.
-----------------------------------------------------------------------------------*/

/*
Remote mappings:
Left stick Y-value ------- left drive ------- Ch3
Right stick Y-value ------ right drive ------ Ch2
Right shoulder buttons --- lift up, down ---- Btn6U, Btn6D
Right buttons U, D ------- claw up, down ---- Btn8U, Btn8D
Right buttons L, R ------- claw open, close - Btn8L, Btn8R
Top left shoulder button - direction switch - Btn5U
*/

// Variable Setup
// Standard sleep value (in milliseconds)
// Can be adjusted for responsiveness as needed
const int sleepValue = 20;
// Current direction-switching state
// false is normal, true is reversed
bool reversed = false;

task main() {
	turnLEDOn(greenLED);
	while (true) {
		// Driver-control code

		// Drive motor control
		if (reversed) {
			motor[leftMotor] = -vexRT[Ch2];
			motor[rightMotor] = -vexRT[Ch3];
		}
		else {
			motor[leftMotor] = vexRT[Ch3];
			motor[rightMotor] = vexRT[Ch2];
		}

		// Drive motor reversal
		if (vexRT[Btn5U] == 1) { reversed = !reversed; }

		// Lift control
		if (vexRT[Btn6U] == 1) { motor[liftMotor] = 127; }
		else if (vexRT[Btn6D] == 1) { motor[liftMotor] = -100; }

		// Claw swivel control
		if (vexRT[Btn8U] == 1) { motor[swivelMotor] = 127; }
		else if (vexRT[Btn8D] == 1) { motor[swivelMotor] = -100; }

		// Claw grip control
		if (vexRT[Btn8L] == 1) { motor[clawMotor] = 127; }
		else if (vexRT[Btn8R] == 1) { motor[clawMotor] = -127; }

		// Pause for sleepValue milliseconds
		sleep(sleepValue);
	}
}