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