void pre_auton()
{
bLCDBacklight = true; // Turn on LCD Backlight
bStopTasksBetweenModes = true;
bDisplayCompetitionStatusOnLcd = false;
clearLCDLine(0); // Clear line 1 (0) of the LCD
clearLCDLine(1); // Clear line 2 (1) of the LCD
string mainBattery;
bool getI2C = testI2C();
if(nImmediateBatteryLevel/1000.0>7 && getI2C == true)
{
displayLCDString(0, 0, "Systems: GREEN");
displayLCDString(1, 0, "Ready to Begin!");
}
else if(nImmediateBatteryLevel/1000.0>7 && getI2C == false)
{
displayLCDCenteredString(0, "I2C Fault!!!");
displayLCDCenteredString(1, "Check Wires!!!");
}
else if(getI2C == false && nImmediateBatteryLevel/1000.0<7)
{
displayLCDCenteredString(0, "I2C Fault!!!");
displayLCDCenteredString(1, "Battery Fault!!!");
}
else
{
displayLCDCenteredString(0, "REPLACE BATT!!!");
displayLCDString(1, 0, "Main V: ");
sprintf(mainBattery, "%1.2f%c", nImmediateBatteryLevel/1000.0,'V');
displayNextLCDString(mainBattery);
}
}
//This function updates the backlight, applies screen strings if they have
//changed, and applies menu hints. Up to now, variables topLCDLine and
//bottomLCDLine have been set to the desired strings. This keeps the LCD from
//being cleared and set excessively (which causes ugly blinking).
void outputLCD()
{
updateBacklight();
if (changedString(topLCDLine))
{
clearLCDLine(0);
displayLCDCenteredString(0,topLCDLine.curr);
}
if (changedString(bottomLCDLine))
{
clearLCDLine(1);
displayLCDCenteredString(1,bottomLCDLine.curr);
}
#ifdef MENU_WRAP //Wrap
if (sysState.curr != AUTONOMOUS && sysError==ERR_NONE)
{
displayLCDString(0,0, "<");
displayLCDString(0,15,">");
}
#else //No wrap
if (sysState.curr != AUTONOMOUS && sysError==ERR_NONE)
{
if (menuItemIndex>0) displayLCDString(0,0, "<"); //If not at the first item, show prev arrow
if (menuItemIndex<(int)M_NO_ITEMS-1) displayLCDString(0,15,">"); //If not at the last item, show next arrow
}
#endif
setLastString(&topLCDLine);
setLastString(&bottomLCDLine);
}
void lcdDispPwr(bool doUseLRPwr) {
if (doUseLRPwr) {
displayLCDCenteredString(0, "Long Power:");
displayLCDNumber(1, 0, flyLRPwr);
}
else {
displayLCDCenteredString(0, "Short Power:");
displayLCDNumber(1, 0, flySRPwr);
}
}
task main() {
displayLCDCenteredString(0, "Started");
moveForward(100, 3000);
turnX(LEFT, 100, _90DEG);
moveForward(100, 3200);
turnX(RIGHT, 100, _90DEG);
moveForward(100, 1500);
turnX(RIGHT, 100, _90DEG);
moveForward(100, 1500);
displayLCDCenteredString(0, "Finished");
}
/////////////////////////////////////////////////////////////////////////////////////////
//
// Pre-Autonomous Functions
//
// You may want to perform some actions before the competition starts. Do them in the
// following function.
//
/////////////////////////////////////////////////////////////////////////////////////////
void pre_auton()
{
PIDreset();
//Turn on LCD Backlight
bLCDBacklight = true;
bStopTasksBetweenModes = false;
displayLCDCenteredString(0,"Calibrating...");
displayLCDCenteredString(1,"Please Wait");
wait1Msec(2000);
StartTask(Menu);
StartTask(updateSonarValues);
}
void updateLCD(){
if(menuCurrActive){
if(menuCurr == 0){
displayBattLevel();
}
}else{
if(menuCurr == 0){
displayLCDCenteredString(0, "Battery Level");
displayLCDCenteredString(1, "<<< GO >>>");
}
}
}
task usercontrol()
{
// User control code here, inside the loop
displayLCDCenteredString(0, "User Control");
// User Control
startTask(controllerPolling);
startTask(driving);
while(true)
{
displayLCDCenteredString(0, "User Control");
delay(2000);
}
}
void LcdUpdateScreen()
{
// Display a heading
displayLCDCenteredString(0, "Clr: Zone: Rtne:");
// Define a variable to hold the autonomous descriptor
string auton_string = "";
// Get the autonomous descriptor
LcdGetAutonString(auton_string);
// Display the autonomous descriptor
displayLCDCenteredString(1, auton_string);
}
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, ...
clearLCD();
displayLCDCenteredString(0, autonNames[selectedAuton]);
displayLCDCenteredString(1, "< Enter >");
while(nLCDButtons != centerButton) // Center LCD button is not pressed
{
if(nLCDButtons == 0) // There are currently no buttons pressed
{
wait1Msec(10); // No need to waste processor cycles
}
else
{
if(nLCDButtons == leftButton && (selectedAuton - 1) >= 0) // The left button is pressed and a subtraction of 1 won't lead to a non-choice
{
selectedAuton--; // Decrease our current autonomous selection index by 1
}
if(nLCDButtons == rightButton && (selectedAuton + 1) <= totalAutons)// The right button is pressed and an addition of 1 won't lead to a non-choice
{
selectedAuton++; // Increase our current autonomous selection index by 1
}
while(nLCDButtons != 0) // Make sure we release the button to prevent it being impossible to use 1 step
{
wait1Msec(10); // No need to waste processor cycles
}
}
// Add our selections to the display
clearLCDLine(0);
displayLCDCenteredString(0, autonNames[selectedAuton]);
}
clearLCD();
displayLCDCenteredString(0, autonNames[selectedAuton]);
displayLCDCenteredString(1, "Locked In");
}
task autonomous()
{
displayLCDCenteredString(0, "Autonomous Mode");
// Fire ball
// Turn on Shooter for 2 seconds
motor[shooterLeft] = 127;
motor[shooterRight] = -127;
wait1Msec(2000);
// Turn off Shooter
motor[shooterLeft] = 0;
motor[shooterRight] = 0;
// Then drive forward with intake and what for 4 seconds
SensorValue[in1] = 0;
motor[intake] = 127;
for (int i=0; i < 2000; i++){
motor[rightDrive] = 100 - (SensorValue[in1] / 4);
motor[leftDrive] = 100 + (SensorValue[in1] / 4);
wait1Msec(2);
}
// Then Turn off everything
motor[rightDrive] = 0;
motor[leftDrive] = 0;
motor[intake] = 0;
}
task autonomous()
{
// .....................................................................................
// Insert user code here.
// .....................................................................................
lcdClear();
char* disp = (Program==0?"BLU Auto":Program==1?"RED Auto":Program==2?"BLU Pole":"RED Pole");
displayLCDCenteredString(0, disp);
switch(Program)
{
case BLU_AUTO:
autonBluAuto();
break;
case RED_AUTO:
autonRedAuto();
break;
case BLU_POLE:
autonBluPole();
break;
case RED_POLE:
autonRedPole();
break;
}
AutonomousCodePlaceholderForTesting(); // Remove this function call once you have "real" code.
}
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;
bLCDBacklight = true;
displayLCDCenteredString(0, "Calibrating Gyro");
SensorType[in1] = sensorNone;
wait1Msec(1000);
displayLCDCenteredString(1, "50% Complete");
SensorType[in1]= sensorGyro;
wait1Msec(2000);
displayLCDCenteredString(0, "Gyro Ready");
clearLCDLine(1);
bLCDBacklight = false;
}
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 = false;
nMotorEncoder[blFly] = nMotorEncoder[brFly] = 0;
for(int i; i < 9; i++) {
lVel[i] = rVel[i] = 0;
}
bLCDBacklight = true;
clearLCDLine(0);
clearLCDLine(1);
batt = nAvgBatteryLevel / 1000.0; //The decimal is necessary because operators return values as simple as the most complex number
sprintf(battStr, "%1.2f", batt);
displayLCDCenteredString(0, "Battery:");
displayLCDCenteredString(1, battStr);
}
//Measures shooter speed, calculates power, and updates LCD
void calculateShooter() {
wait1Msec(50);
lastEncA = currentDistA; //Get the prevoius speed of the shooter
currentDistA = SensorValue[encShooterRight2]; //Get the current speed of the shooter SensorValue[encShooterRight2]
int currSysTime;
//Calculate the motor speed based on the system timer and the motor distance. Average the results, weighing
//heavily on the previous value to smooth out fluctuations
float speed = ((currentDistA - lastEncA) * 50.0 / ((currSysTime = nSysTime) - lastSysTime+1));
speedAverages = speedAverages*0.9+ speed*0.1;
//Save the previous states for next iteration
lastSpeedA = speed;
lastSysTime = currSysTime;
if (speed > 80) { speed = 80; } //Clamp the aspeed to make sure it doesn't go over 80/s
else if (speed < -80) { speed = -80; } // (prevents it from generating erronously high values)
//Calculate error and add in a reversal gain if we are approaching the speed
//(prevents overshooting due to the flywheel behavior of the shooter wheels)
float error = targetSpeed - speedAverages;
if (abs(error) < 0.25) {error = 0;}
else if (abs(error) < 0.5) {error = error*-0.35;}
//Make sure the motors will spin down on their own and not generate a negative power
if (targetSpeed == 0) {error=0;}
//Calculate power based on the error. Clamp the motor output to 127
shooterMotorRaw = manualSetSpeed+ error*(2.9*6.0);
if (shooterMotorRaw <= 0) { shooterMotorRaw = 0; }
//Update the LCD
clearLCDLine(1);
ready = (speedAverages > optimalSpeed - 1 && speedAverages < optimalSpeed + 1);
bLCDBacklight = ready;
string str;
stringFormat(str, "M %-2i/%-3i",manualSetSpeed,shooterMotorRaw);
displayLCDCenteredString(1, str);
clearLCDLine(0);
string str2;
stringFormat(str2, "%2.2f/%2.2f",speedAverages,targetSpeed);
displayLCDCenteredString(0, str2);
}
task autonomous(){
simple_autonomous();
return;
SensorFullCount[ChassisGyro] = 3600 * 100; // 100 revolutions
SensorValue[ChassisGyro] = 0;
get_balls_at_point(48, 48);
wait1Msec(500);
displayLCDCenteredString(0, "Done!!!!");
}
task LCD()
{
//Declare count variable to keep track of our choice
int count = 0;
LCDselect = 0;
//------------- Beginning of User Interface Code ---------------
//Clear LCD
clearLCDLine(0);
clearLCDLine(1);
//Loop while center button is not pressed
while(LCDselect == 0 && nSysTime < 20000)
{
//Switch case that allows the user to choose from 4 different options
while(nSysTime < 20000){
//Display first choice
displayLCDCenteredString(0, "The only autonomous play");
displayLCDCenteredString(1, "< Enter >");
//Increment or decrement "count" based on button press
if(nLCDButtons == leftButton)
{
waitForRelease();
count = 5;
}
else if(nLCDButtons == rightButton)
{
waitForRelease();
count++;
}
else if(nLCDButtons == centerButton){
OoT = 1;
BoR = 1;
LCDselect = 1;
break;
waitForRelease();
}
}
clearLCDLine(0);
clearLCDLine(1);
}
}
void lcdAuton()
{
//Declare count variable to keep track of our choice
int count = 0;
//------------- Beginning of Robot Movement Code ---------------
//Clear LCD
clearLCDLine(0);
clearLCDLine(1);
//Switch Case that actually runs the user choice
switch(autonCount)
{
case 0:
//If count = 0, run the code correspoinding with choice 1
displayLCDCenteredString(0, "Blue Left");
displayLCDCenteredString(1, "is running!");
blueLeft(delayer1);
break;
case 1:
//If count = 1, run the code correspoinding with choice 2
displayLCDCenteredString(0, "Blue Right");
displayLCDCenteredString(1, "is running!");
blueRight(delayer1);
break;
case 2:
//If count = 2, run the code correspoinding with choice 3
displayLCDCenteredString(0, "Red Left");
displayLCDCenteredString(1, "is running!");
redLeft(delayer1);
break;
case 3:
//If count = 3, run the code correspoinding with choice 4
displayLCDCenteredString(0, "Red Right");
displayLCDCenteredString(1, "is running!");
redRight(delayer1);
break;
default:
displayLCDCenteredString(0, "No valid choice");
displayLCDCenteredString(1, "was made!");
break;
}
}
bool testEncoder () {
playSound(soundException);
clearLCD();
displayLCDCenteredString(0,"LIFT");
displayLCDCenteredString(1,"ROBOT");
delay(1000);
SensorValue[encoderError] = 0;
bool performsWell = true;
//Flywheel
clearLCD();
displayLCDCenteredString(0,"Encoder Test");
int initValue = SensorValue[flywheelEncoder];
startAutoFlywheel(VELOCITY_LONG);
delay(1000);
if(SensorValue[flywheelEncoder]==initValue) {
performsWell = false;
displayLCDCenteredString(1,"Failed");
} else {
displayLCDCenteredString(1,"Passed");
}
startTask(stopFlywheel);
//Drivebase
delay(1000);
clearLCD();
displayLCDCenteredString(0,"Drivebase Test");
int initWheelValues[2];
initWheelValues[0] = nMotorEncoder(leftWheel13);
initWheelValues[1] = nMotorEncoder(rightWheel13);
setWheelSpeed();
delay(2000);
if(initWheelValues[0]==nMotorEncoder(leftWheel13)) {
performsWell = false;
displayLCDCenteredString(1,"Left Failed");
} else if(initWheelValues[1]==nMotorEncoder(rightWheel13)) {
performsWell = false;
displayLCDCenteredString(1,"Right Failed");
} else {
displayLCDCenteredString(1,"Passed");
}
setWheelSpeed(0);
return performsWell;
}
task main()
{
int nBiasValues[3];
int X_Accel;
int Y_Accel;
int Z_Accel;
clearLCDLine(0);
clearLCDLine(1);
bLCDBacklight = true;
displayLCDCenteredString(0, "Accel Test");
wait1Msec(500); // Bias values are being calculated
PlaySound(soundBeepBeep);
// Store the bias values in a variable so that they can be easily displayed in the ROBOTC debugger
// global variables window
nBiasValues[0] = SensorBias[xAxis];
nBiasValues[1] = SensorBias[yAxis];
nBiasValues[2] = SensorBias[zAxis];
while (true)
{
// Use the VEX LCD to display the accelerometer values
clearLCDLine(0);
clearLCDLine(1);
displayLCDPos(0, 0);
displayNextLCDString(".X.. ..Y.. ..Z..");
displayLCDPos(1, 0);
displayNextLCDNumber(SensorValue[xAxis], 4);
displayLCDPos(1, 5);
displayNextLCDNumber(SensorValue[yAxis], 5);
displayLCDPos(1, 12);
displayNextLCDNumber(SensorValue[zAxis], 4);
// Also store values in variables so that they can be easily displayed in "Globals" debugger window
X_Accel = SensorValue[xAxis];
Y_Accel = SensorValue[yAxis];
Z_Accel = SensorValue[zAxis];
wait1Msec(100);
}
}
task Display_LCD(){
bLCDBacklight = true;
while(true){
// Converting autonN to a string.
//string autonNstring = autonN;
// Battery level indicator
string display = "R:";
string regularBat, backupBat;
StringFormat(regularBat, "%1.2f", (float)nImmediateBatteryLevel/1000);
//StringFormat(expand_bat, "%1.2f", (float)SensorValue[]/275);
StringFormat(backupBat, "%1.2f", (float)BackupBatteryLevel/1000);
StringConcatenate(display, regularBat);
StringConcatenate(display, "V B:");
StringConcatenate(display, backupBat);
StringConcatenate(display, "V ");
clearLCDLine(0);
clearLCDLine(1);
if(vexRT[Btn7L] == 1)
{
displayLCDCenteredString(0, "Battery Power:");
displayLCDCenteredString(1, display);
}
else
{
if((float)nImmediateBatteryLevel/1000 < 6.45)
{
displayLCDCenteredString(0, "***WARNING***");
displayLCDCenteredString(1, "*LOW BATTERY*");
}
else
{
// Default screen:
string left = SensorValue[LeftLine];
string right = SensorValue[RightLine];
string arm = armRotation;
string lolbeav = isBeingIntaken;
displayLCDCenteredString(0, left);
displayLCDCenteredString(1, lolbeav);
}
}
wait1Msec(100);
}
}
void displayScreen(const string display){ //Standard Display screen format for LCD
displayLCDString(0,0, display);
displayLCDCenteredString(1,"<Select>");
}
task LCDSelect() {
if (showVersion) {
clearLCD();
displayLCDCenteredString(0,"LCD Select v2.0-f"); //briefly display version for debug purposes
wait1Msec(750);
}
showVersion = false; //don't show the version again if autonSelection is ever reset
//Q1: Which side?
showOnLCD("Which side?","Left Right Sk.");
waitForPress();
int q1Response = nLCDButtons;
waitForRelease();
int q2Page = 1; //1 - Big/Small choices shown, 2 - Cube/Fence choices shown, 3 - Nothing choice shown
int q2Response = 0;
int q2PlayChosen = 0;
if (q1Response != 4) {
while (q2PlayChosen == 0) {
if (q2Page == 1) {
if (LCD_CUBE && LCD_STARS) {
showOnLCD("Choose play: 1/2","Cube Star >>");
} else if (LCD_CUBE && !LCD_STARS) {
showOnLCD("Choose play:","Cube >>");
} else if (!LCD_CUBE && LCD_STARS) {
showOnLCD("Choose play:"," Star >>");
} else {
showOnLCD("Press any btn","");
}
wait1Msec(250); //short delay to prevent button press from triggering multiple times
waitForPress();
q2Response = nLCDButtons;
if (q2Response == 4) {
q2Page = 3;
} else if (q2Response == 1) {
q2PlayChosen = 1;
} else if (q2Response == 2) {
q2PlayChosen = 2;
}
waitForRelease();
if (q2Response == 1 ||q2Response == 2) {
break;
}
}/* else if (q2Page == 2) {
showOnLCD("Choose play: 2/3","Cube Fence >>");
wait1Msec(250);
waitForPress();
q2Response = nLCDButtons;
if (q2Response == 4) {
q2Page = 3;
} else if (q2Response == 1) {
q2PlayChosen = 3;
} else if (q2Response == 2) {
q2PlayChosen = 4;
}
waitForRelease();
if (q2Response == 1 ||q2Response == 2) {
break;
}
} */else if (q2Page == 3) {
showOnLCD("Choose play: 2/2","Nothing >>");
wait1Msec(250);
waitForPress();
q2Response = nLCDButtons;
if (q2Response == 4) {
q2Page = 1;
} else if (q2Response == 1) {
q2PlayChosen = 4;
}
waitForRelease();
if (q2Response == 1 ||q2Response == 2) {
break;
}
} //end else if
} //end while
} //end if
if (q1Response == 4) {
AUTON_PLAY = 3;
playConfirmName = "Prog. skills";
} else {
if (q1Response == 1) {
AUTON_SIDE = LEFT;
} else { //q1Response == 2
AUTON_SIDE = RIGHT;
}
switch (q2PlayChosen) {
case 1:
AUTON_PLAY = 1;
playConfirmName = "Cube";
break;
case 2:
AUTON_PLAY = 2;
playConfirmName = "Back stars";
break;
//case 3 would be programming skills, which is handled above
case 4:
AUTON_PLAY = 4;
playConfirmName = "NOTHING";
break;
}
}
startTask(confirmChoice);
wait1Msec(250);
int confirmResult = waitForButtonPress();
stopTask(confirmChoice);
if (confirmResult == 2) {
if (AUTON_PLAY == 5) {
showOnLCD("NOTHING","No auton!");
} else {
showOnLCD(playConfirmName,"will run!");
}
} else {
startTask(restartAutonSelection);
}
} //end task
void showOnLCD(const string line0, const string line1) {
clearLCD();
displayLCDCenteredString(0,line0);
displayLCDCenteredString(1,line1);
}
task main()
{
// Turn on the LCD backlight
bLCDBacklight = true;
// Clear the LCD
LcdClear();
// Display a startup message
displayLCDCenteredString(0, "Starting up...");
// Don't start the robot for two seconds, as the original code did
wait1Msec(2000);
// Call the pre_auton funtion
pre_auton();
// Main loop
while (true)
{
if (bIfiRobotDisabled)
{
// Allow the auton to be set using the LCD when disabled
StartTask(LcdSetAuton);
// Stop all motors when disabled
allMotorsOff();
}
// If the robot is disabled, do nothing until it is enabled
while (bIfiRobotDisabled)
wait1Msec(25);
// Stop allowing the autonomous to be set from the LCD
StopTask(LcdSetAuton);
// Clear the LCD
LcdClear();
// Handle autonomous mode
if (bIfiAutonomousMode)
{
// Clear the LCD
LcdClear();
// Start the autonomous task
StartTask(autonomous);
// Loop to be run while autonomous is running and the robot is enabled
while (bIfiAutonomousMode && !bIfiRobotDisabled)
{
// If the VEXNET disconnects, power off all of the motors
if (!bVEXNETActive)
if (nVexRCReceiveState == vrNoXmiters)
allMotorsOff();
// Don't hog CPU
wait1Msec(25);
}
// After autonomous has expired, power off all of the motors
allMotorsOff();
// Stop all tasks if the user has set tasks to end between modes
if(bStopTasksBetweenModes)
allTasksStop();
}
else
{
// Clear the LCD
LcdClear();
// Display Driver Control to reflect the current mode
displayLCDCenteredString(0, "Driver Control");
// Start the user control task
StartTask(usercontrol);
// Here we repeat loop waiting for user control to end and (optionally) start
// of a new competition run
while (!bIfiAutonomousMode && !bIfiRobotDisabled)
{
// If the VEXNET disconnects, turn off the motors
if (nVexRCReceiveState == vrNoXmiters)
allMotorsOff();
// Don't hog CPU
wait1Msec(25);
}
// If the mode is switched from user control, turn of all motors
allMotorsOff();
// Stop all tasks if the user has set tasks to end between modes
if(bStopTasksBetweenModes)
allTasksStop();
}
}
}
void pre_auton()
{
//------------- Beginning of User Interface Code ---------------
//Clear LCD
clearLCDLine(0);
clearLCDLine(1);
bLCDBacklight = true;
//Loop while center button is not pressed
while(nLCDButtons != centerButton)
{
//Switch case that allows the user to choose from 4 different options
switch(count){
case 0:
//Display first choice
displayLCDCenteredString(0, "Red Middle");
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, "blue Middle");
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, "RED hang");
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, "Blue hang");
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;
}
}
}
task usercontrol()
{
lawnswap = 0;
lawnchair[0] = 0;
lawnchair[1] = 0;
motor[port2] = 0;
motor[port3] = 0;
motor[port4] = 0;
motor[port5] = 0;
motor[port6] = 0;
motor[port7] = 0;
motor[port8] = 0;
motor[port9] = 0;
//////////////////////////make this less stupid///////
int egg;
int lel;
int past1;
int past2;
int enc;
int enc2;
int enc3;//encoder
string sneakydeaky;
float cisstupid;
bLCDBacklight = true;
clk = 0;
enc = nMotorEncoder[port8];
int batdivk[2]; //batterydividedby
string line1; //line1 of lcd
string line2; //line2 of lcd
lawnswap = 0;
while(true)
{
batdivk[0] = nImmediateBatteryLevel;
batdivk[1] = SensorValue[in1];
if(lawnswap == 1)
{
line1 = "MANUAL MODE"; //or Battery or batdivk[0];
lel = 1;
line2 = enc;
}
else
{
if(lel == 1)
{
if(lawnswap2 == 0)
{
line1 = "PID";
lel = 0;
}
else if(lawnswap2 == 2)
{
line1 = "Driver Assisted";
lel = 0;
}
}
if(vexRT[Btn8U] == 1)
{
line1 = "HIGH";
}
if(vexRT[Btn8L] == 1)
{
line1 = "MEDIUM";
}
if(vexRT[Btn8D] == 1)
{
line1 = "LOW";
}
if(vexRT[Btn8R] == 1)
{
line1 = "OFF";
}
}
line2 = enc; //enc
if(nPgmTime - clk >= 70)
{
clk = nPgmTime;
past2 = past1;
past1 = nMotorEncoder[port8];
enc = nMotorEncoder[port8] - past2;
if(enc < ideal)
{
lawnchair[0] ++;
}
if(enc > ideal)
{
lawnchair[0]--;
}
}
if(lawnswap2 == 2)
{
if(enc <= enc3 && enc >= enc2)
{
motor[port6] = 90;
motor[port7] = 90;
}
else
{
motor[port6] = 0;
motor[port7] = 0;
}
}
//why
egg = nLCDButtons;
if (egg == 7) {
line1 = "Nick is dumb.";
}
else if (egg == 4) {
cisstupid = batdivk[0]/1000.;
line1 = cisstupid;
}
else if(egg == 2) {
if(bVEXNETActive == true){
line1 = "Vexnet is on.";
}
else {
line1 = "Vexnet is off.";
}
}
else if(egg == 1) {
line1 = lawnchair[0];
}
else {
}
displayLCDCenteredString(0, line1);
displayLCDCenteredString(1, line2);
//////////////////////////////////////////////////////
intake();
wheels();
launcher();
feedback();
rollers();
}
}
task autonomous()
{
int ideala;
int lawnchaira[2];
int clka[3];
int past1;
int past2;
int enca;
int next;
ideala = 45;
lawnchaira[0] = 40;
next = 1;
string line1;
while(true)
{
line1 = enca;
displayLCDCenteredString(0, line1);
if(next == 1 && enca <= 55)
{
lawnchaira[0] = 60;
next = 0;
}
if(nPgmTime - clka[0] >= 100)
{
clka[0] = nPgmTime;
past2 = past1;
past1 = nMotorEncoder[port8];
enca = nMotorEncoder[port8] - past2;
}
if(nPgmTime - clka[1] >= 300)
{
clka[1] = nPgmTime;
if(enca < ideala)
{
lawnchaira[0] = lawnchaira[0] + 2;
}
}
if(nPgmTime - clka[2] >= 300)
{
clka[2] = nPgmTime;
if(enca > ideala)
{
lawnchaira[0] = lawnchaira[0] - 2;
}
}
motor[port8] = lawnchaira[0];
motor[port9] = lawnchaira[0];
if(enca >= 55 && enca <= 59)
{
motor[port6] = -90;
motor[port7] = -90;
next = 1;
}
else
{
motor[port6] = 0;
motor[port7] = 0;
}
}
}
void pre_auton(){//Pre-Autonomous block begin
clear();
while (bIfiRobotDisabled){//outputs true when robot is disabled, ends subroutine when autonomous starts
displayLCDString(1, 0, "<<");
displayLCDString(1, 14, ">>");
switch(auton){
case 1://red 1
displayLCDCenteredString(0, "Red 1");
if (nLCDButtons == 1){
while (nLCDButtons == 1){
}
auton = 6;
}
else{
}
if (nLCDButtons == 4){
while (nLCDButtons == 4){
}
auton = 2;
}
else{
}
break;
case 2://red 2
displayLCDCenteredString(0, "Red 2");
if (nLCDButtons == 1){
while (nLCDButtons == 1){
}
auton = 1;
}
else{
}
if (nLCDButtons == 4){
while (nLCDButtons == 4){
}
auton = 3;
}
else{
}
break;
case 3://blue 1
displayLCDCenteredString(0, "Blue 1");
if (nLCDButtons == 1){
while (nLCDButtons == 1){
}
auton = 2;
}
else{
}
if (nLCDButtons == 4){
while (nLCDButtons == 4){
}
auton = 4;
}
else{
}
break;
case 4://blue 2
displayLCDCenteredString(0, "Blue 2");
if (nLCDButtons == 1){
while (nLCDButtons == 1){
}
auton = 3;
}
else{
}
if (nLCDButtons == 4){
while (nLCDButtons == 4){
}
auton = 5;
}
else{
}
break;
case 5://Programming skills
displayLCDCenteredString(0, "Pr. Skills");
if (nLCDButtons == 1){
while (nLCDButtons == 1){
}
auton = 4;
}
else{
}
if (nLCDButtons == 4){
while (nLCDButtons == 4){
}
auton = 6;
}
else{
}
break;
case 6://Emergency fallback
displayLCDCenteredString(0, "Emergency");
if (nLCDButtons == 1){
while (nLCDButtons == 1){
}
auton = 5;
}
else{
}
if (nLCDButtons == 4){
while (nLCDButtons == 4){
}
auton = 1;
}
else{
}
break;
}
}
}//Pre-autonomous block end
void autonomousSelection(){
while(LCDselect == 0 && nSysTime < 15000){
//set the while loop for the choices
while(firstchoice == 0 && nSysTime < 15000){
displayLCDCenteredString(0, "Left or Right?");
//Autonomous or Not?
if(nLCDButtons == leftButton){
waitForRelease();
auton = true;
auton1 = true;
firstchoice = 1;
//set up the variables and booleans for autonomous = true
}
if(nLCDButtons == rightButton){
waitForRelease();
auton = false;
firstchoice = 1;
//Do nothing, prepare for User Control
}
} //End the Autonomous yes or no loop
while(auton1 == true && nSysTime < 15000){
//when autonomous is selected
displayLCDCenteredString(0, "Five sack or Y?");
//Left or right Autonomous?
if(nLCDButtons == leftButton){
waitForRelease();
rightLeft = 1;
auton1 = false;
autonL = true;
//Set the variables for autonomous left
}
if(nLCDButtons == rightButton){
waitForRelease();
rightLeft = 2;
auton1 = false;
autonR = true;
//set the variables for autonomous Right
}
}
displayLCDCenteredString(0, "Are you sure?");
//Are you sure with the Code selected?
displayLCDCenteredString(1, "Press center");
//If sure, press center
if(nLCDButtons == centerButton){
//End LCD selections
waitForRelease();
LCDselect = 1;
}
if(nLCDButtons == leftButton)
{
// User is unsure, clear ALL variables to restart selection
resetLCD();
}
if(nLCDButtons == rightButton)
{
// User is unsure, Clear ALL variables to restart Selection
resetLCD();
}
}
}
task main()
{
displayLCDCenteredString(0, "Autonomus");
displayLCDCenteredString(1, "Recorder V1.0");
wait1Msec(2000);
clearLCD();
displayLCDCenteredString(0, "Start");
displayLCDCenteredString(1, "\/");
waitPress(centerButton);
waitRelease();
clearLCD();
displayLCDCenteredString(0, "Time Remaining");
ClearTimer(T1);
int i = -1;
while(time1[T1] < 1000*AUTONLENGTH)
{
i++;
clearLCDLine(1);
timeRemaining = AUTONLENGTH-(time1[T1]*0.001);
sprintf(display, "%1.1f%c", timeRemaining);
displayLCDCenteredString(1, display);
baseEncoderReset();
liftArmEncoderReset();
driverControl(false);
temp[tempRightSpeed] = motor[backRight]; //Right Speed
temp[tempLeftSpeed] = motor[backLeft]; //Left Speed
temp[tempRightDistance] = nMotorEncoder[backRight]; //Right Distance
temp[tempLeftDistance] = nMotorEncoder[backLeft]; //Left Distance
temp[tempHangingArmDistance] = SensorValue[liftArmEncoder]; //Lift Encoder
temp[tempHangingArmSpeed] = motor[liftTopRight]; //Lift Speed
temp[tempIntakeSpeed] = motor[intakeRight]; //Intake Speed
if(abs(temp[0] - temp[2]) < 11) //Straight
{
if(temp[0] > 0) //Forward
{
data[i][dataRightSpeed] = (temp[tempRightSpeed] + temp[tempLeftSpeed])/2; //Right Speed
data[i][dataRightDistance] = (temp[tempRightDistance] + temp[tempLeftDistance])/2; //Right Distance
data[i][dataLeftSpeed] = data[i][dataRightSpeed]; //Left Speed
data[i][dataLeftDistance] = data[i][dataRightDistance]; //Left Distance
data[i][dataMovementCode] = codeForward; //Forward Code
}
else if(temp[0] < 0) //Backward
{
data[i][dataRightSpeed] = (temp[tempRightSpeed] + temp[tempLeftSpeed])/2; //Right Speed
data[i][dataRightDistance] = (temp[tempRightDistance] + temp[tempLeftDistance])/2; //Right Distance
data[i][dataLeftSpeed] = data[i][dataRightSpeed]; //Left Speed
data[i][dataLeftDistance] = data[i][dataRightDistance]; //Left Distance
data[i][dataMovementCode] = codeBackward; //Backward Code
}
else //Not moving
{
data[i][dataRightSpeed] = 0;
data[i][dataRightDistance] = temp[tempRightDistance];
data[i][dataLeftSpeed] = 0;
data[i][dataLeftDistance] = temp[tempLeftDistance];
data[i][dataMovementCode] = codeWait; //Wait Code
}
}
else if((temp[tempRightSpeed] < temp[tempLeftSpeed] && temp[tempRightSpeed] > 0) || (temp[tempRightSpeed] > temp[tempLeftSpeed] && temp[tempRightSpeed] < 0)) //Right Turn
{
data[i][dataRightSpeed] = temp[tempRightSpeed];
data[i][dataLeftSpeed] = temp[tempLeftSpeed];
data[i][dataRightDistance] = temp[tempRightDistance];
data[i][dataLeftDistance] = temp[tempLeftDistance];
data[i][dataMovementCode] = codeRightTurn;
}
else if((temp[tempRightSpeed] > temp[tempLeftSpeed] && temp[tempRightSpeed] > 0) || (temp[tempRightSpeed] < temp[tempLeftSpeed] && temp[tempRightSpeed] < 0)) //Left Turn
{
data[i][dataRightSpeed] = temp[tempRightSpeed];
data[i][dataLeftSpeed] = temp[tempLeftSpeed];
data[i][dataRightDistance] = temp[tempRightDistance];
data[i][dataLeftDistance] = temp[tempLeftDistance];
data[i][dataMovementCode] = codeLeftTurn;
}
if(temp[tempHangingArmSpeed] != 0)
{
data[i][dataMovementCode] += codeHangingArm;
if(temp[tempHangingArmSpeed] > 0)
{
data[i][dataMovementCode] += codePlus;
}
else
{
data[i][dataMovementCode] += codeMinus;
}
}
if(tempIntakeSpeed != 0)
{
data[i][dataMovementCode] += codeIntake;
if(temp[tempIntakeSpeed] > 0)
{
data[i][dataMovementCode] += codePlus;
}
else
{
data[i][dataMovementCode] += codeMinus;
}
}
if(temp[tempBallLift] != 0)
{
data[i][dataMovementCode] += codeBallLift;
data[i][dataMovementCode] += codePlus;
}
data[i][dataHangingArmDistance] = temp[tempHangingArmDistance];
if(!firstRun)
{
if((data[i][dataMovementCode]/10) == (data[i-1][dataMovementCode]/10))
{
i--;
data[i][dataMovementCode] = data[i+1][dataMovementCode];
data[i][dataRightSpeed] = (data[i+1][dataRightSpeed] + data[i][dataRightSpeed]) / 2;
data[i][dataLeftSpeed] = (data[i+1][dataLeftSpeed] + data[i][dataLeftSpeed]) / 2;
}
i++;
data[i][dataRightDistance] += data[i+1][dataRightDistance];
data[i][dataLeftDistance] += data[i+1][dataLeftDistance];
data[i][dataHangingArmDistance] += data[i+1][dataHangingArmDistance];
}
else
{
firstRun = false;
}
}
for(int i=1;i<AUTONLENGTH*20;i++)
{
data[i][dataRightDistance] = 127;
data[i][dataMovementCode] = codeForward+1;
data[i][dataRightSpeed] = 63;
if((data[i][dataMovementCode]/10)*10 == codeForward)
{
writeDebugStreamLine("");
writeDebugStream("forward(");
writeDebugStream("%1.0f%c", data[i][dataRightSpeed]);
writeDebugStream(", ");
writeDebugStream("%1.0f%c", data[i][dataRightDistance]);
writeDebugStream(");");
}
else if((data[i][dataMovementCode]/10)*10 == codeBackward)
{
writeDebugStreamLine("");
writeDebugStream("backward(");
writeDebugStream("%1.0f%c", data[i][dataRightSpeed]);
writeDebugStream(", ");
writeDebugStream("%1.0f%c", data[i][dataRightDistance]);
writeDebugStream(");");
}
else if((data[i][dataMovementCode]/10)*10 == codeLeftTurn)
{
writeDebugStreamLine("");
writeDebugStream("leftTurn(");
writeDebugStream("%1.0f%c", data[i][dataRightSpeed]);
writeDebugStream(", ");
writeDebugStream("%1.0f%c", data[i][dataLeftSpeed]);
writeDebugStream(", ");
writeDebugStream("%1.0f%c", data[i][dataRightDistance]);
writeDebugStream(", ");
writeDebugStream("%1.0f%c", data[i][dataLeftDistance]);
writeDebugStream(");");
}
else if((data[i][dataMovementCode]/10)*10 == codeRightTurn)
{
writeDebugStreamLine("");
writeDebugStream("rightTurn(");
writeDebugStream("%1.0f%c", data[i][dataRightSpeed]);
writeDebugStream(", ");
writeDebugStream("%1.0f%c", data[i][dataLeftSpeed]);
writeDebugStream(", ");
writeDebugStream("%1.0f%c", data[i][dataRightDistance]);
writeDebugStream(", ");
writeDebugStream("%1.0f%c", data[i][dataLeftDistance]);
writeDebugStream(");");
}
}
}