code.v14.c

#pragma config(Sensor, dgtl1,  rightEncoder,   sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  leftEncoder,    sensorQuadEncoder)
#pragma config(Sensor, dgtl5,  LSbumper,       sensorTouch)
#pragma config(Motor,  port2,           bottomRight,   tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port3,           bottomLeft,    tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port4,           rightLS,       tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port5,           leftLS,        tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port6,           cone,          tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port7,           topRight,      tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port8,           topLeft,       tmotorVex393_MC29, 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"


int threshold = 10;
int LSMAX = 1000;

/*---------------------------------------------------------------------------*/
/*                          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 autoLSStop() {
		motor[rightLS] = 0;
		motor[leftLS] = 0;
}

void autoLSUp(int n) {
		//check quadencoders
		motor[rightLS] = 90;
		motor[leftLS] = 80;
		wait1Msec(n);
}

void autoLSDown(int n) {
		motor[rightLS] = -90;
		motor[leftLS] = -80;
		wait1Msec(n);
}

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

void autoRight(int n) {
		motor[bottomRight] = -75;
		motor[topRight] = 75;
		motor[bottomLeft] = 75;
		motor[topLeft] = -75;
		wait1Msec(n);
		stopDrive();
}

void autoLeft(int n) {
		motor[bottomRight] = 75;
		motor[topRight] = -75;
		motor[bottomLeft] = -75;
		motor[topLeft] = 75;
		wait1Msec(n);
		stopDrive();
}

void autoForward(int n) {
		motor[bottomRight] = 75;
		motor[topRight] = 75;
		motor[bottomLeft] = 75;
		motor[topLeft] = 75;
		wait1Msec(n);
		stopDrive();
}

void autoBackward(int n) {
		motor[bottomRight] = -75;
		motor[topRight] = -75;
		motor[bottomLeft] = -75;
		motor[topLeft] = -75;
		wait1Msec(n);
		stopDrive();
}

void shortAutoRightSide() {
		//drive forward, pick up a mobile goal, 180 right, drive forward and place it in the 10 point zone
		autoForward(1000);
		while(SensorValue(LSbumper) == 0) {
				autoForward(100);
				wait1Msec(10);
		}
		stopDrive();

		autoLSUp(1000);
		autoLSStop();

		autoBackward(1000);

		autoLeft(1800);
		autoForward(2500);
		stopDrive();

		autoLSDown(1000);
		autoLSStop();
		autoBackward(1000);
}

void shortAutoRightSide5Points() {
		autoForward(1000);
		while(SensorValue(LSbumper) == 0) {
				autoForward(100);
				wait1Msec(10);
		}
		stopDrive();
		autoRight(1800);
		autoForward(2500);
		autoBackward(1000);
}

void shortAutoLeftSide5Points() {
		autoForward(1000);
		while(SensorValue(LSbumper) == 0) {
				autoForward(100);
				wait1Msec(10);
		}
		stopDrive();
		autoLeft(1800);
		autoForward(2500);
		autoBackward(1000);
}

void longAuto() {
		shortAutoRightSide();
		autoBackward(1000);
		autoLeft(1000);
		autoForward(3000);
		autoLSUp(1000);
		autoForward(1000);
}

void oneMinute() {
		//drive forward, pick up mobile goal, 180 drive forward and place it in the 10 point zone
		//back up, 180, drive forward, pick up mobile goal, place it in the opposite 10 point zone
		//back up, 90 left, forward, 90 left, drive forward, pick up mobile goal, 180 drive forward and place it in the 10 point zone
		//back up, 180, drive forward, pick up mobile goal, place it in the opposite 10 point zone
		longAuto();
		autoBackward(1000);
		autoLeft(500);
		autoForward(1000);
		autoLeft(500);
		longAuto();
}


task autonomous(){
  	//shortAutoRightSide();
}

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              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, bottom left goes forward, top left goes backwards
					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 claw(int open, int close) {
	if(open == 1) {
			motor[cone] = 45;
	}
  else if(close == 1) {
			motor[cone] = -45;
	}
	else {
		motor[cone] = 0;
	}
}

/*
void clawRotate(int clockwise, int counterclockwise) {
	//trigger 8U counter clockwise if looking from the right side of the robot
	//trigger 8D clockwise if looking from the right side of the robot
		if(clockwise == 1) {
				motor[rightCone] = 100;
				motor[leftCone] = 100;
		}
		else if(counterclockwise == 1) {
				motor[rightCone] = -100;
				motor[leftCone] = -100;
		}
		else {
				motor[rightCone] = 0;
				motor[leftCone] = 0;
		}
}
*/

int hold = 0;
void rotateStop(int button) {
		if(button == 1) {
			hold = 1;
		}

}

void Riley() {
	//drive(vexRT[Ch1], vexRT[Ch3]);
	//viking challenge drive
	drive(vexRT[Ch3], vexRT[Ch1]);
	//for claw
			//trigger 5U is claw open
			//trigger 5D is claw close
	claw(vexRT[Btn5U], vexRT[Btn5D]);
	//for LS
			//right trigger, button 6D extends
			//right trigger, button 6U retracts
	LS(vexRT[Btn6D], vexRT[Btn6U]);
	//clawRotate(vexRT[Btn8D], vexRT[Btn8U]);
}

void Emily() {
	//Ch1 right and left, Ch3 forward and backward
	drive(vexRT[Ch1], vexRT[Ch3]);
	//5U opens, 5D closes
	claw(vexRT[Btn5U], vexRT[Btn5D]);
	//6U goes out, 6D comes in
	LS(vexRT[Btn6U], vexRT[Btn6D]);
}

task usercontrol() {

		while(true) {
		  		Riley();
		}
}