VEXSTARSTRUCKSKILLSAUTO.c

#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, dgtl1,  Claw,           sensorDigitalOut)
#pragma config(Sensor, dgtl2,  clawStop,       sensorTouch)
#pragma config(Sensor, I2C_1,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_2,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_3,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_4,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_5,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Motor,  port2,           bottomRight,   tmotorVex393HighSpeed_MC29, PIDControl, encoderPort, I2C_4)
#pragma config(Motor,  port3,           topRight,      tmotorVex393HighSpeed_MC29, PIDControl, encoderPort, I2C_1)
#pragma config(Motor,  port4,           rightSideLift1, tmotorVex393HighSpeed_MC29, PIDControl, reversed, encoderPort, I2C_5)
#pragma config(Motor,  port5,           rightSideLift2, tmotorVex393HighSpeed_MC29, openLoop, reversed)
#pragma config(Motor,  port6,           leftSideLift1, tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port7,           leftSideLift2, tmotorVex393HighSpeed_MC29, openLoop, reversed)
#pragma config(Motor,  port8,           topLeft,       tmotorVex393HighSpeed_MC29, PIDControl, encoderPort, I2C_2)
#pragma config(Motor,  port9,           bottomLeft,    tmotorVex393HighSpeed_MC29, PIDControl, encoderPort, I2C_3)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

// 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"

/*---------------------------------------------------------------------------*/
/*                          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.                               */
/*---------------------------------------------------------------------------*/


int X2 = 0, Y1 = 0, X1 = 0, threshold = 15, i = 1;// user control variables
void pre_auton()
{

	// 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;
	//counter clockwise is positive and current numbers are in degrees
	//lift means tower
	SensorType[in8] = sensorNone;
	wait1Msec(1000);
	//Reconfigure Analog Port 8 as a Gyro sensor and allow time for ROBOTC to calibrate it
	SensorType[in8] = sensorGyro;
	wait1Msec(2000);
}

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              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.   */
/*---------------------------------------------------------------------------*/


int direction;
long dist;
int speed;
int counter;
float CIRC = 12.566;
long TICKS_PER_DEGREE = 3.47*392/360;
float DIST_PER_TILE= 23.4166;
float ROTATIONS_PER_TILE = DIST_PER_TILE/CIRC;
float const TICKS_PER_TILE = ROTATIONS_PER_TILE*392;

void turnLast(int degree);
void turnS(int degree);
void StraightLift(int i,long d,int s,int t);
void turnSpecial(int degree);
void moveStraight(int i,long d,int s);
void SPECIAL();
void MOVE(int i,long d,int s);
void turn (int degree);
//void strafe(int d,int i);
void lift(int i, int ticks);
void liftC(int i, int ticks);



void teleop()
{	//User Control		Forward Rotational	Strafe
	motor[topRight] = -Y1 + X2;
	motor[bottomRight] =  -Y1 +  X2;
	motor[bottomLeft] = Y1 + X2;
	motor[topLeft] =  Y1 + X2;
}

void lift(int speed)
{
	motor[rightSideLift1] = speed;
	motor[rightSideLift2] = speed;
	motor[leftSideLift1] = speed;
	motor[leftSideLift2] = speed;
}
void stop()
{
	motor[rightSideLift1] = 0;
	motor[rightSideLift2] = 0;
	motor[leftSideLift1] = 0;
	motor[leftSideLift2] = 0;
}


task autonomous()
{
	//Part 1 Preloads

	SensorValue(Claw) = 1;
	moveStraight(-1,1,127);
	//moveStraight(1,1,127);
	//wait1Msec(500);
	MOVE(1,100,66);
	SensorValue(Claw) = 0;
	wait1Msec(500);
	StraightLift(-1,800,110,850);
	sleep(50);
	StraightLift(1,800,110,-600);
	//second preloads
	sleep(50);
	SensorValue(Claw) = 0;
	wait1Msec(500);
	StraightLift(-1,800,110,850);
	sleep(50);
	StraightLift(1,800,110,-600);
	//third preload
	sleep(50);
	SensorValue(Claw) = 0;
	wait1Msec(500);
	StraightLift(-1,800,110,850);
	lift(-1,600);

	//added code on Wdnesday morning
	MOVE(1,700,127);
	turn(90);
	sleep(80);
	MOVE(1,1000,127);
	SensorValue(Claw) = 0;
	sleep(50);
	StraightLift(1,1300,70,400);
	//liftC(1,50);
	sleep(50);
	//turn(-90);
	turnS(-90);//lift while turning
	sleep(50);
	moveStraight(-1,1,127);
	//MOVE(-1,800,127);
	lift(1,850);

	StraightLift(1,900,110,-600);
	sleep(50);
	SensorValue(Claw) = 0;
	wait1Msec(500);
	StraightLift(-1,800,110,850);

	lift(-1,100);
	MOVE(1,100,127);
	turn(-90);
	sleep(50);

	lift(-1,500);

	//lift(-1,501);
	MOVE(1,2250,127);
	SensorValue(Claw) = 0;
	turn(90);
	lift(1,800);

}

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              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.   */
/*---------------------------------------------------------------------------*/

task usercontrol()
{
	// User control code here, inside the loop

	while (true)
	{


		if(vexRT[Btn6U]==1 || vexRT[Btn8DXmtr2] == 1)
			SensorValue[Claw]=1;
		else
			SensorValue[Claw]=0;


		//deadzones
		if(abs(vexRT[Ch3]) > threshold)
			Y1 = vexRT[Ch3];
		else
			Y1 = 0;



		if(abs(vexRT[Ch1]) > threshold)
			X2 = vexRT[Ch1];
		else
			X2 = 0;
		//end of deadzone code


		//PID
		/*iterative(abs(X1));
		terative2(abs(X1));
		iterative3(abs(X1));
		iterative4(abs(X1));
		*/

		if (vexRT[Btn6DXmtr2] == 1){
			if(SensorValue(dgtl2) != 1)
			lift(-127);
		}
		else if (vexRT[Btn5DXmtr2] == 1){

				lift(127);
		}
		else
			stop();
		//teleop();
		motor[topRight] = -Y1 + (X2*0.5);
		motor[bottomRight] =  -Y1 +  (X2*0.5);
		motor[bottomLeft] = Y1 + (X2*0.5);
		motor[topLeft] =  Y1 + (X2*0.5);
	}
}
void moveStraight(int i,long d,int s){
	clearTimer(T1);

	nMotorEncoder(bottomLeft) = 0;
	nMotorEncoder(topLeft) = 0;
	nMotorEncoder(bottomRight) = 0;
	nMotorEncoder(topRight) = 0;
	dist = TICKS_PER_TILE*d*i;
	//remember to change code based on wiring
	setMotorTarget(bottomLeft,dist,s,false);
	setMotorTarget(topLeft,dist,s,false);
	setMotorTarget(bottomRight,-dist,s,false);
	setMotorTarget(topRight,-dist,s,false);

	while((!getMotorTargetCompleted(bottomRight) || !getMotorTargetCompleted(topRight) || !getMotorTargetCompleted(bottomLeft) || !getMotorTargetCompleted(topLeft))&time1[T1]<4000)
	{
		sleep(10);
	}
}

void MOVE(int i,long d,int s){
	clearTimer(T1);

	nMotorEncoder(bottomLeft) = 0;
	nMotorEncoder(topLeft) = 0;
	nMotorEncoder(bottomRight) = 0;
	nMotorEncoder(topRight) = 0;
	// dist = ceil(TICKS_PER_TILE*d*i);
	//remember to change code based on wiring
	setMotorTarget(bottomLeft,d*i,s,false);
	setMotorTarget(topLeft,d*i,s,false);
	setMotorTarget(bottomRight,-d*i,s,false);
	setMotorTarget(topRight,-d*i,s,false);

	while((!getMotorTargetCompleted(bottomRight) || !getMotorTargetCompleted(topRight) || !getMotorTargetCompleted(bottomLeft) || !getMotorTargetCompleted(topLeft))&time1[T1]<4000)
	{
		sleep(10);
	}
}

void turn (int degree){//counter clockwise is positiv
	SensorValue[in8] = 0;
	time1[T1] = 0;
	if(degree < 0)
	{

		while(SensorValue[in8] >(degree*10) && time1[T1] < 3000)
		{
			direction = SensorValue[in8];
			motor[bottomLeft] = (35);
			motor[topLeft] = (35);
			motor[bottomRight] =(35);
			motor[topRight]= (35);
		}
	}
	else {
		while(SensorValue[in8] <(degree*10 )&& time1[T1] < 3000)
		{
			direction = SensorValue[in8];
			motor[bottomLeft] = (-35);
			motor[topLeft] = (-35);
			motor[bottomRight] =(-35);
			motor[topRight]= (-35);
		}
	}
	motor[bottomLeft] = 0;
	motor[topLeft] = 0;
	motor[bottomRight] =0;
	motor[topRight]= 0;
}

void turnSpecial (int degree){//counter clockwise is positive
	SensorValue[in8] = 0;
	time1[T1] = 0;
	if(degree < 0)
	{

		while(SensorValue[in8] >(degree*10) && time1[T1] < 5500)
		{
			direction = SensorValue[in8];
			motor[bottomLeft] = (127);
			motor[topLeft] = (127);
			motor[bottomRight] =(127);
			motor[topRight]= (127);
		}
	}
	else {
		while(SensorValue[in8] <(degree*10 )&& time1[T1] < 5500)
		{
			direction = SensorValue[in8];
			motor[bottomLeft] = (-127);
			motor[topLeft] = (-127);
			motor[bottomRight] =(-127);
			motor[topRight]= (-127);
		}
	}
	motor[bottomLeft] = 0;
	motor[topLeft] = 0;

	motor[bottomRight] =0;
	motor[topRight]= 0;
}

void lift(int i, int ticks){//only direction up is positive
	time1[T1]=0;
	nMotorEncoder(rightSideLift1)=0;
	if(i>0){
		while(ticks > abs(nMotorEncoder(rightSideLift1)))
		{
			counter = nMotorEncoder(rightSideLift1);
			motor[leftSideLift1] = 127;
			motor[leftSideLift2] = 127;
			motor[rightSideLift1] = 127;
			motor[rightSideLift2] = 127;

		}
		if(abs(ticks)>500){
			SensorValue[Claw] = 1;
		}
		//	SensorValue[Claw]ticks
	}
	else
	{
		while(ticks > abs(nMotorEncoder(rightSideLift1))&&SensorValue(dgtl2) != 1)
		{
			motor[leftSideLift1] = -127;
			motor[leftSideLift2] = -127;
			motor[rightSideLift1] = -127;
			motor[rightSideLift2] = -127;
		}
	}
	motor[leftSideLift1] = 0;
	motor[leftSideLift2] = 0;
	motor[rightSideLift1] = 0;
	motor[rightSideLift2]  =0;
}


void liftC(int i, int ticks){//only direction up is positive
	time1[T1]=0;
	nMotorEncoder(rightSideLift1)=0;
	if(i>0){
		while(ticks > abs(nMotorEncoder(rightSideLift1)))
		{
			counter = nMotorEncoder(rightSideLift1);
			motor[leftSideLift1] = 127;
			motor[leftSideLift2] = 127;
			motor[rightSideLift1] = 127;
			motor[rightSideLift2] = 127;

		}
		// SensorValue[Claw] = 1;
		//	SensorValue[Claw] = 0;
	}
	else
	{
		while(ticks > abs(nMotorEncoder(rightSideLift1))&&SensorValue(dgtl2) != 1)
		{
			motor[leftSideLift1] = -127;
			motor[leftSideLift2] = -127;
			motor[rightSideLift1] = -127;
			motor[rightSideLift2] = -127;
		}
	}
	motor[leftSideLift1] = 0;
	motor[leftSideLift2] = 0;
	motor[rightSideLift1] = 0;
	motor[rightSideLift2]  =0;
}

void SPECIAL (){
	nMotorEncoder(bottomLeft) = 0;
	nMotorEncoder(topLeft) = 0;
	nMotorEncoder(bottomRight) = 0;
	nMotorEncoder(topRight) = 0;
	// dist = ceil(TICKS_PER_TILE*d*i);
	//remember to change code based on wiring
	setMotorTarget(bottomLeft,700,50,false);
	setMotorTarget(topLeft,700,50,false);


	while((!getMotorTargetCompleted(bottomRight) || !getMotorTargetCompleted(topRight) || !getMotorTargetCompleted(bottomLeft) || !getMotorTargetCompleted(topLeft))&time1[T1]<4000)
	{
		sleep(10);
	}
	nMotorEncoder(bottomLeft) = 0;
	nMotorEncoder(topLeft) = 0;
	setMotorTarget(bottomRight,-700,50,false);
	setMotorTarget(topRight,-700,50,false);
	while((!getMotorTargetCompleted(bottomRight) || !getMotorTargetCompleted(topRight) || !getMotorTargetCompleted(bottomLeft) || !getMotorTargetCompleted(topLeft))&time1[T1]<4000)
	{
		sleep(10);
	}
}

void StraightLift (int i, long d, int s,int t){// move straight and lift to throw at the same time
	clearTimer(T2);

	nMotorEncoder(bottomLeft) = 0;
	nMotorEncoder(topLeft) = 0;
	nMotorEncoder(bottomRight) = 0;
	nMotorEncoder(topRight) = 0;
	// dist = ceil(TICKS_PER_TILE*d*i);
	//remember to change code based on wiring
	setMotorTarget(bottomLeft,d*i,s,false);
	setMotorTarget(topLeft,d*i,s,false);
	setMotorTarget(bottomRight,-d*i,s,false);
	setMotorTarget(topRight,-d*i,s,false);
	sleep(50);
	if (t>0)
		lift(1,t);
	else
		lift(-1,abs(t));



	while((!getMotorTargetCompleted(bottomRight) || !getMotorTargetCompleted(topRight) || !getMotorTargetCompleted(bottomLeft) || !getMotorTargetCompleted(topLeft))&time1[T2]<4000)
	{
		sleep(10);
	}
}

void turnS(int degree){
	time1[T1]=0;
	SensorValue[in8] = 0;
	if(i>0){
		motor[leftSideLift1] = 40;
		motor[leftSideLift2] = 40;
		motor[rightSideLift1] = 40;
		motor[rightSideLift2] = 40;

		if(degree < 0)
		{

			while(SensorValue[in8] >(degree*10) && time1[T1] < 3000)
			{
				direction = SensorValue[in8];
				motor[bottomLeft] = (35);
				motor[topLeft] = (35);
				motor[bottomRight] =(35);
				motor[topRight]= (35);
			}
		}
		else {
			while(SensorValue[in8] <(degree*10 )&& time1[T1] < 3000)
			{
				direction = SensorValue[in8];
				motor[bottomLeft] = (-35);
				motor[topLeft] = (-35);
				motor[bottomRight] =(-35);
				motor[topRight]= (-35);
			}
		}
		motor[leftSideLift1] = 0;
		motor[leftSideLift2] = 0;
		motor[rightSideLift1] = 0;
		motor[rightSideLift2]  =0;

	}
	motor[bottomLeft] = 0;
	motor[topLeft] = 0;
	motor[bottomRight] =0;
	motor[topRight]= 0;
}


void turnLast(int degree){
	time1[T1]=0;
	SensorValue[in8] = 0;
	if(i>0){
		motor[leftSideLift1] = 127;
		motor[leftSideLift2] = 127;
		motor[rightSideLift1] = 127;
		motor[rightSideLift2] = 127;

		if(degree < 0)
		{

			while(SensorValue[in8] >(degree*10) && time1[T1] < 1500)
			{
				direction = SensorValue[in8];
				motor[bottomLeft] = (127);
				motor[topLeft] = (127);
				motor[bottomRight] =(127);
				motor[topRight]= (127);
			}
		}
		else {
			while(SensorValue[in8] <(degree*10 )&& time1[T1] < 1500)
			{
				direction = SensorValue[in8];
				motor[bottomLeft] = (-127);
				motor[topLeft] = (-127);
				motor[bottomRight] =(-127);
				motor[topRight]= (-127);
			}
		}
		motor[leftSideLift1] = 0;
		motor[leftSideLift2] = 0;
		motor[rightSideLift1] = 0;
		motor[rightSideLift2]  =0;

	}
	SensorValue(Claw) = 1;
	motor[bottomLeft] = 0;
	motor[topLeft] = 0;
	motor[bottomRight] =0;
	motor[topRight]= 0;
}