#include "robot.h"
RobotDrive d;

bool stopped;

DIPSwitchRobotSensor on(1);
MicroSwitchRobotSensor  right_sensor(0);
MicroSwitchRobotSensor  left_sensor(2);

void setup()
{
    stopped = !on.On();
    if (!stopped) 
      d.Forwards();
}

void loop()
{
   if (!on.On()) {
       d.Stop();
       stopped = 1;
       return;
   }
   if (stopped) {
       stopped = 0;
       d.Forwards();
   }
   if (left_sensor.On()) {
     d.Stop();      // stop, wait a while
     delay(100);
     d.Backwards();             // back up a bit
     delay(500);
     d.Right();                // turn right
     delay(500);
     d.Stop();
     delay(100);
     d.Forwards();              // go forwards
   } else
     // otherwise if the right switch has hit a wall turn left and go 
   if (right_sensor.On()) {
     d.Stop();
     delay(100);
     d.Backwards();             // back up a bit
     delay(500);
     d.Left();
     delay(500);
     d.Stop();
     delay(100);
     d.Forwards();
     return;
  }
}