Roomba Program Example

/*
  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); 
}

Comments