Программируем РобоФутболиста на 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);
 }
}