/* Drive-Left Maze program         */
/* in "MicroMouse" style           */
/* Thomas Braunl, UWA, 1998, 2019  */
#include "eyebot.h"
#define THRES 400

int main ()
{ int Ffree,Lfree,Rfree;
  LCDPrintf("MAZE Left\n");
  LCDMenu("","","","END");

  do
  { // 1. Check walls
    Ffree = PSDGet(PSD_FRONT) > THRES;
    Lfree = PSDGet(PSD_LEFT)  > THRES; 
    Rfree = PSDGet(PSD_RIGHT) > THRES;

    // 2. Rotations
    if (Lfree)         { VWTurn(+90, 45); VWWait(); } // Turn left
     else if (Ffree)   {  }                           // NO turn
       else if (Rfree) { VWTurn(-90, 45); VWWait(); } // Turn right
         else          { VWTurn(180, 45); VWWait(); } // Dead end
         
    // 3. Driving straight 1 square
    VWStraight(360, 180); VWWait();
  } while (KEYRead() != KEY4);
}
