
/*
 * brmrat -- button-based roomba control
 *
 *  based on: http://roombahacking.com/software/arduino/RoombaBumpTurn.pde
 *-
 * brmrat
 * ------
 * Implement the RoombaComm BumpTurn program in Arduino
 * A simple algorithm that allows the Roomba to drive around-
 * and avoid obstacles.
 *-
 * Arduino pin 3      is connected to Roomba TXD
 * Arduino pin 4      is connected to Roomba RXD
 * Arduino pin 5      is conencted to Roomba DD
 *-
 * In addition, three buttons are available:
 *-
 * Arduino pin 10     is connected to button LEFT
 * Arduino pin 11     is connected to button FORWARD
 * Arduino pin 12     is conencted to button RIGHT
 *-
 */


/* Three modes - uncomment one: */
//#define NO_CONTROL // buttons are ignored
//#define DIR_CONTROL // left/right honored, going forward all the time
#define FULL_CONTROL // left/right/forward honored

#include 

/// serial ports
int rxPin = 0;
int txPin = 1;
int rxPin_nss = 3;
int txPin_nss = 4;
// we want both the SCI conn to the roomba and the USB (for debugging) to be available,
//   one to the hardware UART and one via NewSoftSerial (NSS).
// --------> SCI on NSS / USB on the UART:
int ddPin = 5;  // keep ddPin next to SCI
#define usbSerial Serial
NewSoftSerial sciSerial(rxPin_nss,txPin_nss);

int ledPin = 13;

int leftPin = 10;
int forwardPin = 11;
int rightPin = 12;

char sensorbytes[10];
#define bumpright (sensorbytes[0] & 0x01)
#define bumpleft  (sensorbytes[0] & 0x02)

int butleft, butright, butfwd;

void setup() {
  pinMode(ddPin,  OUTPUT);
  pinMode(ledPin, OUTPUT);

  pinMode(leftPin, INPUT);
  pinMode(forwardPin, INPUT);
  pinMode(rightPin, INPUT);
  // pull up...
  digitalWrite(leftPin, HIGH);
  digitalWrite(forwardPin, HIGH);
  digitalWrite(rightPin, HIGH);

  //sciSerial.begin(57600);
  sciSerial.begin(19200);

  usbSerial.begin(9600);
  usbSerial.print("starting up... ");

  digitalWrite(ledPin, HIGH); // say we're alive

  // wake up the robot
  //digitalWrite(ddPin, HIGH);
  //delay(100);
  //digitalWrite(ddPin, LOW);
  //delay(500);
  //digitalWrite(ddPin, HIGH);
  //delay(2000);
  // set up ROI to receive commands--
  sciSerial.print(128, BYTE);  // START
  delay(50);
  sciSerial.print(130, BYTE);  // CONTROL
  delay(50);

  digitalWrite(ledPin, LOW);  // say we've finished setup
  usbSerial.println("ready!");

  // [re]set baud rate to 57600
  //sciSerial.print(129, BYTE);  // BAUD
  //sciSerial.print(10, BYTE);   // 57600
  //delay(100);

  //dance(); //  demonstrate that controls work
}

void loop() {
  digitalWrite(ledPin, HIGH); // say we're starting loop
  updateSensors();
  digitalWrite(ledPin, LOW);  // say we're after updateSensors

  if(bumpleft) {
    usbSerial.println("left bump!");
    stopMoving();
    delay(500);
    spinRight();
    delay(1000);
    stopMoving();
    delay(500);
  }
  else if(bumpright) {
    usbSerial.println("right bump!");
    stopMoving();
    delay(500);
    spinLeft();
    delay(1000);
    stopMoving();
    delay(500);
  }

#ifdef NO_CONTROL
  goForward();
#else
  updateButtons();

  signed short velocity = 200, dir = 0;
  signed short dir = butleft ? 1 : (butright ? -1 : 0);
#ifdef FULL_CONTROL
  if (!butfwd && !dir)
    velocity = 0;
#endif
  goAnywhere(velocity, dir);
#endif
}

void goAnywhere(signed short velocity, signed short radius)
{
  sciSerial.print(137, BYTE);   // DRIVE
  sciSerial.print(velocity >> 8,BYTE);
  sciSerial.print(velocity & 0xff,BYTE);
  sciSerial.print(radius >> 8,BYTE);
  sciSerial.print(radius & 0xff,BYTE);
}

void goForward() {
  sciSerial.print(137, BYTE);   // DRIVE
  sciSerial.print(0x00,BYTE);   // 0x00c8 == 200
  sciSerial.print(0xc8,BYTE);
  sciSerial.print(0x80,BYTE);
  sciSerial.print(0x00,BYTE);
}
void goBackward() {
  sciSerial.print(137, BYTE);   // DRIVE
  sciSerial.print(0xff,BYTE);   // 0xff38 == -200
  sciSerial.print(0x38,BYTE);
  sciSerial.print(0x80,BYTE);
  sciSerial.print(0x00,BYTE);
}
void spinLeft() {
  sciSerial.print(137, BYTE);   // DRIVE
  sciSerial.print(0x00,BYTE);   // 0x00c8 == 200
  sciSerial.print(0xc8,BYTE);
  sciSerial.print(0x00,BYTE);
  sciSerial.print(0x01,BYTE);   // 0x0001 == spin left
}
void spinRight() {
  sciSerial.print(137, BYTE);   // DRIVE
  sciSerial.print(0x00,BYTE);   // 0x00c8 == 200
  sciSerial.print(0xc8,BYTE);
  sciSerial.print(0xff,BYTE);
  sciSerial.print(0xff,BYTE);   // 0xffff == -1 == spin right
}
void stopMoving() {
  sciSerial.print(137, BYTE);   // DRIVE
  sciSerial.print(0x00,BYTE);   // 0x0000 == 0
  sciSerial.print(0x00,BYTE);
  sciSerial.print(0x00,BYTE);
  sciSerial.print(0x00,BYTE);   // 0x0000
}
  
void updateSensors() {
  //usbSerial.println("in updateSensors()");
  sciSerial.print(142, BYTE);
  sciSerial.print(1,   BYTE);  // sensor packet 1, 10 bytes
  //delay(100); // wait for sensors 
  delay(64);

  // wipe old sensor data
  char i = 0;
  while (i < 10) {
    sensorbytes[i++] = 0;
  }
  i = 0;
  while(sciSerial.available()) {
    //usbSerial.println("rsd");  // reading sensor data
    int c = sciSerial.read();
    if( c==-1 ) {
      usbSerial.println("error read()ing sensors");
      for( int i=0; i<5; i ++ ) {   // say we had an error via the LED
        digitalWrite(ledPin, HIGH); 
        delay(50);
        digitalWrite(ledPin, LOW);  
        delay(50);
      }
    }
    sensorbytes[i++] = c;
  }
  if (i != 10) {   // debug: print # bytes read (should always be 10?)
    usbSerial.print("error:  only ");
    usbSerial.print(i,DEC);
    usbSerial.println(" bytes read");
  }

/*
  i = 0;
  while (i < 10) {
    if (sciSerial.available()) {
      usbSerial.println("rsd");  // reading sensor data
      int c = sciSerial.read();
      if( c==-1 ) {  // shouldn't happen
        usbSerial.println("error read()ing sensors");
      }
      sensorbytes[i++] = c;
    }
*/

}

void updateButtons() {
  /* We are connected through pull-up resistors, so we need to negate
   * the readings. */
  butleft = !digitalRead(leftPin);
  butfwd = !digitalRead(forwardPin);
  butright = !digitalRead(rightPin);
}