#include <ArduinoRobotMotorBoard.h>
#include <EasyTransfer2.h>
#include <LineFollow.h>
#include <Multiplexer.h>

/* 
Manual demonstration by Michael Mirmak
Arduino Mega code developed August 9, 2013
Updated Aug. 24 with results of pool test Aug. 23 for UNO ONLY

Except for the Adafruit Motor Shield code noted below, code is
Copyright (c) 2013 Michael Mirmak
Code may be freely redistributed, used and modified so long as modifications
are noted and this text is not removed.  

################################################################################
Code is for the v1.0 by Adafruit Motor/Stepper/Servo Shield for Arduino 
http://www.adafruit.com/products/81

Libraries at: https://github.com/adafruit/Adafruit-Motor-Shield-library

Tutorials at: http://learn.adafruit.com/adafruit-motor-shield

This is using the DC motor capabilities only.

MOTOR L = Motor 1
MOTOR R = Motor 2
L MOTOR UP/DOWN = Motor 3
R MOTOR UP/DOWN = Motor 4

Digital pins 3,4,5,6,7,8,11,12 now in-use

################################################################################
Pseudo-code of operation:

   start 120 second countdown
   when 90 seconds has expired, turn on the built-in LED
   when 120 seconds has expired
   {
      we assume Poolbot is at the surface
      motor L+R for 7 seconds, three-quarters power
      motor L for 5 seconds, three-quarters power
      motor R for 5 seconds, three-quarters power
      motor L+R+DOWN for 10 seconds, full power on DOWN; three-quarters power on L+R
      motor L+R for 7 seconds, three-quarters power
      motor L for 5 seconds, three-quarters power
      motor R for 5 seconds, three-quarters power
      motor L+R+UP for 5 seconds, full power on UP; three-quarters power on L+R
   }

*/

/*################################################################################*/
// Adafruit Motor shield library
// copyright Adafruit Industries LLC, 2009
// this code is public domain, enjoy!

#include <AFMotor.h>
#define AR_SIZE( a ) sizeof( a ) / sizeof( a[0] )

/*################################################################################*/

AF_DCMotor left_motor(1, MOTOR12_1KHZ);   // create motor #1, 64KHz pwm
AF_DCMotor right_motor(2, MOTOR12_1KHZ);  // create motor #2, 64KHz pwm
AF_DCMotor dive_motor_L(3, MOTOR12_1KHZ); // create motor #3, 64KHz pwm
AF_DCMotor dive_motor_R(4, MOTOR12_1KHZ); // create motor #4, 64KHz pwm

/*################################################################################*/

int loop_control = 0;
int ledpin = 13;

/*################################################################################*/

 void setup() 
 {
  // activate the built-in Uno LED, but keep it low until countdown reaches 30 seconds
  pinMode(ledpin, OUTPUT);
  digitalWrite(ledpin, LOW);
  
  delay(100);
  Serial.begin(9600);
  
  left_motor.setSpeed(192);       // set the speed to 3/4
  right_motor.setSpeed(192);      // set the speed to 3/4
  dive_motor_L.setSpeed(255);     // set the speed to 100%
  dive_motor_R.setSpeed(255);     // set the speed to 100%
}
/*################################################################################*/

void loop() 
{

  while (loop_control != 1) 
  {
    // surface
    delay(90000);                  // delay 1.5 minutes 

    // turn on the Uno LED when 30 seconds remain
    digitalWrite(ledpin,HIGH);

    delay(30000);                  // delay thirty seconds 

    left_motor.run(FORWARD);       // going forward
    right_motor.run(FORWARD);      // going forward
    delay(7000);
    all_stop();                    // separate function to release all motors

    left_motor.run(FORWARD);       // turn starboard!
    delay(5000);
    all_stop();

    right_motor.run(FORWARD);      // turn port!
    delay(5000);
    all_stop();

    // dive!
    dive_motor_L.run(FORWARD);     // FORWARD here is down (and the reverse of the propulsion motors; CHECK THE TERMINALS!)
    dive_motor_R.run(FORWARD);     // Robot must "drive to dive"
    left_motor.run(FORWARD);       // keep going forward
    right_motor.run(FORWARD);      // keep going forward

    delay(10000);
    all_stop();

    // sub-surface operations!    
    left_motor.run(FORWARD);       // Repeating earlier sequence, but now underwater
    right_motor.run(FORWARD);      // going forward
    delay(7000);
    all_stop();

    left_motor.run(FORWARD);       // turn starboard!
    delay(5000);
    all_stop();

    right_motor.run(FORWARD);      // turn port!
    delay(5000);
    all_stop();

    // drive to surface!    
    left_motor.run(FORWARD);        // going forward
    right_motor.run(FORWARD);       
    dive_motor_L.run(BACKWARD);     // reverse dive motors to ensure 
    dive_motor_R.run(BACKWARD);     // robot drives to surface
    delay(7000);

    all_stop();                     // shut down all motors
  
    loop_control = 1;               // run once, then quit
  }
}

void all_stop() 
{
  left_motor.run(RELEASE);         // stopped
  right_motor.run(RELEASE);        // stopped
  dive_motor_L.run(RELEASE);       // stopped
  dive_motor_R.run(RELEASE);       // stopped
}