SkillsCompetitionProgramming.c

#pragma config(Sensor, in7,    lineFollower,   sensorLineFollower)
#pragma config(Sensor, in8,    gyro,           sensorGyro)
#pragma config(Sensor, dgtl1,  rightEncoder,   sensorQuadEncoder)
#pragma config(Motor,  port2,           rightBack,     tmotorVex393HighSpeed_MC29, openLoop, reversed)
#pragma config(Motor,  port3,           rightFront,    tmotorVex393HighSpeed_MC29, openLoop, reversed)
#pragma config(Motor,  port4,           leftBack,      tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port5,           leftFront,     tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port6,           mobileGoal,    tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port7,           mobileGoal2,   tmotorVex393HighSpeed_MC29, openLoop, reversed)
#pragma config(Motor,  port8,           rightMid,      tmotorVex393HighSpeed_MC29, openLoop, reversed)
#pragma config(Motor,  port9,           leftMid,       tmotorVex393HighSpeed_MC29, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

void moveStraight(int speed) {
	motor[rightBack] = speed;
	motor[rightFront] = speed;
	motor[rightMid] = speed;
	motor[leftBack] = speed;
	motor[leftFront] = speed;
	motor[leftMid] = speed;
}

void turnRight(int speed) {
	motor[leftBack] = speed;
	motor[leftFront] = speed;
	motor[leftMid] = speed;
	motor[rightBack] = -speed;
	motor[rightFront] = -speed;
	motor[rightMid] = -speed;
}

void turnLeft(int speed) {
	motor[rightBack] = speed;
	motor[rightFront] = speed;
	motor[rightMid] = speed;
	motor[leftBack] = -speed;
	motor[leftFront] = -speed;
	motor[leftMid] = -speed;
}

void moveLift(int speed) {
	motor[mobileGoal] = speed;
	motor[mobileGoal2] = speed;
}

//Code for VEX 11040B
task main()
{
	/* INFORMATION
	* Time Needed to Move Mobile Goal Lift:
	*/

	//Pre-autonomous
	SensorValue[rightEncoder] = 0;
	SensorType[in1] = sensorNone;
	wait1Msec(1000);
	SensorType[in1] = sensorGyro;
	SensorValue[gyro] = 0;

	//Autonomous

	/* FIRST MOBILE GOAL */

	// Mobile Goal Lift Down
	moveLift(127);
	wait1Msec(950);
	moveLift(0);

	//Move Forward a bit
	moveStraight(127);
	wait1Msec(1500);
	moveStraight(0);

	//Move Towards First Mobile Goal
	while (SensorValue(lineFollower) < 450) {
		moveStraight(127);
	}
	//Move a bit after it hits line
	wait1Msec(600);
	moveStraight(0);

	//Pick Up First Mobile Goal
	moveLift(-127);
	wait1Msec(1450);
	moveLift(0);

	//Move back over white line
	moveStraight(-127);
	wait1Msec(1500);
	moveStraight(0);

	//Move back until it hits line
	while (SensorValue(lineFollower) < 450) {
		moveStraight(-127);
	}
	moveStraight(-80);
	wait1Msec(300);
	moveStraight(0);

	//Turn Right
	SensorValue[gyro] = 0;
	while (abs(SensorValue[gyro]) < 1200) {
			motor[leftBack] = 127;
			motor[leftFront] = 127;
			motor[leftMid] = 127;
			motor[rightBack] = -40;
			motor[rightFront] = -40;
			motor[rightMid] = -40;
	}
	moveStraight(0);

	//Move Forward
	moveStraight(127);
	wait1Msec(670);
	moveStraight(0);

	//Turn Right
	SensorValue[gyro] = 0;
	while (abs(SensorValue[gyro]) < 330) {
		turnRight(100);
	}
	turnRight(0);



	//Move Forward into 20 point zone
	moveStraight(127);
	wait1Msec(690);
	moveStraight(0);
//Lower Lift a little
	moveLift(127);
	wait1Msec(330);
	moveLift(0);
	//ram into 20 pt zone
	moveStraight(127);
	wait1Msec(400);
	moveStraight(0);


	//Move backward a bit
	moveStraight(-127);
	wait1Msec(1200);
	moveStraight(0);

	//Move backward until white line
	while (SensorValue(lineFollower) < 450) {
		moveStraight(-127);
	}
	moveStraight(0);

	//Move forward a bit
	moveStraight(127);
	wait1Msec(300);
	moveStraight(0);

	//Turn Right
	SensorValue[gyro] = 0;
	while(abs(SensorValue[gyro]) < 450) {
		turnRight(127);
	}
	turnRight(0);

	//Move Forward
	moveStraight(127);
	wait1Msec(900);
	moveStraight(0);

	//Turn Right
	SensorValue[gyro] = 0;
	while (abs(SensorValue[gyro]) < 275) {
		turnRight(127);
	}
	turnRight(0);

	//Lower Mobile Goal Lift
	moveLift(127);
	wait1Msec(570);
	moveLift(0);

	//Grab second mobile goal
	moveStraight(127);
	wait1Msec(1000);
	moveStraight(0);

	//Raise mobile goal
	moveLift(-127);
	wait1Msec(1300);
	moveLift(0);

	//Turn around
	SensorValue[gyro] = 0;
	while (abs(SensorValue[gyro]) < 1600) {
		turnLeft(127);
	}
	turnLeft(0);

	//Lower lift a little
	moveLift(127);
	wait1Msec(400);
	moveLift(0);


	//Move into 10 point zone
	moveStraight(127);
	wait1Msec(1400);
	moveStraight(0);

	//Move back into white line
	while(SensorValue(lineFollower) < 450) {
		moveStraight(-127);
	}
	moveStraight(0);

	//Turn Left to head towards third goal
	SensorValue[gyro] = 0;
	while (abs(SensorValue[gyro]) < 390) {
		turnLeft(127);
	}
	turnLeft(0);

	//Move forward
	moveStraight(127);
	wait1Msec(700);
	moveStraight(0);

	//Turn Left
	SensorValue[gyro] = 0;
	while(abs(SensorValue[gyro]) < 450) {
		turnLeft(127);
	}
	turnLeft(0);

	//Lower Mobile Goal Lift
	moveLift(127);
	wait1Msec(700);
	moveLift(0);

	//Move toward 3rd Mobile Goal
	moveStraight(127);
	wait1Msec(1200);
	moveStraight(0);

	//Raise Mobile Goal Lift
	moveLift(-127);
	wait1Msec(1000);
	moveLift(0);

	//Turn Around
	SensorValue[gyro] = 0;
	while(abs(SensorValue[gyro]) < 1450) {
		turnRight(127);
	}
	turnRight(0);

	//Lower lift a little
	moveLift(127);
	wait1Msec(350);
	moveLift(0);

	//Move into 10 point zone
	moveStraight(127);
	wait1Msec(1500);
	moveStraight(0);

	//Move back into white line
	while(SensorValue(lineFollower) < 450) {
		moveStraight(-127);
	}
	moveStraight(0);

}