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);
}
