attemptAtLCD.c

#pragma config(UART_Usage, UART1, uartVEXLCD, baudRate19200, IOPins, None, None)
#pragma config(Sensor, dgtl1,  rightEncoder,   sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  leftEncoder,    sensorQuadEncoder)
#pragma config(Sensor, dgtl5,  LSbumper,       sensorTouch)
#pragma config(Motor,  port2,           topRight,      tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port3,           bottomLeft,    tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port4,           rightLS,       tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port5,           leftLS,        tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port6,           rightCone,     tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port7,           bottomRight,   tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port8,           topLeft,       tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port9,           leftCone,      tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port10,          rubberbands,   tmotorVex393_HBridge, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*        Description: Competition template for VEX EDR                      */
/*                                                                           */
/*---------------------------------------------------------------------------*/

// This code is for the VEX cortex platform
#pragma platform(VEX2)

// Select Download method as "competition"
#pragma competitionControl(Competition)

//Main competition background code...do not modify!
#include "Vex_Competition_Includes.c"


//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)
//Variable that will carry between modes
int Program;


const int threshold = 10;
const int QUAD_TOLERANCE = 40;

/*---------------------------------------------------------------------------*/
/*                          Pre-Autonomous Functions                         */
/*                                                                           */
/*  You may want to perform some actions before the competition starts.      */
/*  Do them in the following function.  You must return from this function   */
/*  or the autonomous and usercontrol tasks will not be started.  This       */
/*  function is only called once after the cortex has been powered on and    */
/*  not every time that the robot is disabled.                               */
/*---------------------------------------------------------------------------*/

void pre_auton()
{
  // Set bStopTasksBetweenModes to false if you want to keep user created tasks running between
  // Autonomous and Tele-Op modes. You will need to manage all user created tasks if set to false.
  bStopTasksBetweenModes = true;

  SensorValue(rightEncoder) = 0;
	SensorValue(leftEncoder) = 0;

	//Leave this value alone
	int lcdScreenMin = 1;
	//This keeps track of which program you want to run
	int lcdScreen = 1;
	//Change this value to be the maximum number of programs you want on the robot
	int lcdScreenMax = 4;
	//Turns on the Backlight
	bLCDBacklight = true;

	//Copied from someone's sample code because the documentation for RobotC won't tell me anything useful
	//These should logically work, but I'm not 100% sure
	const short leftButton = 1;
	const short centerButton = 2;
	const short rightButton = 4;

	while (vrDisabled == 1) { //Ensures this code will run ONLY when the robot is disabled
		if (nLCDButtons == leftButton) { //Scrolls to the left
			if (lcdScreenMin == lcdScreen) {
				lcdScreen = lcdScreenMax;
				wait1Msec(250);
			} else {
				lcdScreen --;
				wait1Msec(250);
			}
		}
		if (nLCDButtons == rightButton) { //Scrolls to the right
			if (lcdScreenMax == lcdScreen) {
				lcdScreen = lcdScreenMin;
				wait1Msec(250);
			} else {
				lcdScreen++;
				wait1Msec(250);
			}
		}
		if (lcdScreen == 1 && Program != 1) {
			displayLCDCenteredString (0, "Program"); //Name the first program here
			displayLCDCenteredString (1, "1"); //Name the first program here
			if (nLCDButtons == centerButton) {
				Program = lcdScreen; //Sets the Program to the one on-screen
				displayLCDCenteredString (0, "Autonomous Has");
				displayLCDCenteredString (1, "Been Selected!");
				wait1Msec(1500);
			}
		} else if (lcdScreen == 1 && Program == 1) {
			displayLCDCenteredString (0, "Program"); //We use brackets to mark which program we have chosen
			displayLCDCenteredString (1, "[1]"); //So that while we're scrolling, we can have one marked
		} else if (lcdScreen == 2 && Program != 2) {
			displayLCDCenteredString (0, "Program"); //Name the second program here
			displayLCDCenteredString (1, "2"); //Name the second program here
			if (nLCDButtons == centerButton) {
				Program = lcdScreen; //Sets the Program to the one on-screen
				displayLCDCenteredString (0, "Autonomous Has");
				displayLCDCenteredString (1, "Been Selected!");
				wait1Msec(1500);
			}
		} else if (lcdScreen == 2 && Program == 2) {
			displayLCDCenteredString (0, "Program"); //We use brackets to mark which program we have chosen
			displayLCDCenteredString (1, "[2]"); //So that while we're scrolling, we can have one marked
		} else if (lcdScreen == 3 && Program != 3) {
			displayLCDCenteredString (0, "Program"); //Name the third program here
			displayLCDCenteredString (1, "3"); //Name the third program here
			if (nLCDButtons == centerButton) {
				Program = lcdScreen; //Sets the Program to the one on-screen
				displayLCDCenteredString (0, "Autonomous Has");
				displayLCDCenteredString (1, "Been Selected!");
				wait1Msec(1500);
			}
		} else if (lcdScreen == 3 && Program == 3) {
			displayLCDCenteredString (0, "Program"); //We use brackets to mark which program we have chosen
			displayLCDCenteredString (1, "[3]"); //So that while we're scrolling, we can have one marked
		} else if (lcdScreen == 4 && Program != 4) {
			displayLCDCenteredString (0, "Program"); //Name the fourth program here
			displayLCDCenteredString (1, "4"); //Name the fourth program here
			if (nLCDButtons == centerButton) {
				Program = lcdScreen; //Sets the Program to the one on-screen
				displayLCDCenteredString (0, "Autonomous Has");
				displayLCDCenteredString (1, "Been Selected!");
				wait1Msec(1500);
			}
		} else if (lcdScreen == 4 && Program == 4) {
			displayLCDCenteredString (0, "Program"); //We use brackets to mark which program we have chosen
			displayLCDCenteredString (1, "[4]"); //So that while we're scrolling, we can have one marked
		}
	}

}


/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              Autonomous Task                              */
/*                                                                           */
/*  This task is used to control your robot during the autonomous phase of   */
/*  a VEX Competition.                                                       */
/*                                                                           */
/*  You must modify the code to add your own robot specific commands here.   */
/*---------------------------------------------------------------------------*/

void resetQuads() {
		SensorValue[rightEncoder] = 0;
		SensorValue[leftEncoder] = 0;
}

void waitTicks(int ticks, int timeout = -1) {
	resetQuads();
	ticks = abs(ticks);
	timeout *= 1000;

	while (abs(SensorValue[rightEncoder])  - QUAD_TOLERANCE < ticks &&
		   abs(SensorValue[leftEncoder]) - QUAD_TOLERANCE < ticks && (timeout > 0 || timeout == -1000))  {
		wait1Msec(100);
		if (timeout != -1000) timeout -= 100;
	}
}

void autoLSStop() {
		motor[rightLS] = 0;
		motor[leftLS] = 0;
}

void stopLift() {
		motor[rightCone] = 0;
		motor[leftCone] = 0;
}

void autoLift(int speed, int time) {
		//lift clockwise = positive
		//lift counterClockwise = negative
		motor[rightCone] = speed;
		motor[leftCone] = speed;
		wait1Msec(time);
		stopLift();
}

void stopRubberbands() {
		motor[rubberbands] = 0;
}

void rubberbandMechanism(int speed, int time) {
		//rotateIn = negative
		//rotateOut = positive
		motor[rubberbands] = speed;
		wait1Msec(time);
		stopRubberbands();
}

void autoLS(const string motion, int time) {
		if(motion == "up") {
				motor[rightLS] = 90;
				motor[leftLS] = 80;
				wait1Msec(time);
		}
		if(motion == "down") {
				motor[rightLS] = -90;
				motor[leftLS] = -80;
				wait1Msec(time);
		}
		autoLSStop();
}

void stopDrive() {
		motor[bottomRight] = 0;
		motor[bottomLeft] = 0;
		motor[topRight] = 0;
		motor[topLeft] = 0;
}

void autoDrive(const string direction, int speed, int ticks) {
		if(direction == "forward") {
				motor[bottomRight] = speed;
				motor[topRight] = speed;
				motor[bottomLeft] = speed;
				motor[topLeft] = speed;
				waitTicks(ticks);
		}
		if(direction == "backward") {
				motor[bottomRight] = -speed;
				motor[topRight] = -speed;
				motor[bottomLeft] = -speed;
				motor[topLeft] = -speed;
				waitTicks(ticks);
		}
		if(direction == "right") {
				motor[bottomRight] = -speed;
				motor[topRight] = -speed;
				motor[bottomLeft] = speed;
				motor[topLeft] = speed;
				waitTicks(ticks);
		}
		if(direction == "left") {
				motor[bottomRight] = speed;
				motor[topRight] = speed;
				motor[bottomLeft] = -speed;
				motor[topLeft] = -speed;
				waitTicks(ticks);
		}
		stopDrive();
}

void bumper() {
	while(SensorValue(LSbumper) == 0) {
			autoDrive("forward", 100, 1);
			wait1Msec(10);
	}
}

void left5points() {
		// for 5 points incase other team has a better 20 point autonomous
		//autoDrive("forward", 90, 100);
		autoLift(-90, 1000);
		bumper();
		stopDrive();
		autoLS("up", 1200);
		autoDrive("backward", 100, 700);
		autoDrive("right", 75, 400);
		autoDrive("forward", 80, 100);
		autoDrive("right", 75, 50);


		autoDrive("forward", 100, 1200);
		autoLS("down", 800);
		autoDrive("backward", 75, 1500);
}

void right5points() {
		// for 5 points incase other team has a better 20 point autonomous
		autoDrive("forward", 90, 1000);
		while(SensorValue(LSbumper) == 0) {
				autoDrive("forward", 75, 100);
				wait1Msec(10);
		}

		stopDrive();
		autoLS("up", 800);
		autoDrive("left", 75, 700);
		autoDrive("forward", 100, 2000);
		autoLS("down", 800);
		autoDrive("backward", 75, 1000);
}

void shortAutoRightSide() {
		//drive forward, pick up a mobile goal, 180 left, drive forward and place it in the 10 point zone
		autoDrive("forward", 90, 1000);
		bumper();
		stopDrive();

		autoLS("up", 1000);
		autoDrive("backward", 100, 700);
		autoDrive("left", 75, 400);
		autoDrive("forward", 80, 100);
		autoDrive("left", 75, 50);

		autoDrive("forward", 100, 1500);
		autoLS("up", 600);
		autoDrive("left", 75, 100);
		autoDrive("forward", 127, 500);
		autoLS("down", 400);
		autoDrive("backward", 75, 200);
}

void shortAutoLeftSide() {
		//drive forward, pick up a mobile goal, 180 right, drive forward and place it in the 10 point zone
		autoDrive("forward", 90, 1000);
		bumper();
		stopDrive();

		autoLS("up", 800);
		autoDrive("backward", 100, 700);
		autoDrive("right", 75, 400);
		autoDrive("forward", 80, 100);
		autoDrive("right", 75, 50);

		autoDrive("forward", 100, 1500);
		autoLS("up", 600);
		autoDrive("right", 75, 100);
		autoDrive("forward", 127, 500);
		autoLS("down", 400);
		autoDrive("backward", 75, 200);
}

void line() {
		//first mobile goal
		bumper();
		autoLS("up", 800);
		autoDrive("right", 75, 400);
		autoDrive("forward", 100, 300);
		autoLS("up", 600);
		autoDrive("forward", 100, 100);
		autoLS("down", 400);
		autoDrive("backward", 100, 50);
		autoDrive("right", 75, 400);
		//far mobile goal
		//bumper();
		autoDrive("forward", 100, 700);
		autoLS("up", 800);
		autoDrive("forward", 100, 100);
		autoDrive("left", 100, 50);
		autoDrive("forward", 100, 20);
		autoDrive("right", 100, 20);
		autoLS("up", 600);
		autoDrive("forward", 127, 200);
		autoDrive("backward", 127, 50);
}

void oneMinute() {
		line();
		autoDrive("backward", 100, 100);
		autoDrive("left", 100, 100);
		autoDrive("forward", 100, 100);
		autoDrive("left", 100, 100);
		line();
}


task autonomous(){
  	//left5points();
  	//right5points();
			line();
  	//shortAutoLeftSide();
  	//shortAutoRightSide();
  	//oneMinute();

		if(Program == 1){
			right5points();
		}
		else if(Program == 2){
			left5points();
		}
		else if(Program == 3){
			//Put your third program in here
		}
}

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              User Control Task                            */
/*                                                                           */
/*  This task is used to control your robot during the user control phase of */
/*  a VEX Competition.                                                       */
/*                                                                           */
/*  You must modify the code to add your own robot specific commands here.   */
/*---------------------------------------------------------------------------*/

void drive(int y, int x) {
			if(abs(x) > threshold) {
					//right, bottom right goes backwards, top right goes forward
					motor[bottomRight] = -x;
					motor[topRight] = -x;
					motor[bottomLeft] = x;
					motor[topLeft] = x;
			}
			//forward and backward
			else if(abs(y) > threshold) {
					motor[bottomRight] = y;
					motor[topRight] = y;
					motor[bottomLeft] = y;
					motor[topLeft] = y;
			} else {
					stopDrive();
			}
}

void LS(int extend, int retract) {
	//positive LS moves backward
				if(extend == 1) {
						//if(abs(SensorValue(rightEncoder)) < LSMAX && abs(SensorValue(leftEncoder)) < LSMAX) {
								motor[rightLS] = 90;
								motor[leftLS] = 80;
						//}
				}
				else if(retract == 1) {
						motor[rightLS] = -90;
						motor[leftLS] = -80;
				}
				else {
						motor[rightLS] = 0;
						motor[leftLS] = 0;
				}
}

void lift(int clockwise, int counterClockwise, int speed) {
		if(clockwise == 1) {
			 motor[rightCone] = speed;
			 motor[leftCone] = speed;
		}
		else if(counterClockwise == 1) {
			motor[rightCone] = -speed;
			motor[leftCone] = -speed;
		}
		else {
			motor[rightCone] = 0;
			motor[leftCone] = 0;
		}
}

void cone(int rotateIn, int rotateOut) {
		if(rotateIn == 1) {
				motor[rubberbands] = -80;
		}
		else if(rotateOut == 1) {
				motor[rubberbands] = 80;
		}
		else {
				motor[rubberbands] = 0;
		}
}


void Riley() {
	drive(vexRT[Ch3], vexRT[Ch1]);
	//for LS
			//right trigger, button 6D extends
			//right trigger, button 6U retracts
	LS(vexRT[Btn6D], vexRT[Btn6U]);
	lift(vexRT[Btn5D], vexRT[Btn5U], 80);
	cone(vexRT[Btn7U], vexRT[Btn7D]);
}

task usercontrol() {

		while(true) {
		  		Riley();
		}
}