/*

This sketch is used along with Android app 'Roomba_Bluetooth_V1.apk' to control 
Roomba vaccum cleaner. Feel free to copy and modify according to your needs.

Created by: Ajay Sonar
Last modified: Dec 24 2013

*/


#include <SoftwareSerial.h>

// Software serial for bluetooth communication
SoftwareSerial mySerial(3,4); // RX, TX

int rxPin = 0;
int txPin = 1;
int ddPin = 2;
int ledPin = 13;
char val, sensorbytes[10];

void setup()
{
  
  /******* Change Bluetooth-Arduino communication speed ***************/
  mySerial.begin(115200); // Default Bluetooth-Arduino communication speed
  delay(320);                     // IMPORTANT DELAY! (Minimum ~276ms)
  mySerial.print("$$$");         // Enter command mode
  delay(15);                      // IMPORTANT DELAY! (Minimum ~10ms)
  mySerial.println("U,9600,N");  // Temporarily Change the baudrate to 9600, no parity
  mySerial.begin(9600); // New Bluetooth-Arduino communication speed
  /************************************************************/
  
  
  Serial.begin(57600); // Roomba-Arduino communication speed
 
  pinMode(ddPin,  OUTPUT);   // sets the pins as output
  pinMode(ledPin, OUTPUT);   // sets the pins as output
  
  digitalWrite(ledPin, HIGH); // say we're alive
  
  // wake up the robot
  digitalWrite(ddPin, HIGH);
  delay(1000);
  digitalWrite(ddPin, LOW);
  digitalWrite(ledPin, LOW);
  delay(500);
  digitalWrite(ddPin, HIGH);
  digitalWrite(ledPin, HIGH);
  delay(1000);
  digitalWrite(ledPin, LOW);
  
  // set up ROI to receive commands  
  Serial.write(0X80);  // START:128
  delay(1000);
  
  Serial.write(0X82); // CONTROL:130
  delay(1000);
  
  Serial.write(0x84);  //FULL:132
  delay(1000);
     
}


void startCleaning()
{
  Serial.write(0X8A); // Cleaning Motors
  Serial.write(0X05); // Start Side brush
    
}


void stopCleaning()
{
  Serial.write(0X8A); // Cleaning Motors
  Serial.write(0X00); // Start Side brush
    
}


void forward()
{
  
  Serial.write(0X89); // Drive motors
  Serial.write(0X00); // Velocity high byte
  Serial.write(0X64); // Velocity low byte
  Serial.write(0X80); // Radius high byte
  Serial.write(0X00); // Radius low byte 
}


void backward()
{
  Serial.write(0X89); // Drive motors
  Serial.write(0XFF); // Velocity high byte
  Serial.write(0X9C); // Velocity low byte
  Serial.write(0X80); // Radius high byte
  Serial.write(0X00); // Radius low byte 
  
}


void leftTurn()
{
 
  Serial.write(0X89); // Drive motors
  Serial.write(0XFF); // Velocity high byte
  Serial.write(0X9C); // Velocity low byte
  Serial.write(0XFF); // Radius high byte
  Serial.write(0XFF); // Radius low byte 
  
}


void rightTurn()
{
   Serial.write(0X89); // Drive motors
  Serial.write(0XFF); // Velocity high byte
  Serial.write(0X9C); // Velocity low byte
  Serial.write(0X00); // Radius high byte
  Serial.write(0X01); // Radius low byte 
  
  
}


void Stop()
{
  // Stop drive motors
  Serial.write(0X89); // Drive motors
  Serial.write(0X00); // Velocity high byte
  Serial.write(0X00); // Velocity low byte
  Serial.write(0X00); // Radius high byte
  Serial.write(0X00); // Radius low byte
  
  // Stop cleaning motors
  Serial.write(0X8A); // Cleaning Motors
  Serial.write(0X00); // Stop all brushes
}


void power()
{
  Serial.write(0X85); // POWER: 131
  digitalWrite(ledPin, LOW);
}

void blinkLED()
{
  digitalWrite(ledPin, HIGH);
  delay(500);
  digitalWrite(ledPin, LOW);
  delay(500);
  digitalWrite(ledPin, HIGH);
  delay(500);
  digitalWrite(ledPin, LOW);
  
}

void loop()
{
  
  if(mySerial.available())
  {
    val=mySerial.read();
  }
  
  switch (val)
  {
    case 'w':
              forward();
              blinkLED();
              break;
              
    case 's': 
              backward();
              blinkLED();
              break;
              
    case 'a':
              leftTurn();
              blinkLED();
              break;
              
    case 'd':
              rightTurn();
              blinkLED();
              break;
              
    case 'q':
              Stop();
              blinkLED();
              break;
              
    case 'p': 
              power();
              blinkLED();
              break;
              
    case 'e':
              startCleaning();
              blinkLED();
              break;
              
  }
  
}