CompetitionCode.c

#pragma config(Motor,  port2,           backright,     tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port3,           leftdrive,     tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port4,           frontright,    tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port5,           chainright,    tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port6,           chainleft,     tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port7,           drfleft,       tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port8,           drfright,      tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port9,           mogo,          tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port10,          claw,          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"


/////////////////////////////////////////////////////////////////////////////////////////
//
//                          Pre-Autonomous Functions
//
// You may want to perform some actions before the competition starts. Do them in the
// following function.
//
/////////////////////////////////////////////////////////////////////////////////////////
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;
	// 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.
//
/////////////////////////////////////////////////////////////////////////////////////////

task autonomous()
{
  //move lift up
		motor [drfleft] = -127;
		motor [drfright] = -127;
		wait1Msec(1500);

		motor [drfleft] = 0;
		motor [drfright] = 0;
		wait1Msec(100);


		//put mogo out
		motor [mogo] = 127;
		wait1Msec(1040);

		motor [mogo] = 0;
		wait1Msec(50);

		//move forward 3ish ft
		motor[frontright]=127;
		motor[leftdrive]=127;
		motor[backright]=127;
		motor [mogo] = 27;
		wait1Msec(1500);

		motor [mogo] = 0;
		motor[frontright]=0;
		motor[leftdrive]=0;
		motor[backright]=0;
		wait1Msec(400);


		//bring mogo back in
		motor [mogo] = -127;
		wait1Msec(1000);

		motor [mogo] = 0;
		wait1Msec(100);

		//move lift back down
		motor [drfleft] = 127;
		motor [drfright] = 127;
		wait1Msec(950);

		motor [drfleft] = 0;
		motor [drfright] = 0;
		wait1Msec(100);


		//move back
		motor [leftdrive] = -127;
		motor [frontright] = -127;
		motor [backright] = -127;
		wait1Msec(1650);

		motor [leftdrive] = 0;
		motor [frontright] = 0;
		motor [backright] = 0;
		wait1Msec(100);

		//turn
		motor [leftdrive] = -127;
		motor [frontright] = 127;
		motor [backright] = 127;
		wait1Msec(900);

		motor [leftdrive] = 0;
		motor [frontright] = 0;
		motor [backright] = 0;
		wait1Msec(100);


		//move forward
		motor [leftdrive] = 127;
		motor [frontright] = 127;
		motor [backright] = 127;
		wait1Msec(250);

		motor [leftdrive] = 0;
		motor [frontright] = 0;
		motor [backright] = 0;
		wait1Msec(100);

		//move lift back up
		motor [drfleft] = -127;
		motor [drfright] = -127;
		motor [claw] = -127;
		wait1Msec(650);


		motor [claw] = 0;
		motor [drfleft] = 0;
		motor [drfright] = 0;
		wait1Msec(100);

		//put mogo back out
		motor [mogo] = 127;
		wait1Msec(1000);

		motor [mogo] = 0;
		wait1Msec(50);

		//move back
		motor [leftdrive] = -127;
		motor [frontright] = -127;
		motor [backright] = -127;
		wait1Msec(600);

		motor [leftdrive] = 0;
		motor [frontright] = 0;
		motor [backright] = 0;
		wait1Msec(50);

}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 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)
	{
    //joystick control:
		motor[leftdrive] = vexRT[Ch3];
		motor[frontright] = vexRT[Ch2];
		motor[backright] = vexRT[Ch2];

		//lift control
		if(vexRT[Btn5D]==1){

			motor [drfleft] = 127;
			motor [drfright] = 127;


			}else if (vexRT[Btn5U]==1){
			motor [drfleft] = -127;
			motor [drfright] = -127;


			}else{
			motor[drfleft] = 0;
			motor[drfright] =0;

		}


		//pick up for first 3 cones
		if(vexRT[Btn8U]==1) {
			//claw closes
			motor [claw] = 127;
			wait1Msec(200);

			motor[claw] = 0;
			wait1Msec(50);

			//lift moves up
			motor[drfright] = -127;
			motor[drfleft] = -127;
			wait1Msec(400);

			motor[drfright] = 0;
			motor[drfleft] = 0;
			wait1Msec(50);

			//chain moves over
			motor[chainleft] = 100;
			motor[chainright] =100;
			wait1Msec(700);

			motor[chainleft] = 0;
			motor[chainright] =0;
			wait1Msec(50);

			//lift moves back down
			motor[drfright] = 127;
			motor[drfleft] = 127;
			wait1Msec(225);

			motor[drfright] = 0;
			motor[drfleft] = 0;
			wait1Msec(50);

			//claw opens
			motor[claw] = -127;
			wait1Msec(200);

			motor[claw] = 0;
			wait1Msec(100);

			//lift moves back up
			motor[drfright] = -127;
			motor[drfleft] = -127;
			wait1Msec(425);

			motor[drfright] = 0;
			motor[drfleft] = 0;
			wait1Msec(50);

			//chain moves back over
			motor[chainleft] = -85;
			motor[chainright] = -85;
			wait1Msec(300);

			motor[chainleft] = 0;
			motor[chainright] =0;
			wait1Msec(75);

			//lift moves down
			motor[drfright] = 127;
			motor[drfleft] = 127;
			wait1Msec(650);

			motor[drfright] = 0;
			motor[drfleft] = 0;
			wait1Msec(50);

		}


		//pick up 4 anad 5 cones
		if(vexRT[Btn8R]==1) {
			//claw closes
			motor [claw] = 127;
			wait1Msec(200);

			motor[claw] = 0;
			wait1Msec(50);

			//lift moves up
			motor[drfright] = -127;
			motor[drfleft] = -127;
			wait1Msec(800);

			motor[drfright] = 0;
			motor[drfleft] = 0;
			wait1Msec(50);

			//chain moves over
			motor[chainleft] = 100;
			motor[chainright] =100;
			wait1Msec(700);

			motor[chainleft] = 0;
			motor[chainright] =0;
			wait1Msec(50);

			//lift moves back down
			motor[drfright] = 127;
			motor[drfleft] = 127;
			wait1Msec(150);

			motor[drfright] = 0;
			motor[drfleft] = 0;
			wait1Msec(50);

			//claw opens
			motor[claw] = -127;
			wait1Msec(200);

			motor[claw] = 0;
			wait1Msec(100);

			//lift moves back up
			motor[drfright] = -127;
			motor[drfleft] = -127;
			wait1Msec(400);

			motor[drfright] = 0;
			motor[drfleft] = 0;
			wait1Msec(50);

			//chain moves back over
			motor[chainleft] = -62;
			motor[chainright] = -62;
			wait1Msec(550);

			motor[chainleft] = 0;
			motor[chainright] =0;
			wait1Msec(75);

			//lift moves down
			motor[drfright] = 127;
			motor[drfleft] = 127;
			wait1Msec(900);

			motor[drfright] = 0;
			motor[drfleft] = 0;
			wait1Msec(50);


		}


		//pick up for next 3 cones
		if(vexRT[Btn8D]==1) {
			//claw closes
			motor [claw] = 127;
			wait1Msec(200);

			motor[claw] = 0;
			wait1Msec(50);

			//lift moves up
			motor[drfright] = -127;
			motor[drfleft] = -127;
			wait1Msec(1250);

			motor[drfright] = 0;
			motor[drfleft] = 0;
			wait1Msec(50);

			//chain moves over
			motor[chainleft] = 100;
			motor[chainright] =100;
			wait1Msec(700);

			motor[chainleft] = 0;
			motor[chainright] =0;
			wait1Msec(50);

			//lift moves back down
			motor[drfright] = 127;
			motor[drfleft] = 127;
			wait1Msec(110);

			motor[drfright] = 0;
			motor[drfleft] = 0;
			wait1Msec(50);

			//claw opens
			motor[claw] = -127;
			wait1Msec(200);

			motor[claw] = 0;
			wait1Msec(100);

			//lift moves back up
			motor[drfright] = -127;
			motor[drfleft] = -127;
			wait1Msec(850);

			motor[drfright] = 0;
			motor[drfleft] = 0;
			wait1Msec(50);

			//chain moves back over
			motor[chainleft] = -62;
			motor[chainright] = -62;
			wait1Msec(550);

			motor[chainleft] = 0;
			motor[chainright] =0;
			wait1Msec(100);

			//lift moves down
			motor[drfright] = 127;
			motor[drfleft] = 127;
			wait1Msec(1500);

			motor[drfright] = 0;
			motor[drfleft] = 0;
			wait1Msec(50);
		}


		//pick up for last cones
		if(vexRT[Btn8L]==1) {
			//claw closes
			motor [claw] = 127;
			wait1Msec(200);

			motor[claw] = 0;
			wait1Msec(50);

			//lift moves up
			motor[drfright] = -127;
			motor[drfleft] = -127;
			wait1Msec(1670);

			//chain moves over
			motor[chainleft] = 110;
			motor[chainright] =110;
			wait1Msec(900);


			motor[drfright] = 0;
			motor[drfleft] = 0;
			motor[chainleft] = 0;
			motor[chainright] =0;
			wait1Msec(50);


			//claw opens
			motor[claw] = -127;
			wait1Msec(200);

			motor[claw] = 0;
			wait1Msec(100);

		}

		//chain control
		if(vexRT[Btn7U]==1){

			motor [chainleft] = 127;
			motor [chainright] = 127;

			}else if (vexRT[Btn7D]==1){
			motor [chainleft] = -127;
			motor [chainright] = -127;


			}else{
			motor [chainleft] = 0;
			motor [chainright] = 0;

		}

		//claw control
		if(vexRT[Btn6D]==1) {
			motor[claw] = 127;

			}else if (vexRT[Btn6U] ==1) {
			motor[claw] = -127;

			}else{
			motor[claw] = 0;
		}


		//mobile goal control
		if(vexRT[Btn7R]==1){
			motor[mogo]=127;

			}else if (vexRT[Btn7L]==1){
			motor[mogo] = -127;

			}else{
			motor[mogo] = 0;

		}
	}
}