task main() { int irsearch = 1; int dirIR1 = 0; int dirIR2 = 0; int dcS1_1, dcS2_1, dcS3_1, dcS4_1, dcS5_1 = 0; int dcS1_2, dcS2_2, dcS3_2, dcS4_2, dcS5_2 = 0; while (irsearch==1) { dirIR1 = HTIRS2readACDir(irsensor1); HTIRS2readAllACStrength(irsensor1, dcS1_1, dcS2_1, dcS3_1, dcS4_1, dcS5_1 ); //HTIRS2readEnhanced(irsensor1, dirEnh1, strEnh1); dirIR2 = HTIRS2readACDir(irsensor2); HTIRS2readAllACStrength(irsensor2, dcS1_2, dcS2_2, dcS3_2, dcS4_2, dcS5_2 ); //HTIRS2readEnhanced(irsensor2, dirEnh2, strEnh2); nxtDisplayTextLine(7, "%d,%d,%d,%d,%d", dcS1_2, dcS2_2, dcS3_2, dcS4_2, dcS5_2); nxtDisplayTextLine(6, "%d,%d,%d,%d,%d", dcS1_1, dcS2_1, dcS3_1, dcS4_1, dcS5_1); } }
task main() { eraseDisplay(); if (HTSMUXreadSensorType(IRL) == HTSMUXIRSeeker) { writeDebugStreamLine("IRL is HTSMUXIRSeeker"); } else if (HTSMUXreadSensorType(IRL) == HTSMUXIRSeekerNew ) { writeDebugStreamLine("IRL is HTSMUXIRSeekerNew"); } else { writeDebugStreamLine("IRL is unknown"); } if (HTSMUXreadSensorType(IRR) == HTSMUXIRSeeker) { writeDebugStreamLine("IRR is HTSMUXIRSeeker"); } else if (HTSMUXreadSensorType(IRR) == HTSMUXIRSeekerNew ) { writeDebugStreamLine("IRR is HTSMUXIRSeekerNew"); } else { writeDebugStreamLine("IRR is unknown"); } while(true) { eraseDisplay(); byte status = HTSMUXreadStatus(S1); displayTextLine(1, "Status: %d", status); if(status & HTSMUX_STAT_BATT == HTSMUX_STAT_BATT) { displayTextLine (1, "no power to MUX"); PlaySound(soundException); // PlaySound(soundBeepBeep); } int currentIRL = HTIRS2readACDir(IRL); int currentIRR = HTIRS2readACDir(IRR); int irl1, irl2, irl3, irl4, irl5 = -1; int irr1, irr2, irr3, irr4, irr5 = -1; HTIRS2readAllACStrength(IRL, irl1, irl2, irl3, irl4, irl5 ); HTIRS2readAllACStrength(IRR, irr1, irr2, irr3, irr4, irr5 ); displayTextLine(2, "D %d %d", currentIRL, currentIRR); displayTextLine(3, "0 %d %d", irl1, irr1); displayTextLine(4, "1 %d %d", irl2, irr2); displayTextLine(5, "2 %d %d", irl3, irr3); displayTextLine(6, "3 %d %d", irl4, irr4); displayTextLine(7, "4 %d %d", irl5, irr5); } }
task updateIRVals() { while(true) { if(!safe) { HTIRS2readAllACStrength(leftIRDev,leftFive[0],leftFive[1],leftFive[2],leftFive[3],leftFive[4]); HTIRS2readAllACStrength(rightIRDev,rightFive[0],rightFive[1],rightFive[2],rightFive[3],rightFive[4]); left = HTIRS2readACDir(leftIRDev); right = HTIRS2readACDir(rightIRDev); safe = true; } } }
// Y is our IR input, X is our dependant variable. task main() { clearDebugStream(); while(true) { sum_y = 0; //from _x->_y sum_xTimesy = 0; irInputSum = 0; for (int j = 0; j<20; j++) { HTIRS2readAllACStrength(IR, acS1, acS2, acS3, acS4, acS5); irInputSum += acS3; } irInputAvg = (irInputSum/20); for (int i = 0; i<20 ; i++) { HTIRS2readAllACStrength(IR, acS1, acS2, acS3, acS4, acS5); if (abs(irInputAvg-acS3) > 20) { acS3 = irInputAvg; } sum_y += acS3; //READINGS: Fill our array with data from the middle node //sum_y += irInput[i]; //CALCULATE: The sum of each value in the array. (its easiest this way) sum_xTimesy += (xInput[i] * acS3) ;//EQUATION: Top left half writeDebugStream("%i,", acS3); writeDebugStreamLine("0"); Sleep(10); }//APROOVED mean_xTimesy = (sum_x * sum_y)/20; //EQUATION: Top right half slopeLOBF = (sum_xTimesy-mean_xTimesy)/(sum_xSquared - mean_sum_xSquared); //CALCULATE: LOBF slope if (slopeLOBF<0) { PlayTone(1000,13); } //y_intLOBF = (sum_y/10) - (5.5 * slopeLOBF); //CALCULATE: LOBF y-intercept -> 5.5 = mean_x /* while (y_intLOBF >= y_intTH) //So long as we are above our y-intercept threshold AKA know we are close to the top of the curve { if (slopeLOBF <= slopeTH || slopeTH >=) //Wait untill our LOBF slope falls within our calibrated threshold { //Make beeping noise perhaps turn left because we have determined the IRSeekerV2 to be facing the IRBeacon } } */ writeDebugStream("0,"); writeDebugStreamLine("%f, ", slopeLOBF); Sleep(50); } }
void getcenterpos() { //default is 2 because #2 routine works for positions 1 & 2 int acS1, acS2, acS3, acS4, acS5;//irsensor values // int acS1avg=0, acS2avg=0, acS3avg=0, acS4avg=0, acS5avg=0; //these were for testing the irsensor // int prevacS1, prevacS2, prevacS3, prevacS4, prevacS5; //these were for testing the irsensor int irthreshold=58.5;//to decide if position 3 or something else int irthreshold2=10;//to decide if position 2 or 1 HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5);//used to get ir sensor values if(acS4>irthreshold)//finding position of centerpiece { centerpos=3; } else if(acS4>irthreshold2) { centerpos=2; } else { centerpos=1; } }
// main task task main () { // dc and ac directional values. int _dirDC = 0; int _dirAC = 0; // DC and AC values from 5 internal detectors. int dcS1, dcS2, dcS3, dcS4, dcS5 = 0; int acS1, acS2, acS3, acS4, acS5 = 0; // we are going to set DSP mode to 1200 Hz. tHTIRS2DSPMode _mode = DSP_1200; // attempt to set to DSP mode. if (HTIRS2setDSPMode(HTIRS2, _mode) == 0) { // unsuccessful at setting the mode. // display error message. eraseDisplay(); nxtDisplayCenteredTextLine(0, "ERROR!"); nxtDisplayCenteredTextLine(2, "Init failed!"); nxtDisplayCenteredTextLine(3, "Connect sensor"); nxtDisplayCenteredTextLine(4, "to Port 2."); // make a noise to get their attention. PlaySound(soundDownwardTones); // wait so user can read message, then leave main task. wait10Msec(300); return; } // initialize the array sTextLines. for (int i = 0; i < 8; ++i) sTextLines[i] = ""; // display some header info at top of screen eraseDisplay(); nxtDisplayTextLine(0, " Piece Of Cake "); nxtDisplayTextLine(1, "------3763-------"); // loop continuously and read from the sensor. while(true) { // Read the current non modulated signal direction //_dirDC = HTIRS2readDCDir(HTIRS2); // if (_dirDC < 0) //break; // I2C read error occurred //// read the current modulated signal direction _dirAC = HTIRS2readACDir(HTIRS2); if (_dirAC < 0) break; // I2C read error occurred //// Read the individual signal strengths of the internal sensors //// Do this for both unmodulated (DC) and modulated signals (AC) //if (!HTIRS2readAllDCStrength(HTIRS2, dcS1, dcS2, dcS3, dcS4, dcS5)) // break; // I2C read error occurred if (!HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5 )) break; // I2C read error occurred nxtDisplayTextLine(2, "Position %d", _dirAC); nxtDisplayTextLine(3, "Strength0 %d", acS1); nxtDisplayTextLine(4, "Strength1 %d", acS2); nxtDisplayTextLine(5, "Strength2 %d", acS3); nxtDisplayTextLine(6, "Strength3 %d", acS4); nxtDisplayTextLine(7, "Strength4 %d", acS5); // wait a little before resuming. wait10Msec(5); } }
task main() { HTIRS2setDSPMode(irsensor, DSP_1200); HTIRS2setDSPMode(irsensor2, DSP_1200); while(true) { int irArray[5]; int irArray2[5]; HTIRS2readAllACStrength(irsensor, irArray[0], irArray[1], irArray[2], irArray[3], irArray[4]); HTIRS2readAllACStrength(irsensor2, irArray2[0], irArray2[1], irArray2[2], irArray2[3], irArray2[4]); int p = (irArray[1]-irArray[2])-10; writeDebugStream("(%d) ",p); for (int i = p+20; i > 0;i-=2){ writeDebugStream("#"); } } }
// main task task main () { int _dirDC = 0; int _dirAC = 0; int dcS1, dcS2, dcS3, dcS4, dcS5 = 0; int acS1, acS2, acS3, acS4, acS5 = 0; string tmpString; // show the user what to do displayInstructions(); eraseDisplay(); for (int i = 0; i < 8; ++i) sTextLines[i] = ""; // display the current DSP mode // When connected to a SMUX, the IR Seeker V2 can only be // used in 1200Hz mode. nxtDisplayTextLine(0, " DC 1200"); // The sensor is connected to the first port // of the SMUX which is connected to the NXT port S1. // To access that sensor, we must use msensor_S1_1. If the sensor // were connected to 3rd port of the SMUX connected to the NXT port S4, // we would use msensor_S4_3 while (true) { // Read the current non modulated signal direction _dirDC = HTIRS2readDCDir(HTIRS2); if (_dirDC < 0) break; // I2C read error occurred // read the current modulated signal direction _dirAC = HTIRS2readACDir(HTIRS2); if (_dirAC < 0) break; // I2C read error occurred // Read the individual signal strengths of the internal sensors // Do this for both unmodulated (DC) and modulated signals (AC) if (!HTIRS2readAllDCStrength(HTIRS2, dcS1, dcS2, dcS3, dcS4, dcS5)) break; // I2C read error occurred if (!HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5 )) break; // I2C read error occurred displayText(1, "D", _dirDC, _dirAC); displayText(2, "0", dcS1, acS1); displayText(3, "1", dcS2, acS2); displayText(4, "2", dcS3, acS3); displayText(5, "3", dcS4, acS4); displayText(6, "4", dcS5, acS5); if (HTSMUXreadPowerStatus(HTSMUX)) nxtDisplayTextLine(7, "Batt: bad"); else nxtDisplayTextLine(7, "Batt: good"); } }
int getSeeker() { int _dirDC = 0; int _dirAC = 0; int dcS1, dcS2, dcS3, dcS4, dcS5 = 0; int acS1, acS2, acS3, acS4, acS5 = 0; // set the DSP to the new mode if ( ! HTIRS2setDSPMode(seeker, DSP_1200)) return -1; // Sensor initialized // Read the current non modulated signal direction _dirDC = HTIRS2readDCDir(seeker); if (_dirDC < 0) return -1; // I2C read error occurred // read the current modulated signal direction _dirAC = HTIRS2readACDir(seeker); if (_dirAC < 0) return -1; // I2C read error occurred // Read the individual signal strengths of the internal sensors // Do this for both unmodulated (DC) and modulated signals (AC) if (!HTIRS2readAllDCStrength(seeker, dcS1, dcS2, dcS3, dcS4, dcS5)) return -1; // I2C read error occurred if (!HTIRS2readAllACStrength(seeker, acS1, acS2, acS3, acS4, acS5 )) return -1; // I2C read error occurred count ++; writeDebugStreamLine("D %d %d", count, _dirAC); //writeDebugStreamLine("0 %d %d", dcS1, acS1); //writeDebugStreamLine("1 %d %d", dcS2, acS2); //writeDebugStreamLine("2 %d %d", dcS3, acS3); //writeDebugStreamLine("3 %d %d", dcS4, acS4); //writeDebugStreamLine("4 %d %d", dcS5, acS5); if(_dirAC == 2)// && acS2 < 50 && acS4 < 50) // make sure that the beacon is right in front of the sensor { static int debugPrinted = 0; PlayTone(440<<2, 10); if (!debugPrinted){ debugPrinted = true; writeDebugStreamLine("Played sound at %d degrees", gyroVal); //basketNumber = degreesToBasket(gyroVal); } return 1; } else if(_dirAC == 3) return 2; else if(_dirAC == 4) return 3; else { return -1*_dirAC; } }
float strengthRight(){ int ac1,ac2,ac3,ac4,ac5 = 0; // reading of all sensors to compute "strength" float s; HTIRS2readAllACStrength(irRight, ac1, ac2, ac3, ac4, ac5 ); // reading the strength of all the sensors of the left beacon s = ac1+ac2+ac3+ac4+ac5; return(s); }
///////////////////////////////////////////////////////////////////////////////////////////////////// // // Main Task // // The following is the main code for the tele-op robot operation Customize as appropriate for // your specific robot // // Game controller / joystick information is sent periodically (about every 50 milliseconds) from // the FMS (Field Management System) to the robot Most tele-op programs will follow the following // logic: // 1 Loop forever repeating the following actions: // 2 Get the latest game controller / joystick settings that have been received from the PC // 3 Perform appropriate actions based on the joystick + buttons settings This is usually a // simple action: // * Joystick values are usually directly translated into power levels for a motor or // position of a servo // * Buttons are usually used to start/stop a motor or cause a servo to move to a specific // position // 4 Repeat the loop // // Your program needs to continuously loop because you need to continuously respond to changes in // the game controller settings // // At the end of the tele-op period the FMS will autonmatically abort (stop) execution of the program // ///////////////////////////////////////////////////////////////////////////////////////////////////// bool bAtBeacon(void) { int zones[5]; HTIRS2readAllACStrength(IR, zones[0], zones[1], zones[2], zones[3], zones[4]); clearDebugStream(); for(int i = 0; i < 5; i++) { writeDebugStreamLine("zone %d:\t%d", i, zones[i]); } return ((zones[1] > 50) && (zones[2] > 50) && (abs(zones[1] - zones[2]) < 10)); }
task infared(){ int _dirAC1 = 0; //int 1acS1, 1acS2, 1acS3, 1acS4, 1acS5 = 0; int _dirAC2 = 0; //int 2ac2S1, 2acS2, 2acS3, 2acS4, 2acS5 = 0; while(true){ _dirAC1 = HTIRS2readACDir(IRS1); if (_dirAC1 < 0) break; // I2C read error occurred _dirAC2 = HTIRS2readACDir(IRS2); if (_dirAC2 < 0) break; // I2C read error occurred if (!HTIRS2readAllACStrength(IRS1, acS1a, acS2a, acS3a, acS4a, acS5a)) break; // I2C read error occurred if (!HTIRS2readAllACStrength(IRS2, acS1b, acS2b, acS3b, acS4b, acS5b)) break; // I2C read error occurred } }
task main() { float r0 = getIRDir(sensorIR)-8, r1; if(r0 > 0) rbtArcRight(-7); else rbtArcLeft(20); rbtMoveFdDist(-10, 5000); ClearTimer(T1); while(time1[T1] < 2000) { r1 = getIRDir(sensorIR)-8; int acS[5]; HTIRS2readAllACStrength(sensorIR, acS[0], acS[1], acS[2], acS[3], acS[4]); if(r0 > 0) {setLeftMotors(acS[4] > acS[3] ? -6 : -50); setRightMotors(acS[4] > acS[3] ? -50 : -8);} else {setLeftMotors(acS[4] > acS[3] ? -6 : -90); setRightMotors(acS[4] > acS[3] ? -30 : 0);} } setLeftMotors(0); setRightMotors(0); int cr = (r0 > 0 ? (r1 > 0 ? 1 : 2) : (r1 > 0 ? 3 : 4)); nxtDisplayBigTextLine(3, "%f", cr); for(;;); }
void searchDirection() { int _dirAC = 0; int trueDir = 0; int maxSig = 0; int acS1, acS2, acS3, acS4, acS5; zeroEncoder(); // IR direction search algorithm while(_dirAC != 5) { // Read the current IR signal directions _dirAC = HTIRS2readACDir(IRSeeker); if ((_dirAC < 0)) { writeDebugStreamLine("Read dir ERROR!"); break; // I2C read error occurred } // Read the individual signal strengths of the internal sensors if (!HTIRS2readAllACStrength(IRSeeker, acS1, acS2, acS3, acS4, acS5)) { writeDebugStreamLine("Read dir ERROR!"); break; // I2C read error occurred } else { // find the max sig strength of all detectors maxSig = (acS1 > acS2) ? acS1 : acS2; maxSig = (maxSig > acS3) ? maxSig : acS3; maxSig = (maxSig > acS4) ? maxSig : acS4; maxSig = (maxSig > acS5) ? maxSig : acS5; } // which way to go? // 0 = no signal found // 1 = far left (~8 o'clock) // 5 = straight ahead (~12 o'clock) // 9 = far right (~4 o'clock) encoderCount = (nMotorEncoder[Lwheel] + nMotorEncoder[Rwheel]) / 2; // raw, dirty direction -> nice, workable direction! :-) trueDir = abs(_dirAC - 5); // only concerned with moving back/forward motor[Lwheel] = 20 + 5 * trueDir; motor[Rwheel] = 20 + 5 * trueDir; wait1Msec(20); } }
// Move foward until the beacon is found. void MovetoIR() { int FindState = 1; bool FoundIt = false; nMotorEncoder[motorL] = 0; nMotorEncoder[motorR] = 0; wait1Msec(200); // Start moving. driveMotors(DRIVE_SPEED, DRIVE_SPEED); while(!FoundIt) { switch (FindState) { case 1: // Look for target // Get the direction. _dirAC = HTIRS2readACDir(IRseeker); // Make 0 straight ahead, all positive, no left or right worry. _dirAC = abs(_dirAC - 5); // Get the strength. nxtDisplayTextLine(1, "IR: %d", _dirAC); HTIRS2readAllACStrength(IRseeker, acS1, acS2, acS3, acS4, acS5); maxSig = (acS1 > acS2) ? acS1 : acS2; maxSig = (maxSig > acS3) ? maxSig : acS3; maxSig = (maxSig > acS4) ? maxSig : acS4; maxSig = (maxSig > acS5) ? maxSig : acS5; nxtDisplayTextLine(2, "maxSig: %d", maxSig); wait10Msec(2); if (_dirAC >= irGoal) { StopMotors(); DistanceToIR = nMotorEncoder[motorR]; FindState++; } break; case 2: // Look for strongest signal. FindState++; break; case 3: // Backup a little. FindState++; FoundIt = true; break; } } }
// main task task main () { int _dirDC = 0; int _dirAC = 0; int dcS1, dcS2, dcS3, dcS4, dcS5 = 0; int acS1, acS2, acS3, acS4, acS5 = 0; // the default DSP mode is 1200 Hz. tHTIRS2DSPMode _mode = DSP_1200; while(true) { // You can switch between the two different DSP modes by pressing the // orange enter button for (int i = 0; i < 8; ++i) sTextLines[i] = ""; // display the current DSP mode nxtDisplayTextLine(0, " DC 1200"); // Read the current non modulated signal direction _dirDC = HTIRS2readDCDir(HTIRS2); if (_dirDC < 0) break; // I2C read error occurred // read the current modulated signal direction _dirAC = HTIRS2readACDir(HTIRS2); if (_dirAC < 0) break; // I2C read error occurred // Read the individual signal strengths of the internal sensors // Do this for both unmodulated (DC) and modulated signals (AC) if (!HTIRS2readAllDCStrength(HTIRS2, dcS1, dcS2, dcS3, dcS4, dcS5)) break; // I2C read error occurred if (!HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5 )) break; // I2C read error occurred displayText(1, "D", _dirDC, _dirAC); displayText(2, "0", dcS1, acS1); displayText(3, "1", dcS2, acS2); displayText(4, "2", dcS3, acS3); displayText(5, "3", dcS4, acS4); displayText(6, "4", dcS5, acS5); } }
// main task task main () { // dc and ac directional values. int _dirDC = 0; int _dirAC = 0; // DC and AC values from 5 internal detectors. int dcS1, dcS2, dcS3, dcS4, dcS5 = 0; int acS1, acS2, acS3, acS4, acS5 = 0; // we are going to set DSP mode to 1200 Hz. tHTIRS2DSPMode _mode = DSP_1200; // attempt to set to DSP mode. // initialize the array sTextLines. for (int i = 0; i < 8; ++i) sTextLines[i] = ""; // display some header info at top of screen eraseDisplay(); nxtDisplayTextLine(0, " DC AC"); nxtDisplayTextLine(1, "-----------------"); // loop continuously and read from the sensor. while(true) { _dirDC = HTIRS2readDCDir(HTIRS2); _dirAC = HTIRS2readACDir(HTIRS2); HTIRS2readAllDCStrength(HTIRS2, dcS1, dcS2, dcS3, dcS4, dcS5); HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5 ); string a = "D"; string b = "0"; string c = "1"; string d = "2"; string e = "3"; string f = "4"; displayText(2, a, _dirDC, _dirAC); displayText(3, b, dcS1, acS1); displayText(4, c, dcS2, acS2); displayText(5, d, dcS3, acS3); displayText(6, e, dcS4, acS4); displayText(7, f, dcS5, acS5); // wait a little before resuming. wait10Msec(5); } }
void searchStrength() { int acS1, acS2, acS3, acS4, acS5; int acTemp[3]; bool offTarget = true; // IR max strength search while(offTarget) { // get input for (int i = 0; i <= 2; ++i) { motor[Lwheel] = 15; motor[Rwheel] = 15; wait10Msec(50); // Read the individual signal strengths of the internal sensors if (!HTIRS2readAllACStrength(IRSeeker, acS1, acS2, acS3, acS4, acS5)) { writeDebugStreamLine("Read dir ERROR!"); break; // I2C read error occurred } else { nxtDisplayCenteredBigTextLine(3, "Sig=%d", acS3); acS3 = acTemp[i]; } } // compare 3 IR strength readings if ((acTemp[2] > acTemp[1]) && (acTemp[2] > acTemp[0])) { motor[Lwheel] = 15; motor[Rwheel] = 15; wait10Msec(20); } else if ((acTemp[2] < acTemp[1]) && (acTemp[2] > acTemp[0])) { motor[Lwheel] = -13; motor[Rwheel] = -13; wait10Msec(20); } else { offTarget = false; stopRobot(); } wait1Msec(500); } }
int getSeeker() { int _dirDC = 0; int _dirAC = 0; int dcS1, dcS2, dcS3, dcS4, dcS5 = 0; int acS1, acS2, acS3, acS4, acS5 = 0; // set the DSP to the new mode if ( ! HTIRS2setDSPMode(seeker, DSP_1200)) return -1; // Sensor initialized // Read the current non modulated signal direction _dirDC = HTIRS2readDCDir(seeker); if (_dirDC < 0) return -1; // I2C read error occurred // read the current modulated signal direction _dirAC = HTIRS2readACDir(seeker); if (_dirAC < 0) return -1; // I2C read error occurred // Read the individual signal strengths of the internal sensors // Do this for both unmodulated (DC) and modulated signals (AC) if (!HTIRS2readAllDCStrength(seeker, dcS1, dcS2, dcS3, dcS4, dcS5)) return -1; // I2C read error occurred if (!HTIRS2readAllACStrength(seeker, acS1, acS2, acS3, acS4, acS5 )) return -1; // I2C read error occurred writeDebugStreamLine("D %d %d", _dirDC, _dirAC); writeDebugStreamLine("0 %d %d", dcS1, acS1); writeDebugStreamLine("1 %d %d", dcS2, acS2); writeDebugStreamLine("2 %d %d", dcS3, acS3); writeDebugStreamLine("3 %d %d", dcS4, acS4); writeDebugStreamLine("4 %d %d", dcS5, acS5); if(acS3 > 100 && acS2 < 50 && acS4 < 50) // make sure that the beacon is right in front of the sensor return 1; else return 0; }
/* Checks to see if we are next to the beacon */ bool bAtBeacon(void) { /* Declare variables */ int zones[5]; int average = 0; bool atBeacon = false; /* Read the infrared seeker into an array */ HTIRS2readAllACStrength(IR, zones[0], zones[1], zones[2], zones[3], zones[4]); /* Find the average infrared noise level */ for(int i = 0; i < 5; i++) average += zones[i]; average /= 5; /* If the critical zones are stronger than the noise, we are in range */ if(((zones[1] - average) > 10) && ((zones[2] - average) > 10)) inRange = true; /* When we are in range and the critical zones are nearly equal (or we have overshot) we are at the beacon */ if(inRange && ((zones[1] - zones[2]) > -15)) atBeacon = true; return atBeacon; }
int findIRBeacon_v2() { int acS1, acS2, acS3, acS4, acS5, _dirAC = 0; int max = 0; bool done; int dir; tHTIRS2DSPMode _mode = DSP_1200; HTIRS2setDSPMode(IRSeeker, _mode); moveBackwardOn(50); while (!done) { HTIRS2readAllACStrength(IRSeeker, acS1, acS2, acS3, acS4, acS5); dir = HTIRS2readACDir(IRSeeker); if (dir == 5) { rangeFind(); } } return 0; }
task main() { initializeRobot(); // waitForStart(); // Wait for the beginning of autonomous phase. /////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////// //// //// //// Add your robot specific autonomous code here. //// //// //// /////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////// // CONFIGURATION VARIABLES // int driveTime = 3000; //in MILLISECONDS 1000 MS = 1 second // int bucketDump = 1600; int bucketStop = 800; int L_Flip_Open = 15; int L_Flip_Close = 150; int R_Flip_Close = 85; int R_Flip_Open = 230; /////////////////////////////////////////////////////// // BEGIN AUTONOMOUS ROUTINE // /////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////// // BEGIN IR ROUTINE ///////////////////////////////////////////////////////////// int _dirAC = 0; int acS1, acS2, acS3, acS4, acS5 = 0; int maxSig = 0; // the max signal strength from the seeker. int val = 0; // the translated directional value. // we are going to set DSP mode to 1200 Hz. tHTIRS2DSPMode _mode = DSP_1200; // attempt to set to DSP mode. if (HTIRS2setDSPMode(HTIRS2, _mode) == 0) { // unsuccessful at setting the mode. // display error message. eraseDisplay(); nxtDisplayCenteredTextLine(0, "ERROR!"); nxtDisplayCenteredTextLine(2, "Init failed!"); nxtDisplayCenteredTextLine(3, "Connect sensor"); nxtDisplayCenteredTextLine(4, "to Port 1."); // make a noise to get their attention. PlaySound(soundBeepBeep); // wait so user can read message, then leave main task. wait10Msec(300); return; } eraseDisplay(); // loop continuously and read from the sensor. while(true) { // read the current modulated signal direction _dirAC = HTIRS2readACDir(HTIRS2); if (_dirAC < 0) { // error! - write to debug stream and then break. writeDebugStreamLine("Read dir ERROR!"); break; } // Get the AC signal strength values. if (!HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5 )) { // error! - write to debug stream and then break. writeDebugStreamLine("Read sig ERROR!"); break; } else { // find the max signal strength of all detectors. maxSig = (acS1 > acS2) ? acS1 : acS2; maxSig = (maxSig > acS3) ? maxSig : acS3; maxSig = (maxSig > acS4) ? maxSig : acS4; maxSig = (maxSig > acS5) ? maxSig : acS5; } // display info nxtDisplayCenteredBigTextLine(1, "Dir=%d", _dirAC); nxtDisplayCenteredBigTextLine(4, "Sig=%d", maxSig); // figure out which direction to go... // a value of zero means the signal is not found. // 1 corresponds to the far left (approx. 8 o'clock position). // 5 corresponds to straight ahead. // 9 corresponds to far right. // first translate directional index so 0 is straight ahead. val = _dirAC - 5; // calculate left and right motor speeds. motor[Left_drive] = 30 * val; motor[Right_drive] = 30 * val; // wait a little before resuming. wait10Msec(2); } ///////////////////////////////////////////////////////////// // END IR ROUTINE ///////////////////////////////////////////////////////////// /* // Drive forward for drivetime @ 20% power // motor[Left_drive] = -20; motor[Right_drive] = -20; wait1Msec(driveTime); motor[Left_drive] = 0; motor[Right_drive] = 0; // Set Flippers to Open // servo[Left_Flipper] = L_Flip_Open; servo[Right_Flipper] = R_Flip_Open; wait1Msec(2000); // Rotate clockwise for 1000ms @ 20% power // motor[Left_drive] = -20; motor[Right_drive] = 20; wait1Msec(2000); motor[Left_drive] = 0; motor[Right_drive] = 0; // Perform lift cycle // nMotorEncoderTarget[bucket] = bucketDump; // target bucket dumping position motor[bucket] = -25; while(nMotorRunState[bucket] != runStateIdle) // While Motor is still running, allow to go to target posititon { // Do not continue. } motor[bucket] = 0; // if(SensorValue(touchBucketZero)== 0) // { nMotorEncoderTarget[bucket] = bucketStop; // target bucket drive position motor[bucket] = 25; while(nMotorRunState[bucket] != runStateIdle) // While Motor is still running, allow to go to target posititon { // Do not continue. } motor[bucket] = 0; // Set Flippers to Close // servo[Left_Flipper] = L_Flip_Close; servo[Right_Flipper] = R_Flip_Close; wait1Msec(1000); // Rotate counterclockwise for 1000ms @ 20% power // motor[Left_drive] = -20; motor[Right_drive] = 20; wait1Msec(2000); motor[Left_drive] = 0; motor[Right_drive] = 0; // Drive forward for 1000ms = drivetime/3 @ 20% power // motor[Left_drive] = 20; motor[Right_drive] = 20; wait1Msec(driveTime/3); motor[Left_drive] = 0; motor[Right_drive] = 0; // Rotate counterclockwise for 500ms @ 20% power // motor[Left_drive] = -20; motor[Right_drive] = 20; wait1Msec(2000); motor[Left_drive] = 0; motor[Right_drive] = 0; // Drive forward for 3500ms @ 20% power // motor[Left_drive] = 40; motor[Right_drive] = 40; wait1Msec(2500); motor[Left_drive] = 0; motor[Right_drive] = 0; //////////////////////////// // End Autonomous Routine // //////////////////////////// */ }
task main() { int highest_seen_value = 0; // TODO go forward here // wait for the cliff in IR readings, then break while(acS1 < cliff_drop_threshold) { HTIRS2readAllACStrength(IR, acS1, acS2, acS3, acS4, acS5); } nxtDisplayString(3, "hit cliff"); // start trying to find the spike bool going_down = false; while(!going_down) { HTIRS2readAllACStrength(IR, acS1, acS2, acS3, acS4, acS5); int previous_ir_value[10]; int previous_ir_value_difference[9]; // set the new highest seen value if (highest_seen_value < acS3) highest_seen_value = acS3; for (int i = 0; i < 10; i++) { int number_of_negative_ir_differences = 0; // add the new data if (i < 10) { // we're in the middle (or start) of the stack, shift lower previous_ir_value[i] = previous_ir_value[i+1]; } else { // i == 10, so we're at the top of the stack // populate with the new data previous_ir_value[i] = acS3; } // if this is negative, then we're probably going down previous_ir_value_difference[i] = previous_ir_value[i+1] - previous_ir_value[i]; if (previous_ir_value_difference[i] < 0) { number_of_negative_ir_differences++; } // determine if we're going down with a threshold of 7/9 or more if (number_of_negative_ir_differences >= going_down_threshold) going_down = true; } } // now the trend is definitely going down, let's back up and find the highest value // TODO this will stop slightly before the top nxtDisplayString(1, "pull me backwards"); // TODO go back here bool reached_highest_value = false; while(!reached_highest_value) { HTIRS2readAllACStrength(IR, acS1, acS2, acS3, acS4, acS5); if (highest_seen_value + curve_top_change_threshold > acS3 && acS3 < highest_seen_value - curve_top_change_threshold) { reached_highest_value = true; } } }
// main task task main () { int last = 0; int stage = 1; int _dirDC = 0; int _dirAC = 0; int dcS1, dcS2, dcS3, dcS4, dcS5 = 0; int acS1, acS2, acS3, acS4, acS5 = 0; // the default DSP mode is 1200 Hz. tHTIRS2DSPMode _mode = DSP_1200; // show the user what to do displayInstructions(); while(true) { // You can switch between the two different DSP modes by pressing the // orange enter button PlaySound(soundBeepBeep); while(bSoundActive) {} eraseDisplay(); nNumbCyles = 0; ++nInits; while (true) { if ((nNumbCyles & 0x04) == 0) nxtDisplayTextLine(0, "Initializing..."); else nxtDisplayTextLine(0, ""); nxtDisplayCenteredBigTextLine(1, "IR Seekr"); // set the DSP to the new mode if (HTIRS2setDSPMode(HTIRS2, _mode)) break; // Sensor initialized ++nNumbCyles; PlaySound(soundShortBlip); nxtDisplayTextLine(4, "Inits: %d / %d", nInits, nNumbCyles); nxtDisplayCenteredTextLine(6, "Connect Sensor"); nxtDisplayCenteredTextLine(7, "to Port S2"); wait1Msec(100); } eraseDisplay(); for (int i = 0; i < 8; ++i) sTextLines[i] = ""; // display the current DSP mode if (_mode == DSP_1200) nxtDisplayTextLine(0, " DC 1200"); else nxtDisplayTextLine(0, " DC 600"); while (true) { ++nNumbCyles; if (nNxtButtonPressed == kEnterButton) { // "Enter" button has been pressed. Need to switch mode _mode = (_mode == DSP_1200) ? DSP_600 : DSP_1200; while(nNxtButtonPressed == kEnterButton) { // Wait for "Enter" button release } break; } // Read the current non modulated signal direction _dirDC = HTIRS2readDCDir(HTIRS2); if (_dirDC < 0) break; // I2C read error occurred // read the current modulated signal direction _dirAC = HTIRS2readACDir(HTIRS2); if (_dirAC < 0) break; // I2C read error occurred // Read the individual signal strengths of the internal sensors // Do this for both unmodulated (DC) and modulated signals (AC) if (!HTIRS2readAllDCStrength(HTIRS2, dcS1, dcS2, dcS3, dcS4, dcS5)) break; // I2C read error occurred if (!HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5 )) break; // I2C read error occurred displayText(1, "D", _dirDC, _dirAC); displayText(2, "0", dcS1, acS1); displayText(3, "1", dcS2, acS2); displayText(4, "2", dcS3, acS3); displayText(5, "3", dcS4, acS4); displayText(6, "4", dcS5, acS5); //displayText(7, "Last", dcS5, acS5); //nxtDisplayTextLine(7, "Last: "+last); /* if(acS3 >= 50){ int speed = 20; motor[DFR] = -speed; motor[DFL]= -speed; motor[DBR] = -speed; motor[DBL]= -speed; } else if(acS4+acS5 >= 10){ //Right int speed = 20; motor[DFR] = speed; motor[DFL]= -speed; motor[DBR] = speed; motor[DBL]= -speed; } else if(acS1+acS2 >= 10){ //Left int speed = 20; motor[DFR] = -speed; motor[DFL]= speed; motor[DBR] = -speed; motor[DBL]= speed; } else{ motor[DFR] = 0; motor[DFL]= 0; motor[DBR] = 0; motor[DBL]= 0; } */ while(stage == 1){ int acS1, acS2, acS3, acS4, acS5 = 0; HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5 ); if(acS1 > acS2-100&&last>10){ motor[driveL]= motor[driveL2] = motor[driveR] = motor[driveR2] = -20; last = -1; stage = 2; wait1Msec(1000); motor[driveL]= motor[driveL2] = motor[driveR] = motor[driveR2] = 0; } else{ if(last<acS1) last = acS1; int speed = 90-3*(acS2 + acS3); if (speed < 9) speed = 9; motor[driveL]= motor[driveL2] = motor[driveR] = motor[driveR2] = speed; } } while(stage == 2){ //Right //stage_done = 1; HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5); int speed = 20-acS3; if(speed < 9) speed = 9; motor[DFR] = -speed; motor[DFL]= speed; motor[DBR] = -speed; motor[DBL]= speed; if(acS3>last) last = acS3; if(last-acS3 > 1&&last!=0){ motor[DFR] = 0; motor[DFL]= 0; motor[DBR] = 0; motor[DBL]= 0; stage = 3; wait1Msec(1000); } // nxtDisplayTextLine(0, "Last: "+last); // nxtDisplayTextLine(1, "acS3: "+dcS3); wait1Msec(500); } } } }
//=================================================== // task to read in all sensors to workspace variables //=================================================== task sensors() { float currDir = 0.0; //prevDir = 0.0, long currtime,prevtime; LSsetActive(LEGOLS); // set the LEGO light sensor to active mode //------------------------- // gyro //------------------------- ir_mux_status=HTSMUXreadPowerStatus(IR_MUX); // read the sensor multiplexor status gyro_mux_status=HTSMUXreadPowerStatus(GYRO_MUX); // read the sensor multiplexor status while (ir_mux_status || gyro_mux_status) // check good battery power on both muxes { PlayTone(750,25); // if not we beep indefinitely wait1Msec(500); } //SMUX_good = true; while(calibrate != 1){}; // wait for a request to start calibrating the gyro wait1Msec(300); // short delay to ensure that user has released the button HTGYROstartCal(HTGYRO); // initiate the GYRO calibration drift = MyHTCal(gyroCalTime*1000); Driver_Cal = HTGYROreadCal(HTGYRO); // read the calculated calibration value for saving to file //--------------------------------------- // write the GYRO calibration data to file for Tele-Op //--------------------------------------- Delete(sFileName, nIoResult); // delete any pre-existing file nFileSize = 100; // new file size will be 100 bytes OpenWrite( hFileHandle, nIoResult, sFileName, nFileSize); // create and open the new file WriteFloat( hFileHandle, nIoResult, drift); // write the current drift value to the file WriteFloat( hFileHandle, nIoResult, Driver_Cal); // write the driver calibration to the file Close(hFileHandle, nIoResult); // close the file //--------------------------------------- for (int i=0;i<5;i++) // check if there is too much spread in the data { if (gyro_noise>10) // if there is too much spread we beep 5 times to alert the drive team { gyroTrue = true; PlayTone (250,25); wait1Msec(500); } } calibrate = 2; // this signifies to the main program that calibration has been completed prevtime = nPgmTime; while(true) { currtime=nPgmTime; rawgyro = HTGYROreadRot(HTGYRO); constHeading += (rawgyro - drift) * (float)(currtime-prevtime)/1000; relHeading += (rawgyro - drift) * (float)(currtime-prevtime)/1000; prevtime = currtime; //wait1Msec(1); //--------------------------------------------------------------------- // Read both sonar sensors and filter out non-valid echo readings (255) // If there is no echo the filter just retains the last good reading //--------------------------------------------------------------------- sonarRaw = USreadDist(LEGOUS); // read the rear mounted sensor if (sonarRaw!=255) sonarLive = sonarRaw; // and copy valid results to workspace sonarRaw2 = USreadDist(LEGOUS2); // read the side mounted sensor if (sonarRaw2!=255) sonarLive2 = sonarRaw2; // and copy valid results to workspace //------------------------- // LEGO light sensor //------------------------- light_normalised = LSvalNorm(LEGOLS); // read the LEGO light sensor //------------------------- // HiTechnic IR Sensor //------------------------- bearingAC = HTIRS2readACDir(HTIRS2); // Read the IR bearing from the sensor bearingAC2 = HTIRS2readACDir(HTIRS2_2);//here 12334 currDir = (float) bearingAC; // copy into workspace - /*if (bearingAC == 0) // No IR signal is being detected { currDir = prevDir; // so retain the previous reading } else // otherwise read all the IR segments { { bearingAC = (bearingAC - 1)/2; if ((bearingAC < 4) && (acS[bearingAC] != 0) && (acS[bearingAC + 1] != 0)) { currDir += (float)(acS[bearingAC + 1] - acS[bearingAC])/ max(acS[bearingAC], acS[bearingAC + 1]); } } } prevDir = currDir; IR_Bearing=currDir-5; // and setup the main variable for others to use */ HTIRS2readAllACStrength(HTIRS2, acS[0], acS[1], acS[2], acS[3], acS[4]); HTIRS2readAllACStrength(HTIRS2_2, acS2[0], acS2[1], acS2[2], acS2[3], acS2[4]); //----------------------------------- // code for the peaks of IR sensor 1 //----------------------------------- if (bearingAC!=0) // we have a valid IR signal { int maximum = -1; int peak = 0, offset=0; for (int i=0;i<5;i++) // scan array to find the peak entry { if (acS[i]>maximum) {peak = i; maximum = acS[i]; } } offset=0; if ((peak < 4) && (peak>0) && (acS[peak] != 0)) // we are not working with extreme value { if (acS[peak-1]!=acS[peak+1]) // if the values either side of the peak are identical then peak is peak { if (acS[peak-1]>acS[peak+1]) // otherwise decide which side has higher signal { offset = -25*(1-(float)(acS[peak]-acS[peak-1])/ // calculate the bias away from the peak max(acS[peak], acS[peak-1])); } else { offset = 25*(1-(float)(acS[peak]-acS[peak+1])/ max(acS[peak], acS[peak+1])); } } } IR_Bearing = (float)((peak-2)*50) + offset; // direction is the total of the peak bias plus the adjacent bias // range is -100 to +100, zero is straight ahead } //----------------------------------- // code for the peaks of IR sensor 2 //----------------------------------- if (bearingAC2!=0) // we have a valid IR signal { int maximum = -1; int peak = 0, offset=0; for (int i=0;i<5;i++) // scan array to find the peak entry { if (acS2[i]>maximum) {peak = i; maximum = acS2[i]; } } offset=0; if ((peak < 4) && (peak>0) && (acS2[peak] != 0)) // we are not working with extreme value { if (acS2[peak-1]!=acS2[peak+1]) // if the values either side of the peak are identical then peak is peak { if (acS2[peak-1]>acS2[peak+1]) // otherwise decide which side has higher signal { offset = -25*(1-(float)(acS2[peak]-acS2[peak-1])/ // calculate the bias away from the peak max(acS2[peak], acS2[peak-1])); } else { offset = 25*(1-(float)(acS2[peak]-acS2[peak+1])/ max(acS2[peak], acS2[peak+1])); } } } IR_Bearing2 = (float)((peak-2)*50) + offset; // direction is the total of the peak bias plus the adjacent bias // range is -100 to +100, zero is straight ahead } } }
task sensors() { //------------------------- // gyro //------------------------- long currtime,prevtime; int acS[5]; while (HTSMUXreadPowerStatus(S3)) // check battery power is on { PlayTone(750,25); wait1Msec(500); } SMUX_good = true; while(calibrate != 1){}; wait1Msec(300); HTGYROstartCal(HTGYRO); float drift = MyHTCal(gyroCalTime*1000); for (int i=0;i<5;i++) // check if there is too much spread in the data { if (abs(highest-lowest)>10) { PlayTone (250,25); wait1Msec(500); } } calibrate = 2; prevtime = nPgmTime; while(true) { currtime=nPgmTime; newgyro = (float)HTGYROreadRot(HTGYRO); constHeading += (newgyro - drift) * (float)(currtime-prevtime)/1000; relHeading += (newgyro - drift) * (float)(currtime-prevtime)/1000; prevtime = currtime; wait1Msec(1); //------------------------- // IR //------------------------- bearingAC = HTIRS2readACDir(HTIRS2); #define max(a, b) (((a) > (b))? (a): (b)) #define min(a, b) (((a) < (b))? (a): (b)) currDir = (float) bearingAC; if (bearingAC == 0) { currDir = prevDir; } else { if (HTIRS2readAllACStrength(HTIRS2, acS[0], acS[1], acS[2], acS[3], acS[4])) { bearingAC = (bearingAC - 1)/2; if ((bearingAC < 4) && (acS[bearingAC] != 0) && (acS[bearingAC + 1] != 0)) { currDir += (float)(acS[bearingAC + 1] - acS[bearingAC])/ max(acS[bearingAC], acS[bearingAC + 1]); } } } prevDir = currDir; ////------------------------- //// Sonar ////------------------------- //num = USreadDist(LEGOUS); //num2 = USreadDist(LEGOUS2); //if(num != 255) sonarLive = num; //if(num2 != 255) sonarLive2 = num2; //------------------------- // light //------------------------- LSsetActive(LEGOLS); nrm = LSvalNorm(LEGOLS); } }
// main task task main () { int _dirAC1 = 0; int _dirAC2 = 0; int acS1, acS2, acS3, acS4, acS5 = 0; int acS12, acS22, acS32, acS42, acS52 = 0; int maxSig = 0; // the max signal strength from the seeker. int maxSig2 = 0; // we are going to set DSP mode to 1200 Hz. tHTIRS2DSPMode _mode = DSP_1200; // attempt to set to DSP mode. if (HTIRS2setDSPMode(HTIRS2, _mode) == 0 || HTIRS2setDSPMode(ir2, _mode) == 0) { // unsuccessful at setting the mode. // display error message. eraseDisplay(); nxtDisplayCenteredTextLine(0, "ERROR!"); nxtDisplayCenteredTextLine(2, "Init failed!"); nxtDisplayCenteredTextLine(3, "Connect sensor"); nxtDisplayCenteredTextLine(4, "to Ports 1 and 4."); // make a noise to get their attention. PlaySound(soundBeepBeep); // wait so user can read message, then leave main task. wait10Msec(300); return; } eraseDisplay(); // loop continuously and read from the sensor. while(true) { // read the current modulated signal direction _dirAC1 = HTIRS2readACDir(HTIRS2); _dirAC2 = HTIRS2readACDir(ir2); if (_dirAC1 < 0) { // error! - write to debug stream and then break. writeDebugStreamLine("Read dir ERROR!"); break; } if (_dirAC2 < 0) { // error! - write to debug stream and then break. writeDebugStreamLine("Read dir ERROR!"); break; } // Get the AC signal strength values. if (!HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5 )) { // error! - write to debug stream and then break. writeDebugStreamLine("Read sig ERROR!"); break; } else { // find the max signal strength of all detectors. maxSig = (acS1 > acS2) ? acS1 : acS2; maxSig = (maxSig > acS3) ? maxSig : acS3; maxSig = (maxSig > acS4) ? maxSig : acS4; maxSig = (maxSig > acS5) ? maxSig : acS5; } if (!HTIRS2readAllACStrength(ir2, acS12, acS22, acS32, acS42, acS52 )) { // error! - write to debug stream and then break. writeDebugStreamLine("Read sig ERROR!"); break; } else { // find the max signal strength of all detectors. maxSig2 = (acS12 > acS22) ? acS12 : acS22; maxSig2 = (maxSig2 > acS32) ? maxSig2 : acS32; maxSig2 = (maxSig2 > acS42) ? maxSig2 : acS42; maxSig2 = (maxSig2 > acS52) ? maxSig2 : acS52; } // display info nxtDisplayCenteredTextLine(1, "DirL = %d", _dirAC1); nxtDisplayCenteredTextLine(4, "DirR = %d", _dirAC2); nxtDisplayCenteredTextLine(2, "SigL = %d", maxSig); nxtDisplayCenteredTextLine(5, "SigR = %d", maxSig2); // figure out which direction to go... // a value of zero means the signal is not found. // 1 corresponds to the far left (approx. 8 o'clock position). // 5 corresponds to straight ahead. // 9 corresponds to far right. // first translate directional index so 0 is straight ahead. // wait a little before resuming. wait10Msec(2); } }
// main task task main () { int _dirDC = 0; int _dirAC = 0; int dcS1, dcS2, dcS3, dcS4, dcS5 = 0; int acS1, acS2, acS3, acS4, acS5 = 0; // the default DSP mode is 1200 Hz. tHTIRS2DSPMode _mode = DSP_1200; // show the user what to do displayInstructions(); while(true) { // You can switch between the two different DSP modes by pressing the // orange enter button PlaySound(soundBeepBeep); while(bSoundActive) {} eraseDisplay(); nNumbCyles = 0; ++nInits; while (true) { if ((nNumbCyles & 0x04) == 0) nxtDisplayTextLine(0, "Initializing..."); else nxtDisplayTextLine(0, ""); nxtDisplayCenteredBigTextLine(1, "IR Seekr"); // set the DSP to the new mode if (HTIRS2setDSPMode(HTIRS2, _mode)) break; // Sensor initialized ++nNumbCyles; PlaySound(soundShortBlip); nxtDisplayTextLine(4, "Inits: %d / %d", nInits, nNumbCyles); nxtDisplayCenteredTextLine(6, "Connect Sensor"); nxtDisplayCenteredTextLine(7, "to Port S1"); wait1Msec(100); } eraseDisplay(); for (int i = 0; i < 8; ++i) sTextLines[i] = ""; // display the current DSP mode if (_mode == DSP_1200) nxtDisplayTextLine(0, " DC 1200"); else nxtDisplayTextLine(0, " DC 600"); while (true) { ++nNumbCyles; if (nNxtButtonPressed == kEnterButton) { // "Enter" button has been pressed. Need to switch mode _mode = (_mode == DSP_1200) ? DSP_600 : DSP_1200; while(nNxtButtonPressed == kEnterButton) { // Wait for "Enter" button release } break; } // Read the current non modulated signal direction _dirDC = HTIRS2readDCDir(HTIRS2); if (_dirDC < 0) break; // I2C read error occurred // read the current modulated signal direction _dirAC = HTIRS2readACDir(HTIRS2); if (_dirAC < 0) break; // I2C read error occurred // Read the individual signal strengths of the internal sensors // Do this for both unmodulated (DC) and modulated signals (AC) if (!HTIRS2readAllDCStrength(HTIRS2, dcS1, dcS2, dcS3, dcS4, dcS5)) break; // I2C read error occurred if (!HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5 )) break; // I2C read error occurred displayText(1, "D", _dirDC, _dirAC); displayText(2, "0", dcS1, acS1); displayText(3, "1", dcS2, acS2); displayText(4, "2", dcS3, acS3); displayText(5, "3", dcS4, acS4); displayText(6, "4", dcS5, acS5); nxtDisplayTextLine(7, "Enter to switch"); } } }
// main task task main () { int _dirDC = 0; int _dirAC = 0; int dcS1, dcS2, dcS3, dcS4, dcS5 = 0; int acS1, acS2, acS3, acS4, acS5 = 0; // show the user what to do displayInstructions(); while(true) { PlaySound(soundBeepBeep); while(bSoundActive) {} eraseDisplay(); nNumbCyles = 0; ++nInits; while (true) { if ((nNumbCyles & 0x04) == 0) nxtDisplayTextLine(0, "Initializing..."); else nxtDisplayTextLine(0, ""); nxtDisplayCenteredBigTextLine(1, "SMUX"); // Before using the SMUX, you need to initialise the driver HTSMUXinit(); // Tell the SMUX to scan its ports for connected sensors if (HTSMUXscanPorts(HTSMUX)) break; ++nNumbCyles; PlaySound(soundShortBlip); nxtDisplayTextLine(4, "Inits: %d / %d", nInits, nNumbCyles); nxtDisplayCenteredTextLine(6, "Connect SMUX"); nxtDisplayCenteredTextLine(7, "to Port S1"); wait1Msec(100); } eraseDisplay(); for (int i = 0; i < 8; ++i) sTextLines[i] = ""; // display the current DSP mode // When connected to a SMUX, the IR Seeker V2 can only be // used in 1200Hz mode. nxtDisplayTextLine(0, " DC 1200"); // The sensor is connected to the first port // of the SMUX which is connected to the NXT port S1. // To access that sensor, we must use msensor_S1_1. If the sensor // were connected to 3rd port of the SMUX connected to the NXT port S4, // we would use msensor_S4_3 while (true) { // Read the current non modulated signal direction _dirDC = HTIRS2readDCDir(msensor_S1_1); if (_dirDC < 0) break; // I2C read error occurred // read the current modulated signal direction _dirAC = HTIRS2readACDir(msensor_S1_1); if (_dirAC < 0) break; // I2C read error occurred // Read the individual signal strengths of the internal sensors // Do this for both unmodulated (DC) and modulated signals (AC) if (!HTIRS2readAllDCStrength(msensor_S1_1, dcS1, dcS2, dcS3, dcS4, dcS5)) break; // I2C read error occurred if (!HTIRS2readAllACStrength(msensor_S1_1, acS1, acS2, acS3, acS4, acS5 )) break; // I2C read error occurred displayText(1, "D", _dirDC, _dirAC); displayText(2, "0", dcS1, acS1); displayText(3, "1", dcS2, acS2); displayText(4, "2", dcS3, acS3); displayText(5, "3", dcS4, acS4); displayText(6, "4", dcS5, acS5); if (HTSMUXreadPowerStatus(HTSMUX)) nxtDisplayTextLine(7, "Batt: bad"); else nxtDisplayTextLine(7, "Batt: good"); } } }
task main() { initializeRobot(); int _dirDC_left = 0; int _dirAC_left = 0; int _dirDC_right = 0; int _dirAC_right = 0; int dcS1_left, dcS2_left, dcS3_left, dcS4_left, dcS5_left = 0; int acS1_left, acS2_left, acS3_left, acS4_left, acS5_left = 0; int dcS1_right, dcS2_right, dcS3_right, dcS4_right, dcS5_right = 0; int acS1_right, acS2_right, acS3_right, acS4_right, acS5_right = 0; eraseDisplay(); for (int i = 0; i < 8; ++i) sTextLines[i] = ""; waitForStart(); tHTIRS2DSPMode _mode = DSP_1200; while(true) { PlaySound(soundShortBlip); if(HTIRS2setDSPMode(irL, _mode)) { break; } } while(true) { PlaySound(soundShortBlip); if(HTIRS2setDSPMode(irR, _mode)) { break; } } while (true) { nxtDisplayTextLine(1, " L R"); _dirDC_left = HTIRS2readDCDir(irL); if (_dirDC_left < 0) break; // I2C read error occurred // read the current irL); _dirAC_left = HTIRS2readACDir(irR); if (_dirAC_left < 0) break; // I2C read error occurred _dirDC_right = HTIRS2readDCDir(irR); if (_dirDC_right < 0) break; // I2C read error occurred // read the current modulated signal direction _dirAC_right = HTIRS2readACDir(irR); if (_dirAC_right < 0) break; // I2C read error occurred if (!HTIRS2readAllDCStrength(irL, dcS1_left, dcS2_left, dcS3_left, dcS4_left, dcS5_left)) break; // I2C read error occurred if (!HTIRS2readAllACStrength(irL, acS1_left, acS2_left, acS3_left, acS4_left, acS5_left)) break; // I2C read error occurred if (!HTIRS2readAllDCStrength(irR, dcS1_right, dcS2_right, dcS3_right, dcS4_right, dcS5_right)) break; // I2C read error occurred if (!HTIRS2readAllACStrength(irR, acS1_right, acS2_right, acS3_right, acS4_right, acS5_right)) break; // I2C read error occurred displayText(1, "D", _dirAC_left, _dirAC_right); displayText(2, "0", acS1_left, acS1_right); displayText(3, "1", acS2_left, acS2_right); displayText(4, "2", acS3_left, acS3_right); displayText(5, "3", acS4_left, acS4_right); displayText(6, "4", acS5_left, acS5_right); } }