#pragma config(Sensor, in1, rightLiftS, sensorPotentiometer)

#pragma config(Sensor, in2, progSel, sensorPotentiometer)

#pragma config(Sensor, dgtl1, rightBackS, sensorQuadEncoder)

#pragma config(Sensor, dgtl3, leftBackS, sensorQuadEncoder)

#pragma config(Sensor, dgtl5, bumper, sensorTouch)

#pragma config(Motor, port1, rightBack, tmotorVex393_HBridge, openLoop)

#pragma config(Motor, port2, rightFront, tmotorVex393_MC29, openLoop)

#pragma config(Motor, port3, rightArmTop, tmotorVex393_MC29, openLoop, reversed)

#pragma config(Motor, port4, rightArmBottom, tmotorVex393_MC29, openLoop, reversed)

#pragma config(Motor, port5, intakeRight, tmotorVex393_MC29, openLoop)

#pragma config(Motor, port6, intakeLeft, tmotorVex393_MC29, openLoop, reversed)

#pragma config(Motor, port7, leftArmBottom, tmotorVex393_MC29, openLoop)

#pragma config(Motor, port8, leftArmTop, tmotorVex393_MC29, openLoop)

#pragma config(Motor, port9, leftFront, tmotorVex393_MC29, openLoop, reversed)

#pragma config(Motor, port10, leftBack, tmotorVex393_HBridge, openLoop)

//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//

#pragma platform(VEX)

//Competition Control and Duration Settings

#pragma competitionControl(Competition)

#pragma autonomousDuration(20)

#pragma userControlDuration(120)

#include "Vex_Competition_Includes.c" //Main competition background code...do not modify!

int cubicMap(int x)

{

return ((((x*3)/25)*((x*3)/25)*((x*3)/25)/27 + x/2)*2)/3;

}

void moveStraight(int power)

{

motor[leftFront] = power;

motor[leftBack] = power;

motor[rightFront] = power;

motor[rightBack] = power;

}

// Stops the base motors

void baseStop()

{

motor[leftFront] = 0;

motor[leftBack] = 0;

motor[rightBack] = 0;

motor[rightFront] = 0;

}

// Sets the motor speeds to the assigned value, depending on the direction assigned.

// If direction is 1, left motors will be set with negative power, whereas right motors will be set with positive power

// This allows the robot to turn left.

/// If direction is 2, it will cause the robot to turn right.

void turnOnSpot(int direction, int power)

{

if (direction == 1)

{

motor[leftFront] = -power;

motor[leftBack] = -power;

motor[rightFront] = power;

motor[rightBack] = power;

}

else if (direction == 2)

{

motor[leftFront] = power;

motor[leftBack] = power;

motor[rightFront] = -power;

motor[rightBack] = -power;

}

}

// Sets the lift motors' speed to the assigned value

void moveLift(int power)

{

motor[leftArmTop] = power;

motor[leftArmBottom] = power;

motor[rightArmTop] = power;

motor[rightArmBottom] = power;

}

// Stops all the lift motors

void liftStop()

{

motor[leftArmTop] = 0;

motor[leftArmBottom] = 0;

motor[rightArmTop] = 0;

motor[rightArmBottom] = 0;

}

// Sets the lifts to move at 127 in the direction desired until height desired is reached

// If direction is 1, while the height is not reached, the lift motors will move up and stop when it is reached.

// Similarly, if direction is -1 , while the height is not reached, the lift motors will cause the robot to go down until the desired height is reached.

void moveLiftAuto(int direction, int height)

{

if (direction == 1)

{

while (SensorValue[rightLiftS] > height)

{

motor[leftArmTop] = 90;

motor[leftArmBottom] = 90;

motor[rightArmTop] = 90;

motor[rightArmBottom] = 90;

}

motor[leftArmTop] = 0;

motor[leftArmBottom] = 0;

motor[rightArmTop] = 0;

motor[rightArmBottom] = 0;

}

else if (direction == -1)

{

while (SensorValue[rightLiftS] < height)

{

motor[leftArmTop] = -90;

motor[leftArmBottom] = -90;

motor[rightArmTop] = -90;

motor[rightArmBottom] = -90;

}

motor[leftArmTop] = 0;

motor[leftArmBottom] = 0;

motor[rightArmTop] = 0;

motor[rightArmBottom] = 0;

}

}

// Sets the value of the intake motors according to the direction determined. 1 for intake, -1 for output.

// Direction 2 and -2 is for testing purposes and is for turning both motors in the same direction.

void intake(int direction)

{

if (direction == 1)

{

motor[intakeLeft] = 127;

motor[intakeRight] = 127;

}

else if (direction == -1)

{

motor[intakeLeft] = -127;

motor[intakeRight] = -127;

}

else if (direction == 2)

{

motor[intakeLeft] = 127;

motor[intakeRight] = -127;

}

else if (direction == -2)

{

motor[intakeLeft] = -127;

motor[intakeRight] = 127;

}

else

{

motor[intakeLeft] = 0;

motor[intakeRight] = 0;

}

}

// Sets the right base motors to the desired speed.

void moveRightSide(int power)

{

motor[rightBack] = power;

motor[rightFront] = power;

}

// Sets the left base motors to the desired speed.

void moveLeftSide(int power)

{

motor[leftBack] = power;

motor[leftFront] = power;

}

/*

// This is a programming function, preset to allow the robot to score the bucky balls as well as the large balls

// when Buckyball count = 3, large ball count = 1

void buckyLargeOutput()

{

// At the start, it outputs the bucky ball at the mouth of the intake

intake(-1);

wait10Msec(30);

// The robot intakes, making sure the large ball does not get disposed from the intake

intake(1);

wait10Msec(70);

intake(-1);

wait10Msec(50);

intake(1);

wait10Msec(90);

intake(-1);

wait10Msec(50);

intake(0);

// The process continues until the three bucky balls are scored.

// The robot then moves backwards, giving allowance for the large ball to be scored.

nMotorEncoder[leftmid] = 0;

while(abs(nMotorEncoder[leftmid]) < 120) moveStraight(-80);

moveStraight(20);

wait10Msec(20);

baseStop();

intake(-1);

wait10Msec(100);

}

*/

void deploy()

{

intake(-1);

wait10Msec(70);

intake(0);

}

void blueSkyriseAuton()

{

//while (SensorValue[bumper] == 0) {}

deploy();

intake(1);

SensorValue[rightBackS] = 0;

while(abs(SensorValue[rightBackS]) < 200)

{

moveStraight(127);

}

baseStop();

wait1Msec(1000);

SensorValue[rightBackS] = 0;

SensorValue[leftBackS] = 0;

while(((abs(SensorValue[rightBackS]) + abs(SensorValue[leftBackS]))/2) < 500)

{

turnOnSpot(1,90);

}

baseStop();

intake(-1);

wait1Msec(5000);

intake(0);

SensorValue[rightBackS] = 0;

while(abs(SensorValue[rightBackS]) < 100)

{

moveStraight(-90);

}

baseStop();

/*

while (SensorValue[bumper] == 0) {}

SensorValue[rightBackS] = 0;

while (abs(SensorValue[rightBackS]) < 70) moveStraight(-90);

baseStop();

// Deploy

deploy();

//Raise Lift

moveLiftAuto(1,3500);

intake(1);

moveLift(-1);

wait1Msec(300);

moveLift(0);

moveStraight(127);

wait1Msec(1000);

baseStop();

moveLiftAuto(-1,3450);

wait1Msec(500);

moveLiftAuto(1,3000);

SensorValue[rightBackS] = 0;

while (abs(SensorValue[rightBackS]) < 200)

{

moveStraight(-60);

}

baseStop();

SensorValue[rightBackS] = 0;

SensorValue[leftBackS] = 0;

while(((abs(SensorValue[rightBackS])+abs(SensorValue[leftBackS]))/2) < 550)

{

moveRightSide(-90);

}

SensorValue[rightBackS] = 0;

while (abs(SensorValue[rightBackS]) < 180)

{

moveStraight(70);

}

baseStop();

moveLiftAuto(-1,3550);

moveStraight(30);

moveLiftAuto(-1,3900);

while(abs(SensorValue[rightBackS]) < 200)

{

moveStraight(-60);

}

baseStop();

//Move Forward

//Move Backward

//Turn around

//Drop Lift

//Score Skyrise*/

}

void blueOppoAuton()

{

//while (SensorValue[bumper] == 0) {}

SensorValue[rightBackS] = 0;

while (abs(SensorValue[rightBackS]) < 70) moveStraight(-90);

baseStop();

// Deploy

deploy();

moveLiftAuto(1,3100);

moveLift(20);

SensorValue[rightBackS] = 0;

while (abs(SensorValue[rightBackS]) < 40) moveStraight(40);

baseStop();

/*

moveStraight(127);

wait1Msec(200);

moveStraight(-127);

wait1Msec(200);

*/

moveStraight(127);

wait1Msec(200);

intake(-1);

moveStraight(-127);

wait1Msec(200);

baseStop();

//intake(-1);

wait1Msec(2000);

moveStraight(50);

wait1Msec(100);

baseStop();

intake(1);

moveLift(-30);

wait1Msec(500);

intake(0);

liftStop();

moveStraight(-40);

wait1Msec(200);

baseStop();

SensorValue[leftBackS] = 0;

SensorValue[rightBackS] = 0;

while (((abs(SensorValue[leftBackS])+abs(SensorValue[rightBackS]))/2) < 450)

{

while (SensorValue[rightLiftS] < 4000)

{

moveLift(-70);

turnOnSpot(2,90);

}

liftStop();

}

baseStop();

intake(1);

moveStraight(70);

wait1Msec(1000);

baseStop();

// Raise Lift

// Output

}

void redSkyriseAuton()

{

//while (SensorValue[bumper] == 0) {}

deploy();

intake(1);

SensorValue[rightBackS] = 0;

while(abs(SensorValue[rightBackS]) < 200)

{

moveStraight(127);

}

baseStop();

wait1Msec(1000);

SensorValue[rightBackS] = 0;

SensorValue[leftBackS] = 0;

while(((abs(SensorValue[rightBackS]) + abs(SensorValue[leftBackS]))/2) < 500)

{

turnOnSpot(1,90);

}

baseStop();

intake(-1);

wait1Msec(5000);

intake(0);

SensorValue[rightBackS] = 0;

while(abs(SensorValue[rightBackS]) < 100)

{

moveStraight(-90);

}

baseStop();

/*

SensorValue[rightBackS] = 0;

while (abs(SensorValue[rightBackS]) < 70) moveStraight(-90);

baseStop();

// Deploy

deploy();

//Raise Lift

moveLiftAuto(1,3500);

intake(1);

moveLift(-1);

wait1Msec(300);

moveLift(0);

moveStraight(127);

wait1Msec(1000);

baseStop();

moveLiftAuto(-1,3450);

wait1Msec(500);

moveLiftAuto(1,3000);

SensorValue[rightBackS] = 0;

while (abs(SensorValue[rightBackS]) < 200)

{

moveStraight(-60);

}

baseStop();

SensorValue[rightBackS] = 0;

SensorValue[leftBackS] = 0;

while(((abs(SensorValue[rightBackS])+abs(SensorValue[leftBackS]))/2) < 480)

{

moveLeftSide(-90);

}

moveLeftSide(1);

wait1Msec(50);

baseStop();

SensorValue[rightBackS] = 0;

while (abs(SensorValue[rightBackS]) < 180)

{

moveStraight(70);

}

baseStop();

moveLiftAuto(-1,3550);

moveStraight(30);

moveLiftAuto(-1,3900);

while(abs(SensorValue[rightBackS]) < 200)

{

moveStraight(-60);

}

baseStop();

//Move Forward

//Move Backward

//Turn around

//Drop Lift

//Score Skyrise*/

}

void redOppoAuton()

{

//while (SensorValue[bumper] == 0) {}

SensorValue[rightBackS] = 0;

while (abs(SensorValue[rightBackS]) < 70) moveStraight(-90);

baseStop();

// Deploy

deploy();

moveLiftAuto(1,3100);

moveLift(20);

SensorValue[rightBackS] = 0;

while (abs(SensorValue[rightBackS]) < 40) moveStraight(40);

baseStop();

/*

moveStraight(127);

wait1Msec(200);

moveStraight(-127);

wait1Msec(200);

*/

moveStraight(127);

wait1Msec(200);

intake(-1);

moveStraight(-127);

wait1Msec(200);

baseStop();

//intake(-1);

wait1Msec(2000);

moveStraight(50);

wait1Msec(100);

baseStop();

intake(1);

moveLift(-30);

wait1Msec(500);

intake(0);

liftStop();

moveStraight(-40);

wait1Msec(200);

baseStop();

SensorValue[leftBackS] = 0;

SensorValue[rightBackS] = 0;

while (((abs(SensorValue[leftBackS])+abs(SensorValue[rightBackS]))/2) < 450)

{

while (SensorValue[rightLiftS] < 4000)

{

moveLift(-70);

turnOnSpot(1,90);

}

liftStop();

}

baseStop();

intake(1);

moveStraight(70);

wait1Msec(1000);

baseStop();

// Raise Lift

// Output

}

void progSkills()

{

//while (SensorValue[bumper] == 0) {}

SensorValue[rightBackS] = 0;

while (abs(SensorValue[rightBackS]) < 70) moveStraight(-90);

baseStop();

// Deploy

deploy();

moveLiftAuto(1,3100);

moveLift(20);

SensorValue[rightBackS] = 0;

while (abs(SensorValue[rightBackS]) < 40) moveStraight(40);

baseStop();

/*

moveStraight(127);

wait1Msec(200);

moveStraight(-127);

wait1Msec(200);

*/

moveStraight(127);

wait1Msec(200);

intake(-1);

moveStraight( -127);

wait1Msec(200);

baseStop();

//intake(-1);

wait1Msec(2000);

moveStraight(50);

wait1Msec(100);

baseStop();

intake(1);

moveLift(-30);

wait1Msec(500);

intake(0);

liftStop();

moveStraight(-40);

wait1Msec(200);

baseStop();

SensorValue[leftBackS] = 0;

SensorValue[rightBackS] = 0;

while (((abs(SensorValue[leftBackS])+abs(SensorValue[rightBackS]))/2) < 450)

{

while (SensorValue[rightLiftS] < 4000)

{

moveLift(-70);

turnOnSpot(2,90);

}

liftStop();

}

baseStop();

intake(1);

moveStraight(70);

wait1Msec(1000);

baseStop();

SensorValue[leftBackS] = 0;

SensorValue[rightBackS] = 0;

while (((abs(SensorValue[leftBackS])+abs(SensorValue[rightBackS]))/2) < 500)

{

turnOnSpot(2,90);

}

baseStop();

moveStraight(127);

wait1Msec(2000);

baseStop();

while(SensorValue[bumper] == 0) {}

intake(-1);

while(SensorValue[bumper] == 0) {}

intake(0);

// Raise Lift

// Output

while(SensorValue[bumper] == 0) {}

intake (1);

moveStraight(127);

wait10Msec(50);

turnOnSpot(1,60);

wait10Msec(30);

turnOnSpot(2,60);

wait10Msec(30);

baseStop();

SensorValue[leftBackS] = 0;

SensorValue[rightBackS] = 0;

while (((abs(SensorValue[leftBackS])+abs(SensorValue[rightBackS]))/2) < 500)

{

turnOnSpot(1,90);

}

intake(-1);

wait10Msec(100);

}

void autonLocation()

{

if (SensorValue[progSel] < 500)

{

blueSkyriseAuton();

}

else if (SensorValue[progSel] < 2148)

{

blueOppoAuton();

}

else if (SensorValue[progSel] < 3300)

{

redSkyriseAuton();

}

else

{

redOppoAuton();

}

}

/////////////////////////////////////////////////////////////////////////////////////////

//

// 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()

{

// .....................................................................................

// Insert user code here.

// .....................................................................................

progSkills();

//autonLocation(); // Remove this function call once you have "real" code.

}

/////////////////////////////////////////////////////////////////////////////////////////

//

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

bool arcade = false;

while (true)

{

// This is the main execution loop for the user control program. Each time through the loop

// your program should update motor + servo values based on feedback from the joysticks.

// .....................................................................................

// Insert user code here. This is where you use the joystick values to update your motors, etc.

// .....................................................................................

if (vexRT[Btn7U] == 1)

{

progSkills();

}

if (vexRT[Btn8U] == 1)

{

arcade = true;

}

else if (vexRT[Btn8D] == 1)

{

arcade = false;

}

if (arcade == true)

{

motor[leftBack] = cubicMap(vexRT[Ch3] + vexRT[Ch1]);

motor[leftFront] = cubicMap(vexRT[Ch3] + vexRT[Ch1]);

motor[rightBack] = cubicMap(vexRT[Ch3] - vexRT[Ch1]);

motor[rightFront] = cubicMap(vexRT[Ch3] - vexRT[Ch1]);

}

else

{

motor[leftBack] = vexRT[Ch3];

motor[leftFront] = vexRT[Ch3];

motor[rightBack] = vexRT[Ch2];

motor[rightFront] = vexRT[Ch2];

}

if (abs(vexRT[Ch2Xmtr2]) > 10)

{

motor[leftArmBottom] = cubicMap(vexRT[Ch2Xmtr2]);

motor[leftArmTop] = cubicMap(vexRT[Ch2Xmtr2]);

motor[rightArmBottom] = cubicMap(vexRT[Ch2Xmtr2]);

motor[rightArmTop] = cubicMap(vexRT[Ch2Xmtr2]);

}

else if (vexRT[Btn6U] == 1)

{

motor[leftArmBottom] = 127;

motor[leftArmTop] = 127;

motor[rightArmBottom] = 127;

motor[rightArmTop] = 127;

}

else if (vexRT[Btn6D] == 1)

{

motor[leftArmBottom] = -127;

motor[leftArmTop] = -127;

motor[rightArmBottom] = -127;

motor[rightArmTop] = -127;

}

else

{

motor[leftArmBottom] = 0;

motor[leftArmTop] = 0;

motor[rightArmBottom] = 0;

motor[rightArmTop] = 0;

}

if (abs(vexRT[Ch3Xmtr2]) > 10)

{

motor[intakeLeft] = vexRT[Ch3Xmtr2];

motor[intakeRight] = vexRT[Ch3Xmtr2];

}

else if (vexRT[Btn5U] == 1)

{

motor[intakeLeft] = 127;

motor[intakeRight] = 127;

}

else if (vexRT[Btn5D] == 1)

{

motor[intakeLeft] = -127;

motor[intakeRight] = -127;

}

}

}