Warner K.
ROBOWARNER.COM
April 19, 2016

Websocket Roomba Control
Arduino Code

#include <Servo.h>
#include <SoftwareSerial.h>

Servo panCamera;  // create servo object to control a servo
Servo tiltCamera;

float panPos = 104;    // variable to store the servo position (for pan and tilt on head)
float tiltPos = 90;
float counter = 0;

int DD = 12;    //Device Detect for roomba awakening
int TX = 11;    //Serial Transmit to Roomba

int velocity = 300;
int radius;
float changePan = 0;
float changeTilt = 0;
bool forwardIntent = false;


SoftwareSerial mySerial(8, TX); // RX, TX

void setup() {
pinMode(TX, OUTPUT);
 pinMode(DD, OUTPUT);
 digitalWrite(DD, LOW);
delay(1000);
 digitalWrite(DD,HIGH);
 delay(100);
 digitalWrite(DD, LOW);
delay(100);
 digitalWrite(DD,HIGH);
 delay(100);
  digitalWrite(DD, LOW);
delay(100);
 digitalWrite(DD,HIGH);
 delay(100);
  digitalWrite(DD, LOW);
delay(100);
 digitalWrite(DD,HIGH);
 Serial.begin(9600);
 // while (!Serial) {
   // ;
 // }


  mySerial.begin(19200);
  // put your setup code here, to run once:
panCamera.attach(7);  // attaches the servo on pin 9 to the servo object
tiltCamera.attach(6);  // attaches the servo on pin 9 to the servo object

panCamera.write(panPos);              // tell servo to go to position in variable 'pos'
tiltCamera.write(tiltPos);              // tell servo to go to position in variable 'pos'
delay(500);

 mySerial.write((byte)128);
 delay(1000);
 mySerial.write((byte)129);
 mySerial.write((byte)7);
  delay(500);
  mySerial.write((byte)130);
  delay(500);


 // mySerial.write((byte)133);
//int motor[] = {138,2};
 
    //mySerial.write(motor[2]);

    mySerial.write((byte)132);

    delay(1000);

int song0[11]={5,84,16,84,16,85,16,87,16,93,32};
int song1[11]={5,87,16,94,16,85,8,79,16,85,16};
int song2[11]={5,84,8,86,16,88,8,90,8,92,16};
int song3[11]={5,84,8,84,10,85,16,82,12,70,8};

  initSongs(0, song0);
   initSongs(1, song1);
    initSongs(2, song2);
     initSongs(3, song3);
 
}




void loop() {

 if (mySerial.available()) {
    Serial.print(mySerial.read());
  }
  
if (Serial.available() > 0) {
    int thisChar = Serial.read();
 counter = 0;
    if (thisChar == 'a')
    {
panPos = 60;//changePan = -.1;   
    }
     if (thisChar == 'd')
    {
panPos = 140; //changePan = .1;
    }
      if (thisChar == 'Z')
    {
panPos = 104;//changePan = 0;     
    }
    if (thisChar == 's')
    {
changeTilt = -.15;   
    }
     if (thisChar == 'w')
    {
changeTilt = .15;
      
    }
      if (thisChar == 'X')
    {
changeTilt = 0;     
    }

    
    if (thisChar == 'F')
    {
      velocity = 500;
      radius = 32768;
      forwardIntent = true;
      drive();    
    }
    else if (thisChar == 'B')
    {
      velocity = -300;
      radius = 32768;  
      drive();     
    }
     else if (thisChar == 'L' )
    {
      if (forwardIntent == true)
      {
        velocity = 500;
        radius = 200;
      }
      else
      {
        velocity = 300;
        radius = 20;  
      }
      drive();     
    }
    else if (thisChar == 'R'){
      if (forwardIntent == true)
      {
        velocity = 500;
        radius = -200;
      }
      else
      {
        velocity = 300;
        radius = -20;  
      }  
      drive();   
    }
    else if (thisChar == 'S')
    {
      forwardIntent = false;
      velocity = 0;
      radius = 32768; 
      drive();
    }
     else if (thisChar == '0')
    {
     mySerial.write((byte)141);
     mySerial.write((byte)0);
    }
       else if (thisChar == '1')
    {
     mySerial.write((byte)141);
     mySerial.write((byte)1);
    }
       else if (thisChar == '2')
    {
     mySerial.write((byte)141);
     mySerial.write((byte)2);
    }
       else if (thisChar == '3')
    {
     mySerial.write((byte)141);
     mySerial.write((byte)3);
    }





//delay(50);
}
else
{
delay(5);
counter ++;
/*
if ((panPos < 160)&& (changePan > 0))
{
  panPos = panPos + changePan;
}
if ((panPos > 20) && (changePan < 0))
{
  panPos = panPos + changePan;
}*/

if ((tiltPos < 160)&& (changeTilt > 0))
{
  tiltPos = tiltPos + changeTilt;
}
if ((tiltPos > 70) && (changeTilt < 0))
{
  tiltPos = tiltPos + changeTilt;
}

panCamera.write(panPos);              // tell servo to go to position in variable 'pos'
tiltCamera.write(tiltPos);              // tell servo to go to position in variable 'pos'

if (counter > 2000)     //Counts length of time since last serial input. Used to stop the roomba if the signal is lost.
{
velocity = 0;
radius = 32768;  
drive();
      

counter = 0;
}
}
}


void drive(){                   //Send driving commands to roomba
   mySerial.write((byte)137);
   mySerial.write(highByte(velocity));
   mySerial.write(lowByte(velocity));
   mySerial.write(highByte(radius));
   mySerial.write(lowByte(radius));
}

void initSongs(int songNum, int song[]){      //Write songs to Roomba memory
mySerial.write((byte)140);
mySerial.write((byte)songNum);
mySerial.write((byte)song[0]);
for (int i = 1; i <= (song[0]*2);)
{
  mySerial.write((byte)song[i++]);
  mySerial.write((byte)song[i++]);
}
}