#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, in1, leftpot, sensorNone)
#pragma config(Sensor, in2, rightpot, sensorPotentiometer)
#pragma config(Sensor, dgtl1, shootlimit, sensorTouch)
#pragma config(Sensor, dgtl2, backpnu, sensorDigitalOut)
#pragma config(Sensor, dgtl3, frontpnu, sensorDigitalOut)
#pragma config(Sensor, I2C_1, shooter1, sensorQuadEncoderOnI2CPort, , AutoAssign )
#pragma config(Sensor, I2C_2, shooter2, sensorNone)
#pragma config(Sensor, I2C_3, shooter3, sensorNone)
#pragma config(Sensor, I2C_4, shooter4, sensorNone)
#pragma config(Motor, port1, leftlift, tmotorVex393_HBridge, openLoop)
#pragma config(Motor, port2, base1, tmotorVex393HighSpeed_MC29, openLoop, reversed)
#pragma config(Motor, port3, base2, tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor, port4, base3, tmotorVex393HighSpeed_MC29, openLoop, reversed)
#pragma config(Motor, port5, base4, tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor, port6, frontr, tmotorVex393_MC29, PIDControl, reversed, encoderPort, I2C_1)
#pragma config(Motor, port7, backr, tmotorVex393_MC29, PIDControl, encoderPort, I2C_1)
#pragma config(Motor, port8, frontl, tmotorVex393_MC29, PIDControl, encoderPort, I2C_1)
#pragma config(Motor, port9, backl, tmotorVex393_MC29, PIDControl, reversed, encoderPort, I2C_1)
#pragma config(Motor, port10, rightlift, tmotorVex393_HBridge, openLoop)
//!!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. /
/---------------------------------------------------------------------------*/
bool killvar = false;//kill switch var
const short leftButton = 1;
const short centerButton = 2;
const short rightButton = 4;
int AtonNumber = 0;
//Wait for Press--------------------------------------------------
void waitForPress()
{
while(nLCDButtons == 0){}
wait1Msec(5);
}
//----------------------------------------------------------------
//Wait for Release------------------------------------------------
void waitForRelease()
{
while(nLCDButtons != 0){}
wait1Msec(5);
}
//----------------------------------------------------------------
void pre_auton()
{
bStopTasksBetweenModes = true;
//Declare count variable to keep track of our choice
int count = 0;
//------------- Beginning of User Interface Code ---------------
//Clear LCD
clearLCDLine(0);
clearLCDLine(1);
//Loop while center button is not pressed
while(nLCDButtons != centerButton)
{
if(SensorValue[killvar]==1){killvar=true;}while(killvar==true){sleep(10);}//kill switch
//Switch case that allows the user to choose from 4 different options
switch(count){
case 0:
//Display first choice
displayLCDCenteredString(0, "Autonomous 1");
displayLCDCenteredString(1, "< Enter >");
waitForPress();
//Increment or decrement "count" based on button press
if(nLCDButtons == leftButton)
{
waitForRelease();
count = 3;
}
else if(nLCDButtons == rightButton)
{
waitForRelease();
count++;
}
break;
case 1:
//Display second choice
displayLCDCenteredString(0, "Autonomous 2");
displayLCDCenteredString(1, "< Enter >");
waitForPress();
//Increment or decrement "count" based on button press
if(nLCDButtons == leftButton)
{
waitForRelease();
count--;
}
else if(nLCDButtons == rightButton)
{
waitForRelease();
count++;
}
break;
case 2:
//Display third choice
displayLCDCenteredString(0, "Autonomous 3");
displayLCDCenteredString(1, "< Enter >");
waitForPress();
//Increment or decrement "count" based on button press
if(nLCDButtons == leftButton)
{
waitForRelease();
count--;
}
else if(nLCDButtons == rightButton)
{
waitForRelease();
count++;
}
break;
case 3:
//Display fourth choice
displayLCDCenteredString(0, "Autonomous 4");
displayLCDCenteredString(1, "< Enter >");
waitForPress();
//Increment or decrement "count" based on button press
if(nLCDButtons == leftButton)
{
waitForRelease();
count--;
}
else if(nLCDButtons == rightButton)
{
waitForRelease();
count = 0;
}
break;
default:
count = 0;
break;
}
}
//------------- End of User Interface Code ---------------------
//Clear LCD
clearLCDLine(0);
clearLCDLine(1);
//Switch Case that actually runs the user choice
switch(count){
case 0:
//If count = 0, run the code correspoinding with choice 1
displayLCDCenteredString(0, "Autonomous 1");
displayLCDCenteredString(1, "is picked!");
AtonNumber = 1;
break;
case 1:
//If count = 1, run the code correspoinding with choice 2
displayLCDCenteredString(0, "Autonomous 2");
displayLCDCenteredString(1, "is picked!");
AtonNumber = 2;
break;
case 2:
//If count = 2, run the code correspoinding with choice 3
displayLCDCenteredString(0, "Autonomous 3");
displayLCDCenteredString(1, "is picked!");
AtonNumber = 3;
break;
case 3:
//If count = 3, run the code correspoinding with choice 4
displayLCDCenteredString(0, "Autonomous 4");
displayLCDCenteredString(1, "is picked!");
AtonNumber = 1;
break;
default:
displayLCDCenteredString(0, "No valid choice");
displayLCDCenteredString(1, "was made!");
break;
}
}
task autonomous()
{
while(AtonNumber == 1)
{
if(SensorValue[killvar]==1){killvar=true;}while(killvar==true){sleep(10);}//kill switch
wait1Msec(15000);
}
while(AtonNumber == 2)
{
if(SensorValue[killvar]==1){killvar=true;}while(killvar==true){sleep(10);}//kill switch
wait1Msec(15000);
}
while(AtonNumber == 3)
{
if(SensorValue[killvar]==1){killvar=true;}while(killvar==true){sleep(10);}//kill switch
wait1Msec(15000);
}
while(AtonNumber == 4)
{
if(SensorValue[killvar]==1){killvar=true;}while(killvar==true){sleep(10);}//kill switch
wait1Msec(15000);
}
task usercontrol()
{
while(1==1)
{
if(vexRT[Btn5UXmtr2]==1)
{
motor[leftlift] = 115;
motor[rightlift] = 115;
if(SensorValue[leftpot] == 0)
{
motor[leftlift] = 0;
motor[rightlift] = 0;
}
}
else
{
motor[leftlift] = 0;
motor[rightlift] = 0;
}
if(vexRT[Btn5DXmtr2] == 1)
{
motor[leftlift] = -50;
motor[rightlift] = -50;
}
else
{
motor[leftlift] = 0;
motor[rightlift] = 0;
}
{
motor[base1] = vexRT[Ch2]/1.27;
motor[base3] = vexRT[Ch2]/1.27;
motor[base2] = vexRT[Ch3]/1.27;
motor[base4] = vexRT[Ch3]/1.27;
}
if(vexRT[Btn6UXmtr2] == 1)
{
motor[frontr] = 110;
motor[frontl] = 110;
motor[backr] = 110;
motor[backl] = 110;
if(SensorValue[shootlimit] == 1)
{
motor[frontr] = 0;
motor[frontl] = 0;
motor[backr] =0;
motor[backl] = 0;
}
}
else
{
motor[frontr] = 0;
motor[frontl] = 0;
motor[backr] = 0;
motor[backl] = 0;
}
if(vexRT[Btn8DXmtr2] == 1)
{
motor[frontl] = 100;
motor[frontr] = 100;
motor[backl] = 100;
motor[backr] = 100;
if(SensorValue[backpnu] == 0)
{
motor[frontl] = 0;
motor[frontr] = 0;
motor[backl] = 0;
motor[backr] = 0;
}
}
if(vexRT[Btn8UXmtr2] == 1)
{
SensorValue[backpnu] = 1;
SensorValue[frontpnu] = 1;
}
if(vexRT[Btn7UXmtr2] == 1)
{
SensorValue[frontpnu] = 0;
SensorValue[backpnu] = 0;
}
}
}
}