Просмотр исходного текста страницы Программируем РобоФутболиста на NXC

Датчики 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);
  }
 }