Mindboards

Code: Select all

int dir,s1,s3,s5,s7,s9;
task main()
{
  SetSensorLowspeed(S1);
  while (true)
  {
    ReadSensorHTIRSeeker(S1,dir,s1,s3,s5,s7,s9);

    ClearScreen();
    NumOut(0,LCD_LINE1,dir);
    NumOut(0,LCD_LINE3,s1);
    NumOut(0,LCD_LINE4,s3);
    NumOut(0,LCD_LINE5,s5);
    NumOut(0,LCD_LINE6,s7);
    NumOut(0,LCD_LINE7,s9);
    
    Wait (50);
  }
}

Code: Select all

//
// 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;

  SetSensorLowspeed(S1);

  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);

    Wait(50);
    
  }
}

lo que quiero es poner un una función  de este tipo

void rodar()
{

 while(dir != 5){

ReadSensorHTIRSeever2AC(IN_1, dir, s1, s3, s5, s7, s9)

OnFwdSinc(OUT_AB, 75, 100)
Wait(50);
}
}