task main() { bool valid = false; initializeRobot(); waitForStart(); // Get the lift going StartTask(Lift); waitLiftReady(); // Goal to side if(readSonarMax(5) > 130) { //Drive out to not hit wall driveToEncoder(AUTO_DRIVE_SPEED, 150); // Turn left, drive past the beacon, turn back driveToGyro(30, TURN_LEFT); driveToEncoder(AUTO_DRIVE_SPEED, 6250); driveToGyro(120, !TURN_LEFT); // Turn to IR alignment valid = driveToIR(-AUTO_DRIVE_SPEED, true, false, IR_MID); // Goal intermediate or ahead } else { // Drive ahead to get a second reading driveToEncoder(AUTO_DRIVE_SPEED, 2300); // Goal intermediate int sonar = readSonarMax(10); if (sonar == 0 || (sonar <= 90 && sonar >= 70)) { driveToGyro(80, TURN_LEFT); driveToEncoder(AUTO_DRIVE_SPEED, 2500); driveToGyro(115, !TURN_LEFT); // Turn to IR alignment valid = driveToIR(-AUTO_DRIVE_SPEED, true, false, IR_MID); // Goal ahead } else { valid = true; } } // Score if we are aligned if (valid) { valid = AutoScore(); } // Kick if we scored if (valid) { AutoKickstand(); } // Wiggle and flash if something went wrong if (!valid) { servoHookCapture(); wait1Msec(500); servoHookRelease(); } // Always return the lift to COLLECT, even if we failed setWaitLiftCmd(COLLECT); }
/* LCD Debugging Menu - Runs asynchronously WARNING: WHEN RUNNING POST ASYNCHRONOUSLY, DO NOT ATTEMPT JOYSTICK CONTROL */ task lcdMenu() { while(true) { wait1Msec(50); lcdClear(); if(nLCDButtons == LCD_L) { option++; wait1Msec(100); } else if (nLCDButtons == LCD_R) { option--; wait1Msec(100); } //Show options based on active menu switch(menu) { case MENU_MAIN: displayLCDString(0,3,"Main Menu"); switch(option) { case 0: option=6; break; case 1: displayLCDString(1,0,"< BATTERY LVL >"); if(nLCDButtons==LCD_M) { menu=MENU_BATT; option=1; wait10Msec(50); } break; case 2: displayLCDString(1,0,"< SELF-TEST >"); if(nLCDButtons==LCD_M) { powerOnSelfTest(); } break; case 3: displayLCDString(1,0,"< MANUAL TEST >"); if(nLCDButtons==LCD_M) { menu=MENU_MST; option=1; wait10Msec(50); } break; case 4: displayLCDString(1,0,"< PLAY MUSIC >"); if(nLCDButtons==LCD_M) { menu=MENU_MEMES; option=1; wait10Msec(50); } break; case 5: displayLCDString(1,0,"< SHOOTER SPD >"); if(nLCDButtons==LCD_M) { menu=MENU_SHOOTER; option=1; wait10Msec(50); } break; case 6: displayLCDString(1,0,"< POT SPD >"); if(nLCDButtons==LCD_M) { menu=MENU_POT; option=1; wait10Msec(50); } break; case 7: option = 1; break; default: displayLCDString(1,0,"< ERROR >"); break; } break; case MENU_BATT: lcdDisplayVoltage(); if(nLCDButtons == LCD_L) { menu=MENU_MAIN; option=1; wait10Msec(50); } break; case MENU_SHOOTER: lcdDisplayShooter(); if(nLCDButtons == LCD_L) { menu=MENU_MAIN; option=1; wait10Msec(50); } break; case MENU_MST: displayLCDString(0,0," Select Motor "); switch(option) { case 0: option=11; break; case 1: displayLCDString(1,0,"< Port 1 >"); if(nLCDButtons==LCD_M) { pulseMotor(port1); } break; case 2: displayLCDString(1,0,"< Port 2 >"); if(nLCDButtons==LCD_M) { pulseMotor(port2); } break; case 3: displayLCDString(1,0,"< Port 3 >"); if(nLCDButtons==LCD_M) { pulseMotor(port3); } break; case 4: displayLCDString(1,0,"< Port 4 >"); if(nLCDButtons==LCD_M) { pulseMotor(port4); } break; case 5: displayLCDString(1,0,"< Port 5 >"); if(nLCDButtons==LCD_M) { pulseMotor(port5); } break; case 6: displayLCDString(1,0,"< Port 6 >"); if(nLCDButtons==LCD_M) { pulseMotor(port6); } break; case 7: displayLCDString(1,0,"< Port 7 >"); if(nLCDButtons==LCD_M) { pulseMotor(port7); } break; case 8: displayLCDString(1,0,"< Port 8 >"); if(nLCDButtons==LCD_M) { pulseMotor(port8); } break; case 9: displayLCDString(1,0,"< Port 9 >"); if(nLCDButtons==LCD_M) { pulseMotor(port9); } break; case 10: displayLCDString(1,0,"< Port 10 >"); if(nLCDButtons==LCD_M) { pulseMotor(port10); } break; case 11: displayLCDString(1,0,"< Back >"); if(nLCDButtons==LCD_M) { menu=MENU_MAIN; option=1; wait10Msec(100); } break; case 12: option = 1; break; default: displayLCDString(1,0,"< ERROR >"); break; } break; case MENU_MEMES: switch(option) { case 0: option=8; break; case 1: displayLCDString(1,0,"< Beep >"); if(nLCDButtons==LCD_M) { beep(); } break; case 2: displayLCDString(1,0,"< Boop >"); if(nLCDButtons==LCD_M) { boop(); } break; case 3: displayLCDString(1,0,"< Dhoom >"); if(nLCDButtons==LCD_M) { StartTask(Dhoom); } break; case 4: displayLCDString(1,0,"< Take On Me >"); if(nLCDButtons==LCD_M) { StartTask(TakeOnMe); } break; case 5: displayLCDString(1,0,"< BigBoyRayC >"); if(nLCDButtons==LCD_M) { StartTask(PinkPanther); } break; case 6: displayLCDString(1,0,"< HotlineBling >"); if(nLCDButtons==LCD_M) { StartTask(HotlineBling); } break; case 7: displayLCDString(1,0,"< John Cena >"); if(nLCDButtons==LCD_M) { StartTask(JohnCena); } break; case 8: displayLCDString(1,0,"< Back >"); if(nLCDButtons==LCD_M) { option = 1; menu = MENU_MAIN; wait10Msec(100); } break; case 9: option = 1; break; default: displayLCDString(1,0,"< ERROR >"); break; } break; case MENU_POT: lcdDisplayPot(); if(nLCDButtons == LCD_L) { menu=MENU_MAIN; option=1; wait10Msec(50); } break; default: break; } } }
void startGyro() { StartTask(gyro); while (!GYRO_READY) { abortTimeslice(); } }
task main() { initializeRobot(); waitForStart(); // Wait for the beginning of autonomous phase. StartTask( Zelda_Theme ); int DistFrom = 30; // in Cm int Deadband = 2; int Distmin = 35; // In Inches float COUNTS_PER_INCH = 90.55; nMotorEncoder[R_Motor] = 0; nMotorEncoder[L_Motor] = 0; servo[IRS_1] = 160; servo[Block_Chuck] = 000; ClearTimer(T1); while (SensorValue[IR] != 6 && nMotorEncoder[ L_Motor] / COUNTS_PER_INCH < Distmin && nMotorEncoder[L_Motor] / COUNTS_PER_INCH < Distmin && time100[T1] < 50 ) { if ((USreadDist(SONAR_1) < DistFrom - Deadband) && (nMotorEncoder[ L_Motor] / COUNTS_PER_INCH > 18)) { motor[L_Motor] = 25; motor[R_Motor] = 50; } else if ((USreadDist(SONAR_1) > DistFrom + Deadband) && (nMotorEncoder[ L_Motor] / COUNTS_PER_INCH > 18)) { motor[L_Motor] = 50; motor[R_Motor] = 25; } else { motor[L_Motor] = 50; motor[R_Motor] = 50; } }//End of while. motor[L_Motor] = 0; motor[R_Motor] = 0; wait10Msec(55); servo[Block_Chuck] = 255; wait10Msec(55); servo[Block_Chuck] = 0; wait10Msec(55); while( USreadDist(SONAR_1) < 60) { if ((USreadDist(SONAR_1) < DistFrom - Deadband) && (nMotorEncoder[ L_Motor] / COUNTS_PER_INCH > 18)) { motor[L_Motor] = 25; motor[R_Motor] = 50; } else if ((USreadDist(SONAR_1) > DistFrom + Deadband) && (nMotorEncoder[ L_Motor] / COUNTS_PER_INCH > 18)) { motor[L_Motor] = 50; motor[R_Motor] = 25; } else { motor[L_Motor] = 50; motor[R_Motor] = 50; } /* if (time100[T1] > 60 || nMotorEncoder[L_Motor] / COUNTS_PER_INCH > 40) { motor[L_Motor] = 0; motor[R_Motor] = 0; noOp(); alive(); StopAllTasks(); wait10Msec(55); }*/ }//End of while motor[L_Motor] = 0; motor[R_Motor] = 0; wait10Msec(55); Turn(65); wait10Msec(55); Move(25,100); wait10Msec(55); Turn(75); wait10Msec(55); Move(30 ,100); wait10Msec(55); servo[Block_Chuck] = 250; wait10Msec(55); servo[Spring_Release] = 250; wait10Msec(55); servo[Spring_Release] = 0; wait10Msec(55); servo[Block_Chuck] = 0; wait10Msec(55); }
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); }
task main () { string BOFHserver = "192.168.0.100"; // Linux VM int BOFHport = 6666; string dataString; getFriendlyName(dataString); int avail = 0; nNxtButtonTask = -2; StartTask(updateScreen); // initialise the port, etc RS485initLib(); memset(RS485rxbuffer, 0, sizeof(RS485rxbuffer)); memset(RS485txbuffer, 0, sizeof(RS485txbuffer)); // Disconnect if already connected N2WDisconnect(); N2WchillOut(); if (!N2WCustomExist()) { StopTask(updateScreen); wait1Msec(50); eraseDisplay(); PlaySound(soundException); nxtDisplayCenteredBigTextLine(1, "ERROR"); nxtDisplayTextLine(3, "No custom profile"); nxtDisplayTextLine(4, "configured!!"); while(true) EndTimeSlice(); } N2WLoad(); wait1Msec(100); N2WConnect(true); connStatus = "connecting"; while (!N2WConnected()) wait1Msec(1000); connStatus = "connected"; PlaySound(soundBeepBeep); wait1Msec(3000); N2WgetIP(IPaddress); wait1Msec(1000); while (true) { while (nNxtButtonPressed != kEnterButton) EndTimeSlice(); while (nNxtButtonPressed != kNoButton) EndTimeSlice(); if (N2WTCPOpenClient(1, BOFHserver, BOFHport)) { data[0] = 0x0A; N2WTCPWrite(1, data, 1); txbytes++; // How many bytes are available in the buffers? avail = N2WTCPAvail(1); if (avail > 0) { sizeOfReceivedData = avail; rxbytes += avail; N2WchillOut(); // read the current buffer N2WTCPRead(1, avail); processData(); for (int i = 0; i < avail; i++) { writeDebugStream("%c", RS485rxbuffer[i]); } writeDebugStreamLine(""); } // Wait a bit N2WchillOut(); // Disconnect N2WTCPClose(1); } } }
BOOL CALLBACK Filesets_General_DlgProc (HWND hDlg, UINT msg, WPARAM wp, LPARAM lp) { if (AfsAppLib_HandleHelp (IDD_SET_GENERAL, hDlg, msg, wp, lp)) return TRUE; if (msg == WM_INITDIALOG) SetWindowLongPtr (hDlg, DWLP_USER, ((LPPROPSHEETPAGE)lp)->lParam); LPIDENT lpi = (LPIDENT)GetWindowLongPtr (hDlg, DWLP_USER); switch (msg) { case WM_INITDIALOG_SHEET: PropCache_Add (pcSET_PROP, (LPIDENT)lp, hDlg); break; case WM_DESTROY_SHEET: PropCache_Delete (hDlg); break; case WM_INITDIALOG: Filesets_General_OnInitDialog (hDlg, lpi); StartTask (taskSET_PROP_INIT, hDlg, lpi); NotifyMe (WHEN_OBJECT_CHANGES, lpi, hDlg, 0); break; case WM_DESTROY: DontNotifyMeEver (hDlg); break; case WM_ENDTASK: LPTASKPACKET ptp; if ((ptp = (LPTASKPACKET)lp) != NULL) { if (ptp->idTask == taskSET_PROP_INIT) Filesets_General_OnEndTask_InitDialog (hDlg, ptp, lpi); FreeTaskPacket (ptp); } break; case WM_NOTIFY_FROM_DISPATCH: StartTask (taskSET_PROP_INIT, hDlg, lpi); Delete ((LPNOTIFYSTRUCT)lp); break; case WM_COMMAND: switch (LOWORD(wp)) { case IDOK: case IDCANCEL: break; case IDAPPLY: Filesets_General_OnApply (hDlg, lpi); break; case IDC_SET_WARN: case IDC_SET_WARN_SETFULL: case IDC_SET_WARN_SETFULL_DEF: Filesets_General_OnWarnings (hDlg, lpi); PropSheetChanged (hDlg); break; case IDC_SET_WARN_SETFULL_PERCENT: PropSheetChanged (hDlg); break; case IDC_SET_QUOTA: if (Filesets_SetQuota (lpi)) { // this new task will block until the setquota req is done StartTask (taskSET_PROP_INIT, hDlg, lpi); } break; case IDC_SET_LOCK: StartTask (taskSET_LOCK, NULL, lpi); break; case IDC_SET_UNLOCK: StartTask (taskSET_UNLOCK, NULL, lpi); break; } break; } return FALSE; }
task main () { initializeRobot(); waitForStart(); StartTask(autoCover); while (1) { getJoystickSettings(joystick); //Holonomic Drive Variables const int t = 8; const float standardDriveScale = 0.79; const float precisionDriveScale = 0.15; int x1 = (abs(joystick.joy1_x1) > t) ? joystick.joy1_x1 : 0; int y1 = (abs(joystick.joy1_y1) > t) ? joystick.joy1_y1 : 0; int x2 = (abs(joystick.joy1_x2) > t) ? joystick.joy1_x2 : 0; //int y2 = (abs(joystick.joy1_y2) > t) ? joystick.joy1_y2 : 0; int stdFrontLeftMotorSetting = (- x1 - y1 + x2); int stdFrontRightMotorSetting = (x1 - y1 - x2); int stdRearLeftMotorSetting = (x1 - y1 + x2); int stdRearRightMotorSetting = (- x1 - y1 - x2); int egFrontLeftMotorSetting = x2; int egFrontRightMotorSetting = y1; int egRearLeftMotorSetting = y1; int egRearRightMotorSetting = x2; //Standard holonomic if (driveMode == DRIVE_MODE_STD) { motor [frontLeft] = ((joy1Btn (5) == 1) || (joy1Btn (6) == 1) || (joy1Btn (7) == 1) || (joy1Btn (8) == 1)) ? stdFrontLeftMotorSetting * precisionDriveScale : stdFrontLeftMotorSetting * standardDriveScale; motor [frontRight] = ((joy1Btn (5) == 1) || (joy1Btn (6) == 1) || (joy1Btn (7) == 1) || (joy1Btn (8) == 1)) ? stdFrontRightMotorSetting * precisionDriveScale : stdFrontRightMotorSetting * standardDriveScale; motor [rearLeft] = ((joy1Btn (5) == 1) || (joy1Btn (6) == 1) || (joy1Btn (7) == 1) || (joy1Btn (8) == 1)) ? stdRearLeftMotorSetting * precisionDriveScale : stdRearLeftMotorSetting * standardDriveScale; motor [rearRight] = ((joy1Btn (5) == 1) || (joy1Btn (6) == 1) || (joy1Btn (7) == 1) || (joy1Btn (8) == 1)) ? stdRearRightMotorSetting * precisionDriveScale : stdRearRightMotorSetting * standardDriveScale; } else if (driveMode == DRIVE_MODE_EG) { motor [frontLeft] = (egFrontLeftMotorSetting * precisionDriveScale - ((joy1Btn(7) == 1) ? 20 : (joy1Btn(8) == 1) ? -20 : 0)); motor [frontRight] = (egFrontRightMotorSetting * precisionDriveScale + ((joy1Btn(7) == 1) ? 20 : (joy1Btn(8) == 1) ? -20 : 0)); motor [rearLeft] = (egRearLeftMotorSetting * precisionDriveScale - ((joy1Btn(7) == 1) ? 20 : (joy1Btn(8) == 1) ? -20 : 0)); motor [rearRight] = (egRearRightMotorSetting * precisionDriveScale + ((joy1Btn(7) == 1) ? 20 : (joy1Btn(8) == 1) ? -20 : 0)); } if (joy1Btn(1) == 1 && joy1Btn(2) == 1 && joy1Btn(3) == 1 && joy1Btn(4) == 1) { if (driveMode == DRIVE_MODE_STD) { switchDriveMode(DRIVE_MODE_EG); } else if (driveMode == DRIVE_MODE_EG) { switchDriveMode(DRIVE_MODE_STD); } } if (joy2Btn(10) == 1) { if (mechMode == MECH_MODE_STD) { switchMechMode(MECH_MODE_INVERTED); } else if (mechMode == MECH_MODE_INVERTED) { switchMechMode(MECH_MODE_STD); } } if (mechMode == MECH_MODE_STD) { if (joy2Btn(7) == 1) { motor [arm] = -100; } else if (joy2Btn(5) == 1) { motor [arm] = 100; } else { motor [arm] = 0; } if (joy2Btn(6) == 1) { motor [leftLift] = 100; motor [rightLift] = 100; } else if (joy2Btn(8) == 1) { motor [leftLift] = -100; motor [rightLift] = -100; } else { motor [leftLift] = 0; motor [rightLift] = 0; } } else if (mechMode == MECH_MODE_INVERTED) { if (joy2Btn(7) == 1) { motor [arm] = 100; } else if (joy2Btn(5) == 1) { motor [arm] = -100; } else { motor [arm] = 0; } if (joy2Btn(6) == 1) { motor [leftLift] = -100; motor [rightLift] = -100; } else if (joy2Btn(8) == 1) { motor [leftLift] = 100; motor [rightLift] = 100; } else { motor [leftLift] = 0; motor [rightLift] = 0; } } if (joy2Btn(6) != 1 && joy2Btn(8) != 1) { motor [rightLift] = abs(joystick.joy2_y1) > t ? joystick.joy2_y1 * standardDriveScale : 0; motor [leftLift] = abs(joystick.joy2_y2) > t ? joystick.joy2_y2 * standardDriveScale : 0; } if (joystick.joy2_TopHat == 2) { servo [scoopCover] = 230; } else if (joystick.joy2_TopHat == 6) { servo [scoopCover] = 20; } if (joystick.joy2_TopHat == 0) { servo [leftLatch] = 20; servo [rightLatch] = 238; } else if (joystick.joy2_TopHat == 4) { servo [leftLatch] = 252; servo [rightLatch] = 10; } if (joy2Btn(1) == 1) { motor [flagSpinner] = -100; } else if (joy2Btn(3) == 1) { motor [flagSpinner] = 100; } else { motor [flagSpinner] = 0; } if (joy2Btn(2) == 1) { servo [spinnerLift] = 255; } else if (joy2Btn(4) == 1) { servo [spinnerLift] = 0; } else { servo [spinnerLift] = 127; } } }