//
//  Wiring for motor control mappings to arduino pin numbers:
//
//  Motor 1:
//    EN1 - pin 4
//    I1  - pin 6
//    I2  - pin 7
//  Motor 2:
//    EN3 - pin 5
//    I3  - pin 9
//    I4  - pin 10
//
//  Front sensors 
//
//  left  - pin 14
//  right - pin 17
//
// The motor controls are wired to the L298 - refer to its data 
// sheet for the meanings o the pins
// 
// the front sensors here are used as digital inputs (they could
// be analog) and the built in AVR pullup resistors are enabled
// so they can be connected to simple microswitches - they will read
// as 0 when pressed and 1 when released
unsigned char running;
void
stop() {  // all motors stop
    digitalWrite(6, 0);
    digitalWrite(7, 0);
    digitalWrite(4, 0);
    digitalWrite(9, 0);
    digitalWrite(10, 0);
    digitalWrite(5, 0);
}
void
back() { // all motors reverse
    digitalWrite(6, 1);
    digitalWrite(7, 0);
    digitalWrite(4, 1);
    digitalWrite(9, 1);
    digitalWrite(10, 0);
    digitalWrite(5, 1);
}
void
forward() { // all motors forwards
    digitalWrite(6, 0);
    digitalWrite(7, 1);
    digitalWrite(4, 1);
    digitalWrite(9, 0);
    digitalWrite(10, 1);
    digitalWrite(5, 1);
}
void
left() {  // left motor back right motor forward
    digitalWrite(6, 0);
    digitalWrite(7, 1);
    digitalWrite(4, 1);
    digitalWrite(9, 1);
    digitalWrite(10, 0);
    digitalWrite(5, 1);
}
void
right() { // left motor forwards, right motor back
    digitalWrite(6, 1);
    digitalWrite(7, 0);
    digitalWrite(4, 1);
    digitalWrite(9, 0);
    digitalWrite(10, 1);
    digitalWrite(5, 1);
}

void
setup()
{
    // set up motor control as outputs
    pinMode(6, OUTPUT); // I1  PD6 
    pinMode(7, OUTPUT); // I2  PD7
    pinMode(4, OUTPUT); // EN1 PD3
    pinMode(9, OUTPUT); // I3  PB1
    pinMode(10, OUTPUT);// I4  PB2
    pinMode(5, OUTPUT);// EN2 PD4

    // stop the motors
    stop();
    
    pinMode(3, INPUT);
    digitalWrite(3, 1); 
    // set up the sensors ans inputs and enable the pullups
    pinMode(14, INPUT);     // PC0 - sensor 1 (microswitch)
    digitalWrite(14, 1); 
    pinMode(17, INPUT);     // PC1 - sensor 2 
    digitalWrite(17, 1); // enable pullups 
    
    delay(100);
    // go forwards
    if (digitalRead(3)) {
         running = 0;
    } else {
         running = 1;
         forward();
    }
}

void
loop()
{
     if (digitalRead(3)) {
         if (running) {
           running = 0;
           stop();
           return;
         }
     } else {
         if (!running) {
           running = 1;
           forward();
           return;
         }
     }
     // if the left switch has hit a wall ....    
     if (!digitalRead(14)) {
         stop();      // stop, wait a while
         delay(100);
         if (!digitalRead(17)) { // if both switches are pressed 
             back();             // back up a bit
             delay(500);
         }
         right();                // turn right
         delay(500);
         stop();
         delay(100);
         forward();              // go forwards
         return;
     }
     // otherwise if the right switch has hit a wall turn left and go 
     if (!digitalRead(17)) {
         stop();
         delay(100);
         left();
         delay(500);
         stop();
         delay(100);
         forward();
         return;
     }
}