/*

  Oct 9,2013 Jim Isaak 

  v2 -- Do a Figure 8 -- Just one time and thats all (restart to restart)

Roomba 2 works with 19200 Baud (need to hold down power key on power on for a bit to force it to this mode) Oct. 7, 2013

  The software serial circuit: 

 * RX is digital pin 10 (connect to TX of other device)

 * TX is digital pin 11 (connect to RX of other device)

 

  */

#include <SoftwareSerial.h>;

//Drive Commands

byte Rstop[6]={137,0,0,0,0};

byte Rright[6]={137,0,100,0,1};

byte Rleft[6]={137,0,100,255,255};

byte Rfwd[6]={137,0,100,0x80,0}  ;  //radius hex 8000 is straight

byte Rback[6]= {137,255,-100,0x80,0}; //But back is not as "reliable" as forward

byte RCircleR[6]={137,0,100,0,128}; //Circle Right

byte RCircleL[6]= {137,0,100,-1,-128}; //Circle Left

byte Song0[17]={140,0,7,31,60,43,55,55,50,67,45,79,40,91,35,103,30};

byte Song1[35]={140,1,16, 36,20, 37,20, 0,40,  36,20, 37,20, 0,30,  36,20, 37,20, 0,20,  36,20, 37,20, 0,10,  36,20, 37,20, 0,5,  38,64}; //Jaws

byte Song9[33]={140,9,15, 36,20, 37,20, 0,65,  36,20, 37,20, 0,60,  36,20, 37,20, 0,55,  36,20, 37,20, 0,50,  36,20, 37,20, 0,45};

byte Song2[13]={140,2,5,79,48,81,48,77,48,65,50,72,64}; //Strange Encounters 1

byte Song3[13]={140,3,5,69,48,71,48,67,48,55,48,62,64}; //Strange Encounters

byte Song15[11]={140,15,4,33,16,45,20,43,12,31,40};

byte RoombaPlay[2] = {141,15};  //Play song 0 to 15

byte RoombaMotorsOff[2] = {138,0};  //motors off code (brushes, vac)

byte RoombaOff = 133;

byte zero = 0;

int ptr, cnt, cmd;

char inbuf[128], cin;

unsigned long t1,t2;

boolean play = false;

SoftwareSerial mySerial(10, 11); // RX, TX (instance of serial object)

void setup()  

{

  // Open serial communications and wait for port to open:

  Serial.begin(19200);

  Serial.println("OneShot Figure 8");  //tell monitor we are alive

   // set the data rate for the SoftwareSerial port

  mySerial.begin(19200); //Softserial works at 19200!!

  delay(10);

 cnt=1; // ----------------------- Number of figure 8s to do before stoping 

//------------ Initialize

    mySerial.write(128);  //start (goes to Passive)

      delay(20);  //delay 20 milliseconds after state change

      mySerial.write(130);  //Command mode - goes to safe

      delay(20);  //delay 20 milliseconds after state change

      Serial.println("Roomba Initialized (start, command)"); 

//------------ Set Songs

      mySerial.write(Song0,17);

      mySerial.write(Song1,35);

      mySerial.write(Song9,33);

      mySerial.write(Song2,13);

      mySerial.write(Song3,13);

      mySerial.write(Song15,11);

      Serial.println("Songs Set"); 

      

      

}

void loop() // run over and over

{

  if (mySerial.available())   //First order of business: listen to Roomba

    Serial.write(mySerial.read());   //writes to USB input from soft serial if connected to laptop serial monitor

  

  if (cnt>0)

  {

   Fig8();

   cnt=cnt-1;

  }

  {

    StopIt();

    mySerial.write(133);  //power off mode

  }

  ;

  

}

void TurnRight()

{

       mySerial.write(Rright,6);

       Serial.println("turning right");

       delay(2000);

       mySerial.write(Rstop,6);

}

void TurnLeft()

{

       mySerial.write(Rleft,6);

       Serial.println("turning left");

       delay(2000);

       mySerial.write(Rstop,6);

}

void Go(int seconds)

{

       //PlayIt(1);

       mySerial.write(Rfwd,6);

       Serial.println("going forward");

       delay(seconds*1000);

       mySerial.write(Rstop,6);

}

void Back(int seconds)

{

       mySerial.write(Rback,6);

       Serial.println("Backing up");

       delay(seconds*1000);

       mySerial.write(Rstop,6);

}

void Fig8()

{

 int t8 = 4500;

 TurnLeft();

 PlayIt(2);

 CircleRight();

 PlayIt(2);

 delay(t8);

 CircleRight();

 PlayIt(3);

 delay(t8);

 Go(1);   //=====================

 TurnRight();

 PlayIt(9);

 CircleLeft();

 delay(t8);

 PlayIt(1);

 CircleLeft();

 delay(t8);

 Go(1); 

 TurnLeft();

 Go(2);

 TurnRight();

 StopIt();

}

void CircleRight()

{

  mySerial.write(RCircleR,6);

}

void CircleLeft()

{

  mySerial.write(RCircleL,6);

}

void StopIt()

{

      mySerial.write(Rstop,6);

      mySerial.write(RoombaMotorsOff,2);

      Serial.println("All Stopped");

}

void PlayIt(byte s)

{

 mySerial.write(141);

 mySerial.write(s); 

}