Mindboards

Code: Select all

#define LET_MY_ROBOT_MOVE_A_BIT_BEFORE_CHANGING_ITS_MIND_AGAIN MS_50

int powerDiff;
int powerLeft;
int powerRight;
int dir;
int se1, se2, se3, se4;
int se5, se6, se7, se8, se9;

task main()
{
  SetSensorLowspeed(S2);
  while (true)
  {
    if (ReadSensorHTIRSeeker2AC(S2, dir, se1, se3, se5, se7, se9))
    {
      NumOut( 0, LCD_LINE1, dir);
      NumOut(30, LCD_LINE1, se1);
      NumOut(60, LCD_LINE1, se3);
      NumOut( 0, LCD_LINE2, se5);
      NumOut(30, LCD_LINE2, se7);
      NumOut(60, LCD_LINE2, se9);
      powerDiff = 10 * (dir - 5);
      powerRight = 50 - powerDiff;
      powerLeft = 50 + powerDiff;
      if (dir == 5) {
        OnFwd(OUT_AC, 100);
      }
      else if ((dir > 5) && (dir <= 9)) {
        OnFwd(OUT_C, powerLeft);
        OnFwd(OUT_A, powerRight);
      }
      else if ((dir >= 1) && (dir <= 5)) {
        OnFwd(OUT_A, powerRight);
        OnFwd(OUT_C, powerLeft);
      }
      else if (dir == 0) {
        OnFwd(OUT_AC, 25);
      }
    }
    else
      Off(OUT_AC); // don't move if we can't read data
    Wait(LET_MY_ROBOT_MOVE_A_BIT_BEFORE_CHANGING_ITS_MIND_AGAIN);
  }
}