task main() { int speed; int sonar_value; int distance = 25; while (true) { sonar_value = SensorValue(sonarSensor); displayLCDPos(0,0); displayNextLCDString("Sonar: "); displayNextLCDNumber(sonar_value); if (sonar_value < 0) { speed = 127; } else { speed = (sonar_value - distance)^2; } clearLCDLine(1); displayLCDPos(1,0); displayNextLCDString("Speed: "); displayNextLCDNumber(speed); motor[frontLeftMotor] = speed; motor[frontRightMotor] = speed; motor[backLeftMotor] = speed; motor[backRightMotor] = speed; wait1Msec(100); } }
void pre_auton() { bStopTasksBetweenModes = true; bLCDBacklight = true; displayLCDPos(0,0); displayNextLCDString("program select"); redteam = true; screenrefresh(); time1[T1] = 0; while (programselecting == true) { if (nLCDButtons & kButtonLeft) { while (nLCDButtons & kButtonLeft) { } if (redteam == true) { redteam = false; } else if (redteam == false) { redteam = true; } screenrefresh(); }// end while if (nLCDButtons & kButtonCenter) { while (nLCDButtons & kButtonCenter) { } programselect = programselect+1; if (programselect > totalprogramnumber) { programselect = 1; } screenrefresh(); } if (nLCDButtons & kButtonRight) { clearLCDLine(0); clearLCDLine(1); displayLCDPos(0,0); displayNextLCDString("Robot ready"); wait1Msec(300); displayNextLCDString("."); wait1Msec(300); displayNextLCDString("."); wait1Msec(300); displayNextLCDString("."); wait1Msec(500); bLCDBacklight = false; programselecting = false; } } }//end pre_auton
void LCD(){ { clearLCDLine(0); clearLCDLine(1); // Clear LCD Bottom Line displayLCDPos(1,0); // Set the cursor to bottom line, first position displayNextLCDString("Arm: "); // Print "Potentio: " starting at the cursor position displayNextLCDNumber(SensorValue(ncoderarm)); // Display the reading of the Potentiometer to current cursor position displayLCDPos(0,5); displayNextLCDString("Touch: "); displayNextLCDNumber(SensorValue(Touch)); } }
task changeBallCount() { bool button8DPressed = false, button8LPressed = false; while (1) { if (vexRT[Btn8D] && !button8DPressed) { ballsInIntake--; button8DPressed = true; } else if (vexRT[Btn8L] && !button8LPressed) { ballsInIntake++; button8LPressed = true; } if (!vexRT[Btn8D]) { button8DPressed = false; } if (!vexRT[Btn8L]) { button8LPressed = false; } if (ballsInIntake > 4) { ballsInIntake = 4; } else if (ballsInIntake < 0) { ballsInIntake = 0; } clearLCDLine(0); displayLCDPos(0,0); displayNextLCDNumber(ballsInIntake); wait1Msec(100); } }
task main() { clearLCDLine(0); clearLCDLine(1); displayLCDPos(0, 0); displayNextLCDString("Titties"); nVolume = 4; #ifndef NOSOUND PlaySoundFile(SONGNAME); #endif resetVars(); // reset all variables resetSensors(); // reset all sensors #ifndef NOAUTON AutoSelector();//run the RedFront autonomous Autonomous(); #endif AutoRedPost(); while (true) { #ifndef NOSOUND if (bSoundQueueAvailable) { PlaySoundFile(SONGNAME); } #endif RC(); // recieve inputs calcMotorValues(); //writeStream(); //beltPower = 127; RunRobot(); } }
void driver(){ conInput(); chaDrive(); clearLCDLine(0); clearLCDLine(1); displayLCDPos(1,1); displayNextLCDString("Cor"); displayLCDPos(1,5); displayNextLCDString("Mid"); displayLCDPos(1,9); displayNextLCDString("Cse"); SensorValue[red] = 1; SensorValue[yellow] = 1; SensorValue[green] = 1; if(launchSpeedSet == corner){ displayLCDPos(1,0); displayNextLCDString("*"); SensorValue[red] = 0; }else if(launchSpeedSet == middle){ displayLCDPos(1,4); displayNextLCDString("*"); SensorValue[yellow] = 0; }else if(launchSpeedSet == close){ displayLCDPos(1,8); displayNextLCDString("*"); SensorValue[green] = 0; }else{ } displayLCDPos(0,0); displayNextLCDString("Motor Speed: "); displayLCDPos(0,13); string response = (int)launchSpeedSet; displayNextLCDString(response); displayLCDPos(1,13); }
task main() { startTask(Scoop); startTask(Shoot); startTask(Drive); clearLCDLine(0); clearLCDLine(1); while(true) { displayLCDPos(0, 0); displayNextLCDString("Move Bitch"); displayLCDPos(1, 0); displayNextLCDString("get out the way"); } }
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); } }
int initializeDisplay() { //clearLCDLine(0); //clearLCDLine(1); bLCDBacklight = true; //Turn Backlight on displayLCDPos(1, 0); //Move "Cursor" to first spot displayNextLCDString("Ready to PWN :-D"); //Display motivational message return 0; }
void RobotReady() { clearLCDLine(0); clearLCDLine(1); displayLCDPos(0,0); displayNextLCDString("Robot ready"); wait1Msec(300); displayNextLCDString("."); wait1Msec(300); displayNextLCDString("."); wait1Msec(300); displayNextLCDString("."); wait1Msec(500); bLCDBacklight = false; }
void screenrefresh() { clearLCDLine(1); displayLCDPos(1,0); if (redteam == true) { displayNextLCDString("RED "); } if (redteam == false) { displayNextLCDString("BlUE"); } displayNextLCDString(" "); displayNextLCDNumber(programselect); displayNextLCDString(" OK"); }
static void displayStatusAndTime() { displayLCDPos(1, 0); if (bIfiRobotDisabled) displayNextLCDString("Disable "); else { if (bIfiAutonomousMode) displayNextLCDString("Auton "); else displayNextLCDString("Driver "); } displayNextLCDNumber(nTimeXX / 600, 2); displayNextLCDChar(':'); displayNextLCDNumber((nTimeXX / 10) % 60, -2); displayNextLCDChar('.'); displayNextLCDNumber(nTimeXX % 10, 1); }
void LCDER(string here) { clearLCDLine(1); bLCDBacklight = true; displayLCDPos(1,11); displayNextLCDString(here); }
task main() { // Master CPU will not let competition start until powered on for at least 2-seconds clearLCDLine(0); clearLCDLine(1); displayLCDPos(0, 0); displayNextLCDString("Startup"); wait1Msec(2000); while (true) { clearLCDLine(0); clearLCDLine(1); displayLCDPos(0, 0); while (bIfiRobotDisabled) { if (JustDisabled == true){ startTask(LCD); SensorValue(BaseLock) = 0; SensorType[ in2 ] = sensorNone; wait1Msec(1000); SensorType[ in2 ] = sensorGyro; wait1Msec(2000); } else { wait1Msec(25); } JustDisabled = false; } JustDisabled = true; nTimeXX = 0; clearLCDLine(0); clearLCDLine(1); displayLCDPos(0, 0); if (bIfiAutonomousMode) { displayNextLCDString("Autonomous"); startTask(autonomous); // Waiting for autonomous phase to end while (bIfiAutonomousMode && !bIfiRobotDisabled) { if (!bVEXNETActive) { if (nVexRCReceiveState == vrNoXmiters) // the transmitters are powered off!! allMotorsOff(); } wait1Msec(25); // Waiting for autonomous phase to end } allMotorsOff(); if(bStopTasksBetweenModes) { allTasksStop(); } } else { displayNextLCDString("User Control"); startTask(usercontrol); // Here we repeat loop waiting for user control to end and (optionally) start // of a new competition run while (!bIfiAutonomousMode && !bIfiRobotDisabled) { if (nVexRCReceiveState == vrNoXmiters) // the transmitters are powered off!! allMotorsOff(); wait1Msec(25); } allMotorsOff(); if(bStopTasksBetweenModes) { allTasksStop(); } } } }
task usercontrol() { while (true) { motor[rightfrontdrive]=vexRT[Ch2]; motor[rightmiddledrive]=vexRT[Ch2]; motor[rightreardrive]=vexRT[Ch2]; motor[leftfrontdrive]=vexRT[Ch3]; motor[leftmiddledrive]=vexRT[Ch3]; motor[leftreardrive]=vexRT[Ch3]; if (1 == 1) { clearLCDLine(0); // Clear line 1 (0) of the LCD clearLCDLine(1); // Clear line 2 (1) of the LCD //Display the Primary Robot battery voltage displayLCDString(0, 0, "Primary: "); sprintf(mainBattery, "%1.2f%c", nImmediateBatteryLevel/1000.0,'V'); //Build the value to be displayed displayNextLCDString(mainBattery); displayLCDPos(1,0); displayNextLCDNumber(SensorValue(pot)); //Short delay for the LCD refresh rate wait1Msec(100); } if(vexRT[Btn5U]==1) { if(SensorValue(pot) < 1700){ motor[leftlift] = 127; motor[rightlift] = 127; } else if(SensorValue(pot) > 1700){ motor[leftlift] = 70; motor[rightlift] = 70; } } else if(vexRT[Btn5D]==1) { if(SensorValue(pot) < 1700){ motor[leftlift] = -127; motor[rightlift] = -127; } else if(SensorValue(pot) > 1700){ motor[leftlift] = -70; motor[rightlift] = -70; } } else { motor[leftlift] = 10; motor[rightlift] = 10; } if (vexRT[Btn6U]==1) { motor[leftintake]=-127; motor[rightintake]=-127; } else if (vexRT[Btn6D]==1) { motor[leftintake]=127; motor[rightintake]=127; } else { motor[leftintake]=0; motor[rightintake]=0; } if (vexRT[Btn7U]==1) { SensorValue[cat1]=0; SensorValue[cat2]=0; } else if (vexRT[Btn7D]==1) { SensorValue[cat2]=1; SensorValue[cat1]=1; } if (vexRT[Btn8U]==1) { SensorValue[hang2]=1; SensorValue[hang1]=1; } else if (vexRT[Btn8D]==1) { SensorValue[hang1]=0; SensorValue[hang2]=0; } while(vexRT[Btn7L]==1) { if(SensorValue(pot) < 1450){ motor[leftlift] = 127; motor[rightlift] = 127; } else if(SensorValue(pot) > 1500){ motor[leftlift] = -127; motor[rightlift] = -127; } else { motor[leftlift] = 10; motor[rightlift] = 10; } } if(vexRT[Btn7R]==1) { if(SensorValue(pot) < 1900){ motor[leftlift] = 127; motor[rightlift] = 127; } else if(SensorValue(pot) > 2025){ motor[leftlift] = -127; motor[rightlift] = -127; } else { motor[leftlift] = 10; motor[rightlift] = 10; } } if(vexRT[Btn8L]==1) { if(SensorValue(pot) < 700){ motor[leftlift] = 50; motor[rightlift] = 50; } else if(SensorValue(pot) > 710){ motor[leftlift] = -50; motor[rightlift] = -50; } else if(SensorValue(pot) < 703){ motor[leftlift] = 35; motor[rightlift] = 35; } else if(SensorValue(pot) > 707){ motor[leftlift] = -35; motor[rightlift] = -35; } else { motor[leftlift] = 10; motor[rightlift] = 10; } } } }
task countBallsInIntake() { int numConsecZeros = 0, //number of consecutive 0s return by the limit switch numConsecOnes = 0; //number of consecutive 1s return by the limit switch while(1) { if (ballsInIntake < 3 && rollerState == 1) { //if there are 2 or fewer balls in the intake, and intaking balls in while(!SensorValue[intakeLimit]) { //limit switch indicates that a ball is not at the bottom of the dangle wait1Msec(25); //wait until a ball approaches the dangle } while(numConsecZeros < 2) { //condition: SensorValue[intakeLimit]; wait until the ball has triggered the limit switch if (!SensorValue[intakeLimit]) { numConsecZeros++; } else { numConsecZeros = 0; } wait1Msec(25); } } else if (ballsInIntake <= 3 && rollerState == -1) { //if there are 3 or fewer balls in the intake and we are outtaking via the roller while(!SensorValue[intakeLimit]) { //limit switch indicates that a ball is not at the bottom of the dangle wait1Msec(25); //wait until a ball approaches the dangle } while(numConsecZeros < 2) { //condition: SensorValue[intakeLimit]; wait until the ball has triggered the limit switch if (!SensorValue[intakeLimit]) { numConsecZeros++; } else { numConsecZeros = 0; } wait1Msec(25); } } else if (ballsInIntake >= 3 && rollerState == 1) { //limit switch is pressed when there are 4 balls in the intake - do this when the third ball is in the intake, so ballsInIntake = 3 while(numConsecOnes < 2) { //condition: !SensorValue[intakeLimit]; only do this when going forward - this means we are intaking if (SensorValue[intakeLimit]) { numConsecOnes++; } else { numConsecOnes = 0; } wait1Msec(25); } } else if (ballsInIntake > 3 && rollerState == -1) { //limit switch is pressed when there are 4 balls in the intake - do this when the fourth ball is in the intake, so ballsInIntake = 3 while(numConsecZeros < 2) { //condition: SensorValue[intakeLimit]; only do this when going backward - this means we are releasing balls from the lower end of the intake; the 4th ball keeps the limt switch pressed if (!SensorValue[intakeLimit]) { numConsecZeros++; } else { numConsecZeros = 0; } wait1Msec(25); } } numConsecZeros = 0; //reset to zero once limit we move on numConsecOnes = 0; //NOTE: this doesn't account for balls leaving the intake via the flywheel //reach this point once the intake limit switch has been pressed and then released (so balls are counted after they are done passing the dangle) if (rollerState == 1) { //if the roller is moving forward ballsInIntake++; //increment the number of balls in the intake } else if (rollerState == -1) { //if the roller is moving backwards (indicated by a rollerState value of -1) ballsInIntake--; //decrement the number of balls in the intake } //limit the ballsinIntake variable to 0-4 (inclusive) only if (ballsInIntake > 4) { ballsInIntake = 4; } else if (ballsInIntake < 0) { ballsInIntake = 0; } /* if (ballsInIntake < 4) { overrideAutoOuttakeOnly = false; } if (flywheelMode == 3 || flywheelMode == 4) { outtakeOnly = false; } else { if (ballsInIntake == 4) { if (vexRT[Btn6U] && !overrideAutoOuttakeOnly) { outtakeOnly = true; } else { outtakeOnly = false; overrideAutoOuttakeOnly = true; } } else if (ballsInIntake < 4 && outtakeOnly) { //check if we can disable outttake only mode outtakeOnly = false; } }*/ clearLCDLine(0); displayLCDPos(0,0); displayNextLCDNumber(ballsInIntake); } }
task main() { // Master CPU will not let competition start until powered on for at least 2-seconds clearLCDLine(0); clearLCDLine(1); displayLCDPos(0, 0); displayNextLCDString("Startup"); wait1Msec(2000); pre_auton(); //wait1Msec(500); while (true) { clearLCDLine(0); clearLCDLine(1); displayLCDPos(0, 0); while (bSimulatedRobotDisabled) { displayLCDPos(0, 0); displayNextLCDString("Disabled"); nTimeXX = 0; while (true) { displayStatusAndTime(); if (!bSimulatedRobotDisabled) break; wait1Msec(25); displayStatusAndTime(); if (!bSimulatedRobotDisabled) break; wait1Msec(25); displayStatusAndTime(); if (!bSimulatedRobotDisabled) break; wait1Msec(25); displayStatusAndTime(); if (!bSimulatedRobotDisabled) break; wait1Msec(25); ++nTimeXX; } } nTimeXX = 0; clearLCDLine(0); clearLCDLine(1); displayLCDPos(0, 0); if (bSimulatedAutonomousMode) { displayNextLCDString("Autonomous"); StartTask(autonomous); // Waiting for autonomous phase to end while (bSimulatedAutonomousMode && !bSimulatedRobotDisabled) { if (!bVEXNETActive) { if (nSimulatedRecieveState == vrNoXmiters) // the transmitters are powered off!! allMotorsOff(); } wait1Msec(25); // Waiting for autonomous phase to end } allMotorsOff(); allTasksStop(); } else { displayNextLCDString("User Control"); StartTask(usercontrol); // Here we repeat loop waiting for user control to end and (optionally) start // of a new competition run while (!bSimulatedAutonomousMode && !bSimulatedRobotDisabled) { if (nSimulatedRecieveState == vrNoXmiters) // the transmitters are powered off!! allMotorsOff(); wait1Msec(25); } allMotorsOff(); allTasksStop(); } } }
////// Display stuff on the LCD ////// task LCD() { bool ButtonPressed = false; string AutoName = "Nothing"; bLCDBacklight = true; // Turn on LCD Backlight string mainBattery, backupBattery; while(true) // An infinite loop to keep the program running until you terminate it { clearLCDLine(0); // Clear line 1 (0) of the LCD clearLCDLine(1); // Clear line 2 (1) of the LCD displayLCDPos(0, 0); //Display the Primary Robot battery voltage displayLCDString(0, 0, "Primary: "); sprintf(mainBattery, "%1.2f%c", nImmediateBatteryLevel/1000.0,'V'); //Build the value to be displayed displayNextLCDString(mainBattery); if (bIfiAutonomousMode){ displayLCDString(1, 0, "AUTO:"); displayLCDString(1, 6, AutoName); } else { //displayLCDNumber(1, 0, Heading); } if (nLCDButtons == 2){ if (AutoSelect > 0 && ButtonPressed == false){ AutoSelect--; } ButtonPressed = true; } else if (nLCDButtons == 4){ if (AutoSelect < 9 && ButtonPressed == false){ AutoSelect++; } ButtonPressed = true; } else { ButtonPressed = false; } switch(AutoSelect) { // SKILLS case 1: AutoName = "SKILLS"; break; // AUTO'S case 2: AutoName = "BALL AUTO"; break; //TEST CODE case 9: AutoName = "TEST"; break; default: AutoName = "NOTHING"; } } //Short delay for the LCD refresh rate wait1Msec(100); }
task main() { int i; const int kNumbOfMainSamples = 4000; bLCDBacklight = true; displayLCDPos(0, 0); displayNextLCDString("Gyro Noise Test "); memset(nSampleHisogram[0], 0, sizeof(nSampleHisogram)); wait1Msec(1000); // Calculate a rough mezsure of 'bias' sum that fits in 16-bit signed. nSampleSum = 0; const int kRoughBiasCounts = 30; for (i = 0; i < kRoughBiasCounts; ++i) { nSampleSum += SensorValue[gyro]; wait1Msec(1); } nBiasRough = nSampleSum / kRoughBiasCounts; nSampleSum = 0; for (i = 0; i < kNumbOfMainSamples; ++i) { nOffset = SensorValue[gyro] - nBiasRough; nSampleSum += nOffset; wait1Msec(1); } nBias = nBiasRough + nSampleSum / kNumbOfMainSamples; nBiasSmall = nSampleSum / (kNumbOfMainSamples / 100) - (nSampleSum / kNumbOfMainSamples) * 100; nBiasSmaller = nSampleSum / (kNumbOfMainSamples / 1000) - nBiasSmall * 10; StartTask(testMotorNoise); nLastCycles = -1; bool bDisplayIt; bool bOverflow = false; for (nCycles = 0; true; ++nCycles) { //ASSERT(nCycles == (nLastCycles + 1)); nLastCycles = nCycles; nTimeStamp = nPgmTime; nOffset = SensorValue[gyro] - nBias; nDrift += nOffset; if (nCycles >= 0) { if ((nCycles % 100) == 50) nDrift -= nBiasSmall; if ((nCycles % 1000) == 500) { nDrift -= nBiasSmaller; bDisplayIt = true; } else bDisplayIt = false; } else { if ((nCycles % 1000) == -50) nDrift -= nBiasSmall; if ((nCycles % 1000) == -500) { nDrift -= nBiasSmaller; bDisplayIt = true; } else bDisplayIt = false; } if (nOffset < -32) nOffset = -32; else if (nOffset > 32) nOffset = 32; nCount = nSampleHisogram[nOffset + 32]; if (nCount >= 32767) bOverflow = true; if (!bOverflow) ++nSampleHisogram[nOffset + 32]; if (true) { nPlusMinusWeighted += nOffset; if (nOffset < 0) --nPlusMinus; else if (nOffset > 0) ++nPlusMinus; } if (bDisplayIt) { ++nSeconds; displayLCDPos(1, 0); displayNextLCDNumber(nSeconds, 3); displayNextLCDString(" : "); if (nDrift > 0) { displayNextLCDNumber(nDrift / 1000); displayNextLCDChar('.'); displayNextLCDNumber(nDrift % 1000, -3); } else { displayNextLCDChar('-'); displayNextLCDNumber(- nDrift / 1000); displayNextLCDChar('.'); displayNextLCDNumber(- nDrift % 1000, -3); } displayNextLCDChar(' '); displayNextLCDChar(' '); } while (nTimeStamp == nPgmTime) {} } StopTask(testMotorNoise); }