Программируем РобоФутболиста на NXC
Материал из Wiki ТФТЛ
Версия от 22:10, 11 октября 2016; SergeyKosachenko (обсуждение | вклад) (Новая страница: «Датчики HTIRSeeker и HTCompass //===================================================================== // Sample Program to display the values from the…»)
Датчики HTIRSeeker и HTCompass
//===================================================================== // Sample Program to display the values from the // HiTechnic IRSeeker V2 // #define IRSEEKER IN_1 // Convenience macro that combines TextOut and NumOut #define TextNumOut(xPos,yPos,str,col,num) TextOut(xPos,yPos,str); \ NumOut(xPos+6*col,yPos,num) task main() { int dir, s1, s2, s3, s4, s5, result; SetSensorLowspeed(IRSEEKER); while(true) { ReadSensorHTIRSeeker2AC(IRSEEKER, dir, s1, s2, s3, s4, s5); TextNumOut(0, LCD_LINE3, "Dir: ",4,dir); TextNumOut(6, LCD_LINE4, "S1: ",3,s1); TextNumOut(6, LCD_LINE5, "S2: ",3,s2); TextNumOut(6, LCD_LINE6, "S3: ",3,s3); TextNumOut(6, LCD_LINE7, "S4: ",3,s4); TextNumOut(6, LCD_LINE8, "S5: ",3,s5); Wait(100); } }
// Enhanced IR SeekerV2 Function in NXC // // This function implements the same algorithm used in the // Enhanced IRSeekerV2 Block. // // This method of determining direction and signal strength combines // both the DC and AC data from the IRSeeker sensor for improved full // range performance. Using just AC mode works great for longer ranges // but can cause problems at short range. Using just DC mode works very // poorly at long range and is also very prone to IR interference. Using // both AC and DC modes gives the most reliable direction and signal strength // determination regardless of is the ball is near or far from the sensor. // you improved information on the direction to the IR Ball. // // dir A direction value similar to standard block from 0-9 // with 0 as no signal // 1-4 to the left of the sensor // 5 straight ahead // 5-9 to the right of the sensor // To get approximate direction in degrees use: // (dir - 5) * 30 // strength A single strength value based on the strength of the // IR signal. // // void HTEnhancedIRSeekerV2(const byte port, int &dir, int &strength) { int cResp; byte cmdBuf[] = {0x10, 0x43}; // Read DC signal strengths (skip the dir) byte respBuf[]; // Response Buffer bool fSuccess; int i, iMax; long dcSigSum, dcStr; dir = 0; strength = 0; // Read DC mode cResp=6; fSuccess = I2CBytes(port, cmdBuf, cResp, respBuf); if (fSuccess) { // Find the max DC sig strength iMax = 0; for (i=1; i<5; i++) if (respBuf[i] > respBuf[iMax]) iMax = i; // Calc base DC direction value dir = iMax*2+1; // Set base dcStrength based on max signal and average dcSigSum = respBuf[iMax] + respBuf[5]; // Check signal strength of neighboring sensor elements if ((iMax > 0) && (respBuf[iMax-1] > respBuf[iMax]/2)) { dir--; dcSigSum += respBuf[iMax-1]; } if ((iMax < 4) && (respBuf[iMax+1] > respBuf[iMax]/2)) { dir++; dcSigSum += respBuf[iMax+1]; } // Make DC strength compatible with AC strength. use: sqrt(dcSigSum*500) dcSigSum *= 500; dcStr = 1; repeat(10) dcStr = (dcSigSum/dcStr + dcStr) / 2; // sqrt approx strength = dcStr; // Decide if using DC strength or should read and use AC strength if (strength <= 200) { // Use AC Dir dir = 0; strength = 0; cmdBuf[1] = 0x49; // Recycle rest of cmdBuf from the DC read operation cResp=6; fSuccess = I2CBytes(port, cmdBuf, cResp, respBuf); if (fSuccess) { dir = respBuf[0]; // Sum the sensor elements to get strength if (dir > 0) for (i=1; i<=5; i++) strength += respBuf[i]; } } } } task main() { int dir; int strength; int kompass; SetSensorLowspeed(S1); SetSensorLowspeed(IN_2); TextOut(0, LCD_LINE1, "Demo EIRSeekerV2"); while(true) { HTEnhancedIRSeekerV2(S1, dir, strength); TextOut(0, LCD_LINE3, "Edir: "); NumOut(6*5, LCD_LINE3, dir); TextOut(6*7, LCD_LINE3, "Estr: "); NumOut(6*12, LCD_LINE3, strength); kompass = SensorHTCompass(IN_2); TextOut(0, LCD_LINE4, "Compass: "); NumOut(0, LCD_LINE5, kompass); Wait(50); } }
task main() { int heading1; //heading (0-359 value) SetSensorLowspeed(IN_1); //compass sensor in port 1 while(true){ heading1 = SensorHTCompass(IN_1); // get current heading TextOut(0, LCD_LINE3, "Compass:"); NumOut(0, LCD_LINE4, heading1); Wait(500); } }