#include "vex.h"
vex::competition Competition;
vex::motor LeftMotor = vex::motor( vex::PORT11,true );
vex::motor BackLeftMotor = vex::motor( vex::PORT12,true );
//vex::motor_group LDrive = vex::motor_group(LeftMotor,BackLeftMotor);
vex::motor RightMotor = vex::motor( vex::PORT20,false );
vex::motor BackRightMotor = vex::motor( vex::PORT19,false );
//vex::motor_group RDrive = vex::motor_group(RightMotor,BackRightMotor);
//vex::motor_group Drive = vex::motor_group(LeftMotor,BackLeftMotor,RightMotor,BackRightMotor);
vex::motor Intake = vex::motor(vex::PORT17,true );
vex::motor Intake2 = vex::motor(vex::PORT14,false);
vex::motor_group Intakes = vex::motor_group(Intake,Intake2);
vex::motor Index = vex::motor(vex::PORT18,false );
vex::motor Index2 = vex::motor(vex::PORT13,false);
vex::motor_group Indexer = vex::motor_group(Index,Index2);
vex::controller Controller1 = vex::controller(vex::controllerType::primary);
vex::controller Controller2 = vex::controller(vex::controllerType::partner);
triport ThreeWirePort = vex::triport( vex::PORT22 );
vex::limit LimitSwitchA = vex::limit(ThreeWirePort.A);
vex::limit LimitSwitchB = vex::limit(ThreeWirePort.B);
void wait(float mSec){
vex::task::sleep(mSec);
}
int driveControlTask(){
while (Competition.isAutonomous()!=1){
LeftMotor.spin(vex::directionType::fwd, Controller1.Axis3.value() + Controller1.Axis1.value(), vex::velocityUnits::pct);
RightMotor.spin(vex::directionType::fwd, Controller1.Axis3.value() - Controller1.Axis1.value(), vex::velocityUnits::pct);
BackLeftMotor.spin(vex::directionType::fwd, Controller1.Axis3.value() + Controller1.Axis1.value(), vex::velocityUnits::pct);
BackRightMotor.spin(vex::directionType::fwd, Controller1.Axis3.value() - Controller1.Axis1.value(), vex::velocityUnits::pct);
wait(10);
} return(0);
}
void Indexcontroll()
{
if(Controller1.ButtonL2.pressing() == true)
{
Indexer.spin(vex::directionType::fwd,-100,vex::velocityUnits::pct);
Intakes.spin(vex::directionType::fwd,100,vex::velocityUnits::pct);
}
else if(Controller1.ButtonR2.pressing() == true)
{
Indexer.spin(vex::directionType::fwd,100,vex::velocityUnits::pct);
Intakes.spin(vex::directionType::fwd,-100,vex::velocityUnits::pct);
}
else
{
Indexer.stop(vex::brakeType::brake);
Intakes.stop(vex::brakeType::brake);
}
vex::task::sleep(15);
}
void AutoIndex()
{
if(Controller1.ButtonR1.pressing() == true)
{
Index.spin(vex::directionType::fwd,-100,vex::velocityUnits::pct);
Index2.spin(vex::directionType::fwd,100,vex::velocityUnits::pct);
}
else
{
Index.stop(vex::brakeType::brake);
Index2.stop(vex::brakeType::brake);
}
vex::task::sleep(15);
}
//////////////////
// LCD Commands //
//////////////////
int sq1StartX = 0;
int sq1SizeX = 80;
int sq1EndX = sq1StartX + sq1SizeX;
int sq1StartY = 10;
int sq1SizeY = 80;
int sq1EndY = sq1StartY + sq1SizeY;
int sq2StartX = 100;
int sq2SizeX = 80;
int sq2EndX = sq2StartX + sq2SizeX;
int sq2StartY = 10;
int sq2SizeY = 80;
int sq2EndY = sq2StartY + sq2SizeY;
int sq3StartX = 200;
int sq3SizeX = 80;
int sq3EndX = sq3StartX + sq3SizeX;
int sq3StartY = 10;
int sq3SizeY = 80;
int sq3EndY = sq3StartY + sq3SizeY;
int sq4StartX = 300;
int sq4SizeX = 80;
int sq4EndX = sq4StartX + sq4SizeX;
int sq4StartY = 10;
int sq4SizeY = 80;
int sq4EndY = sq4StartY + sq4SizeY;
int sq5StartX = 0;
int sq5SizeX = 80;
int sq5EndX = sq5StartX + sq5SizeX;
int sq5StartY = 100;
int sq5SizeY = 80;
int sq5EndY = sq5StartY + sq5SizeY;
int sq6StartX = 100;
int sq6SizeX = 80;
int sq6EndX = sq6StartX + sq6SizeX;
int sq6StartY = 100;
int sq6SizeY = 80;
int sq6EndY = sq6StartY + sq6SizeY;
int sq7StartX = 200;
int sq7SizeX = 80;
int sq7EndX = sq7StartX + sq7SizeX;
int sq7StartY = 100;
int sq7SizeY = 80;
int sq7EndY = sq7StartY + sq7SizeY;
int sq8StartX = 300;
int sq8SizeX = 80;
int sq8EndX = sq8StartX + sq8SizeX;
int sq8StartY = 100;
int sq8SizeY = 80;
int sq8EndY = sq8StartY + sq8SizeY;
int selected = 0;
void colorChanger(bool selection){
if (selection == true){Brain.Screen.setFillColor(vex::color::green);}
else {Brain.Screen.setFillColor(vex::color::purple);}
}
//Make sure lettering is to the correct square
//Make all autonomouses into voids and put them into the autonomos task
//
int LCD(){
Brain.Screen.setPenColor(vex::color::white);
Brain.Screen.setFillColor(vex::color::purple);
while (true){
if (selected == 1){colorChanger(true);} else {colorChanger(false);}
Brain.Screen.drawRectangle(sq1StartX, sq1StartY, sq1SizeX, sq1SizeY);
if (selected == 2){colorChanger(true);} else {colorChanger(false);}
Brain.Screen.drawRectangle(sq2StartX, sq2StartY, sq2SizeX, sq2SizeY);
if (selected == 3){colorChanger(true);} else {colorChanger(false);}
Brain.Screen.drawRectangle(sq3StartX, sq3StartY, sq3SizeX, sq3SizeY);
if (selected == 4){colorChanger(true);} else {colorChanger(false);}
Brain.Screen.drawRectangle(sq4StartX, sq4StartY, sq4SizeX, sq4SizeY);
if (selected == 5){colorChanger(true);} else {colorChanger(false);}
Brain.Screen.drawRectangle(sq5StartX, sq5StartY, sq5SizeX, sq5SizeY);
if (selected == 6){colorChanger(true);} else {colorChanger(false);}
Brain.Screen.drawRectangle(sq6StartX, sq6StartY, sq6SizeX, sq6SizeY);
if (selected == 7){colorChanger(true);} else {colorChanger(false);}
Brain.Screen.drawRectangle(sq7StartX, sq7StartY, sq7SizeX, sq7SizeY);
if (selected == 8){colorChanger(true);} else {colorChanger(false);}
Brain.Screen.drawRectangle(sq8StartX, sq8StartY, sq8SizeX, sq8SizeY);
int xPressed = Brain.Screen.xPosition();
int yPressed = Brain.Screen.yPosition();
if (Brain.Screen.pressing()){
if (xPressed > sq1StartX && xPressed < sq1EndX && yPressed > sq1StartY && yPressed < sq1EndY){selected = 1;}
if (xPressed > sq2StartX && xPressed < sq2EndX && yPressed > sq2StartY && yPressed < sq2EndY){selected = 2;}
if (xPressed > sq3StartX && xPressed < sq3EndX && yPressed > sq3StartY && yPressed < sq3EndY){selected = 3;}
if (xPressed > sq4StartX && xPressed < sq4EndX && yPressed > sq4StartY && yPressed < sq4EndY){selected = 4;}
if (xPressed > sq5StartX && xPressed < sq5EndX && yPressed > sq5StartY && yPressed < sq5EndY){selected = 5;}
if (xPressed > sq6StartX && xPressed < sq6EndX && yPressed > sq6StartY && yPressed < sq6EndY){selected = 6;}
if (xPressed > sq7StartX && xPressed < sq7EndX && yPressed > sq7StartY && yPressed < sq7EndY){selected = 7;}
if (xPressed > sq8StartX && xPressed < sq8EndX && yPressed > sq8StartY && yPressed < sq8EndY){selected = 8;}
Brain.Screen.setFillColor(vex::color::transparent);
Brain.Screen.printAt(sq1StartX,sq1StartY+20,true,"BL/RR");
Brain.Screen.printAt(sq1StartX,sq1StartY+40,true,"Sort");
Brain.Screen.printAt(sq2StartX,sq2StartY+20,true,"BR/RL");
Brain.Screen.printAt(sq2StartX,sq2StartY+40,true,"Sort");
Brain.Screen.printAt(sq3StartX,sq3StartY+20,true,"BL/RR");
Brain.Screen.printAt(sq3StartX,sq3StartY+40,true,"2Goal");
Brain.Screen.printAt(sq4StartX,sq4StartY+20,true,"BR/RL");
Brain.Screen.printAt(sq4StartX,sq4StartY+40,true,"2Goal");
Brain.Screen.printAt(sq5StartX,sq5StartY+20,true,"One");
Brain.Screen.printAt(sq5StartX,sq5StartY+40,true,"Ball");
Brain.Screen.printAt(sq6StartX,sq6StartY+20,true,"Skills");
Brain.Screen.printAt(sq6StartX,sq6StartY+40,true,"");
Brain.Screen.printAt(sq7StartX,sq7StartY+20,true,"BL/RR");
Brain.Screen.printAt(sq7StartX,sq7StartY+40,true,"Center");
Brain.Screen.printAt(sq8StartX,sq8StartY+20,true,"BR/RL");
Brain.Screen.printAt(sq8StartX,sq8StartY+40,true,"Center");
Brain.Screen.render();
}
wait(10);
} return(0);
}
void pre_auton( void ) {
vexcodeInit();
// All activities that occur before the competition starts
// Example: clearing encoders, setting servo positions, ...
vex::task LCDL(LCD);
}
void BL_RR_Sort(){//Blue Left, Red Right, Sorts corner goal
//Drive.rotateFor(900, vex::rotationUnits::raw, 50, vex::velocityUnits::pct,false);//Drives forward and picks up ball
Intakes.rotateFor(4000, vex::rotationUnits::raw, 100, vex::velocityUnits::pct,true);
//LDrive.rotateFor(-900, vex::rotationUnits::raw, 70, vex::velocityUnits::pct,false);//Angles towards goal
//RDrive.rotateFor(900, vex::rotationUnits::raw, 70, vex::velocityUnits::pct,true);
//Drive.rotateFor(900, vex::rotationUnits::raw, 50, vex::velocityUnits::pct,false);//Drives into goal to score/descore
Intakes.rotateFor(4000, vex::rotationUnits::raw, 100, vex::velocityUnits::pct,true);
Intakes.rotateFor(4000, vex::rotationUnits::raw, 100, vex::velocityUnits::pct,false);//score/descores
Indexer.rotateFor(1800, vex::rotationUnits::raw, 100 , vex::velocityUnits::pct,true);
//Drive.rotateFor(-900, vex::rotationUnits::raw, 50, vex::velocityUnits::pct,false);//Backs away from goal
Intakes.rotateFor(-4000, vex::rotationUnits::raw, 100, vex::velocityUnits::pct,true);
}
void BR_RL_Sort(){//Blue Right, Red Left, sort corner goal
//Drive.rotateFor(900, vex::rotationUnits::raw, 50, vex::velocityUnits::pct,false);//Drives forward and picks up ball
Intakes.rotateFor(4000, vex::rotationUnits::raw, 100, vex::velocityUnits::pct,true);
//LDrive.rotateFor(900, vex::rotationUnits::raw, 70, vex::velocityUnits::pct,false);//Angles towards goal
//RDrive.rotateFor(-900, vex::rotationUnits::raw, 70, vex::velocityUnits::pct,true);
//Drive.rotateFor(900, vex::rotationUnits::raw, 50, vex::velocityUnits::pct,false);//Drives into goal to score/descore
Intakes.rotateFor(4000, vex::rotationUnits::raw, 100, vex::velocityUnits::pct,true);
Intakes.rotateFor(4000, vex::rotationUnits::raw, 100, vex::velocityUnits::pct,false);//score/descores
Indexer.rotateFor(1800, vex::rotationUnits::raw, 100 , vex::velocityUnits::pct,true);
//Drive.rotateFor(-900, vex::rotationUnits::raw, 50, vex::velocityUnits::pct,false);//Backs away from goal
Intakes.rotateFor(-4000, vex::rotationUnits::raw, 100, vex::velocityUnits::pct,true);
}
void BL_RR_2G(){//Blue Left, Red Right, scores in corner and center goal
//Drive.rotateFor(900, vex::rotationUnits::raw, 50, vex::velocityUnits::pct,true);//Drives forward into goal
Index.rotateFor(1800, vex::rotationUnits::raw, 100 , vex::velocityUnits::pct,false);//scores
Index2.rotateFor(1800, vex::rotationUnits::raw, 100 , vex::velocityUnits::pct,true);
//Drive.rotateFor(-900, vex::rotationUnits::raw, 50, vex::velocityUnits::pct,false);//Backs away from goal
Intakes.rotateFor(-4000, vex::rotationUnits::raw, 100, vex::velocityUnits::pct,true);
//LDrive.rotateFor(900, vex::rotationUnits::raw, 70, vex::velocityUnits::pct,false);//Angles back towards wall
//RDrive.rotateFor(-900, vex::rotationUnits::raw, 70, vex::velocityUnits::pct,true);
//Drive.rotateFor(-900, vex::rotationUnits::raw, 50, vex::velocityUnits::pct,true);//Backs into wall
//Drive.rotateFor(900, vex::rotationUnits::raw, 50, vex::velocityUnits::pct,true);//Drives forward from wall
//LDrive.rotateFor(900, vex::rotationUnits::raw, 70, vex::velocityUnits::pct,false);//Angles toward ball
//RDrive.rotateFor(-900, vex::rotationUnits::raw, 70, vex::velocityUnits::pct,true);
//Drive.rotateFor(900, vex::rotationUnits::raw, 50, vex::velocityUnits::pct,false);//Drives forward to ball and intakes
Intakes.rotateFor(-4000, vex::rotationUnits::raw, 100, vex::velocityUnits::pct,true);
//LDrive.rotateFor(900, vex::rotationUnits::raw, 70, vex::velocityUnits::pct,false);//Angles toward goal and sets ball.
//RDrive.rotateFor(-900, vex::rotationUnits::raw, 100 , vex::velocityUnits::pct,false);
Indexer.rotateFor(1800, vex::rotationUnits::raw, 100 , vex::velocityUnits::pct,true);
Indexer.rotateFor(1800, vex::rotationUnits::raw, 100 , vex::velocityUnits::pct,true);//scores
//Drive.rotateFor(-900, vex::rotationUnits::raw, 100, vex::velocityUnits::pct,true);//Backs away from goal.
}
void BR_RL_2G(){//Blue Right, Red Left, scores corner and center goal
//Drive.rotateFor(900, vex::rotationUnits::raw, 50, vex::velocityUnits::pct,true);//Drives forward into goal
Index.rotateFor(1800, vex::rotationUnits::raw, 100 , vex::velocityUnits::pct,false);//scores
Index2.rotateFor(1800, vex::rotationUnits::raw, 100 , vex::velocityUnits::pct,true);
//Drive.rotateFor(-900, vex::rotationUnits::raw, 50, vex::velocityUnits::pct,false);//Backs away from goal
Intakes.rotateFor(-4000, vex::rotationUnits::raw, 100, vex::velocityUnits::pct,true);
//LDrive.rotateFor(-900, vex::rotationUnits::raw, 70, vex::velocityUnits::pct,false);//Angles back towards wall
//RDrive.rotateFor(900, vex::rotationUnits::raw, 70, vex::velocityUnits::pct,true);
//Drive.rotateFor(-900, vex::rotationUnits::raw, 50, vex::velocityUnits::pct,true);//Backs into wall
//Drive.rotateFor(900, vex::rotationUnits::raw, 50, vex::velocityUnits::pct,true);//Drives forward from wall
//LDrive.rotateFor(-900, vex::rotationUnits::raw, 70, vex::velocityUnits::pct,false);//Angles toward ball
//RDrive.rotateFor(900, vex::rotationUnits::raw, 70, vex::velocityUnits::pct,true);
//Drive.rotateFor(900, vex::rotationUnits::raw, 50, vex::velocityUnits::pct,false);//Drives forward to ball and intakes
Intakes.rotateFor(-4000, vex::rotationUnits::raw, 100, vex::velocityUnits::pct,true);
//LDrive.rotateFor(-900, vex::rotationUnits::raw, 70, vex::velocityUnits::pct,false);//Angles toward goal and sets ball.
//RDrive.rotateFor(900, vex::rotationUnits::raw, 100 , vex::velocityUnits::pct,false);
Indexer.rotateFor(1800, vex::rotationUnits::raw, 100 , vex::velocityUnits::pct,true);
Indexer.rotateFor(1800, vex::rotationUnits::raw, 100 , vex::velocityUnits::pct,true);//scores
//Drive.rotateFor(-900, vex::rotationUnits::raw, 100, vex::velocityUnits::pct,true);//Backs away from goal.
}
void Skills(){
}
void OneBall(){
}
void BL_RR_CG(){//Blue Left, Red Right, scores in corner and back center goal
//Drive.rotateFor(900, vex::rotationUnits::raw, 50, vex::velocityUnits::pct,false);//Drives forward and picks up ball
Intakes.rotateFor(4000, vex::rotationUnits::raw, 100, vex::velocityUnits::pct,true);
//LDrive.rotateFor(-900, vex::rotationUnits::raw, 70, vex::velocityUnits::pct,false);//Angles towards goal
//RDrive.rotateFor(900, vex::rotationUnits::raw, 70, vex::velocityUnits::pct,true);
//Drive.rotateFor(900, vex::rotationUnits::raw, 50, vex::velocityUnits::pct,true);//Drives forward into goal
Indexer.rotateFor(1800, vex::rotationUnits::raw, 100 , vex::velocityUnits::pct,true);//Scores
//Drive.rotateFor(-900, vex::rotationUnits::raw, 50, vex::velocityUnits::pct,true);//Back out of goal
//LDrive.rotateFor(900, vex::rotationUnits::raw, 50, vex::velocityUnits::pct,false);//Turns back side towards wall
//RDrive.rotateFor(-900, vex::rotationUnits::raw, 50, vex::velocityUnits::pct,true);
//Drive.rotateFor(-900, vex::rotationUnits::raw, 50, vex::velocityUnits::pct,true);//Backs into wall
//Drive.rotateFor(900, vex::rotationUnits::raw, 50, vex::velocityUnits::pct,false);//Drive forward to pick up ball on
Intakes.rotateFor(4000, vex::rotationUnits::raw, 100, vex::velocityUnits::pct,true);//line and intakes it
}
void BR_RL_CG(){//Blue Right, Red Left, scores in corner and back center goal.
//Drive.rotateFor(900, vex::rotationUnits::raw, 50, vex::velocityUnits::pct,false);//Drives forward and picks up ball
Intakes.rotateFor(4000, vex::rotationUnits::raw, 100, vex::velocityUnits::pct,true);
//LDrive.rotateFor(900, vex::rotationUnits::raw, 70, vex::velocityUnits::pct,false);//Angles towards goal
//RDrive.rotateFor(-900, vex::rotationUnits::raw, 70, vex::velocityUnits::pct,true);
//Drive.rotateFor(900, vex::rotationUnits::raw, 50, vex::velocityUnits::pct,true);//Drives forward into goal
Indexer.rotateFor(1800, vex::rotationUnits::raw, 100 , vex::velocityUnits::pct,true);//Scores
//Drive.rotateFor(-900, vex::rotationUnits::raw, 50, vex::velocityUnits::pct,true);//Back out of goal
//LDrive.rotateFor(-900, vex::rotationUnits::raw, 50, vex::velocityUnits::pct,false);//Turns back side towards wall
//RDrive.rotateFor(900, vex::rotationUnits::raw, 50, vex::velocityUnits::pct,true);
//Drive.rotateFor(-900, vex::rotationUnits::raw, 50, vex::velocityUnits::pct,true);//Backs into wall
//Drive.rotateFor(900, vex::rotationUnits::raw, 50, vex::velocityUnits::pct,false);//Drive forward to pick up ball on
Intakes.rotateFor(4000, vex::rotationUnits::raw, 100, vex::velocityUnits::pct,true);//line and intakes it
}
void autonomous( void ) {
if (selected == 1){BL_RR_Sort();}
if (selected == 2){BR_RL_Sort();}
if (selected == 3){BL_RR_2G();}
if (selected == 4){BR_RL_2G();}
if (selected == 5){OneBall();}
if (selected == 6){Skills();}
if (selected == 7){BL_RR_CG();}
if (selected == 8){BR_RL_CG();}
}
void usercontrol( void ) {
vex::task driveControl(driveControlTask);
while (1) {
Indexcontroll();
AutoIndex();
// 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.
// ........................................................................
vex::task::sleep(20); //Sleep the task for a short amount of time to prevent wasted resources.
}
}
//
// Main will set up the competition functions and callbacks.
//
int main() {
//Run the pre-autonomous function.
pre_auton();
//Set up callbacks for autonomous and driver control periods.
Competition.autonomous( autonomous );
Competition.drivercontrol( usercontrol );
//Prevent main from exiting with an infinite loop.
while(1) {
vex::task::sleep(100);//Sleep the task for a short amount of time to prevent wasted resources.
}
}