code.v22.c

#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)
#pragma config(Motor,  port3,           bottomLeft,    tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port4,           rightLS,       tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port5,           leftLS,        tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port6,           rightCone,     tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port7,           bottomRight,   tmotorVex393_MC29, openLoop, reversed)
#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"


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(){
	SensorValue(rightEncoder) = 0;
	SensorValue(leftEncoder) = 0;

  // Set bStopTasksBetweenModes to false if you want to keep user created tasks
  // running between Autonomous and Driver controlled modes. You will need to
  // manage all user created tasks if set to false.
  bStopTasksBetweenModes = true;

	// Set bDisplayCompetitionStatusOnLcd to false if you don't want the LCD
	// used by the competition include file, for example, you might want
	// to display your team name on the LCD in this function.
	// bDisplayCompetitionStatusOnLcd = false;

  // All activities that occur before the competition starts
  // Example: clearing encoders, setting servo positions, ...
}

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              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 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);
		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();
}

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              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();
		}
}