Hello -
I am currently making a fleet of robotic musical instruments mounted onto iCreate robots (the user friendly Roombas). These devices are supposed to be able to accept XBee commands to activate servos to strike bells. This is all controlled in Max/MSP.
My problem was that I started with an Arduino Leonardo, and learned that I couldn't do these things simultaneously:
(1) use a hardware serial1 to give commands to the Arduino Leo, which then passed via software serial to the Roomba,
(2) run three servos
(3) Send sensor data from the Roomba to the Arduino Leo via software serial
This seems to be because the timing gets messed up by interrupts. As far as I can tell, the software serial is the culprit, making the timed pulses to the servos unstable. This makes the servos flail and spasm rather than operate under my command.
I have tested my system without the streaming content from the Roomba's sensors being fed back to Arduino via software serial, and everything is fine. It's the addition of the streaming content that seems to mess everything up. Also looking at the signal on the servo pins confirms that the pulses are highly irregular in their timing, but the basic shape is still there.
So to me, I thought that using a Teensy 3 should solve this problem: 3 hardware ports, after all! But no, the problem persists.
I could probably solve this problem with a TLC5940 chip but is it necessary to go there? Why is it that this problem persists even after switching out of the software serial?
I have tried detaching and reattaching the servos as well, this isn't helping.
Here's the code in case anyone sees something odd in the coding; the communications with the Xbee are very consistent and the sending of messages is not a problem.
Cheers!
Erin
*~*~**~*~*~*~*~*~**~~**~*~*~*~*~**~*~*~*~**~*~*~**~*~*~*~*~*~*~**~*~~*~*~*
#include <Servo.h>
#include <XBee.h>
Servo myservo1;
Servo myservo2;
Servo myservo3;
void runScript(byte*command, int numCommands);
void streamControl(int state);
int testLed = 13;
XBee xbee = XBee();
XBeeResponse response = XBeeResponse();
ZBRxResponse rx = ZBRxResponse();
ModemStatusResponse msr = ModemStatusResponse();
int length;
int resting1 = 80; //the resting point value for the servos
int resting2 = 80;
int resting3 = 80;
int roombaBuffer [16];
int roombaBuffpointer = 0;
int leftBump = 0;
int rightBump = 0;
int virtualWall = 0;
int bumperLed = 13;
int checksum = 0;
int state = 1;
// radius then velocity
byte turnright[] = { 152, 13, 137, 0, 200, 0, 0, 157, 0, 90, 137, 0, 1, 1, 100 }; //these scripts tell the Roomba to turn when its bumper hits something
byte turnleft[] = { 152, 13, 137, 255, 56, 0, 0, 157, 255, 166, 137, 0, 1, 1, 100 };
char command;
byte commandBuffer[24];
int hit1a;
int hit1b;
int hit2a;
int hit2b;
int hit3a;
int hit3b;
int servoDelay1 = 0;
int servoDelay2 = 0;
int servoDelay3 = 0;
int returnTime = 20;
void servoReturn() { //this tells the servos to return to resting position automatically
if (servoDelay1 != 0) {
servoDelay1--;
if (servoDelay1 == 0) myservo1.write(resting1);
}
if (servoDelay2 != 0) {
servoDelay2--;
if (servoDelay2 == 0) myservo2.write(resting2);
}
if (servoDelay3 != 0) {
servoDelay3--;
if (servoDelay3 == 0) myservo3.write(resting3);
}
delay(1);
}
void setup()
{
delay(2000); //This is to allow the Roomba the chance to boot
// Serial.begin(9600); //debugging serial
// while (!Serial) delay (1000);
// Serial.println("serial began");
myservo1.attach(3);
myservo2.attach(4);
myservo3.attach(5);
delay(10);
myservo1.write(resting1);
myservo2.write(resting2);
myservo3.write(resting3);
// Serial.println("servos set");
pinMode(bumperLed, OUTPUT);
Serial1.begin(9600);
xbee.begin(Serial1);
// Serial.println("xbee serial on");
digitalWrite(testLed, HIGH);
delay(100);
digitalWrite(testLed, LOW);
delay(100);
digitalWrite(testLed, HIGH);
delay(100);
digitalWrite(testLed, LOW);
delay(100);
Serial2.begin(57600); //This is for the Roomba communication. The Roomba can also be set to 9600 but this doesn't seem to solve my problem either.
// Serial.println("Serial2 on");
delay(1000);
Serial2.write(128);
// Serial.println("Passive Mode");
delay(100);
Serial2.write(132);
// Serial.println("Full Control");
delay(100);
commandBuffer [0]=148;
commandBuffer [1]=2;
commandBuffer [2]=7;
commandBuffer [3]=13;
Serial2.write(commandBuffer, 4);
// Serial.println("commence roomba streaming sensor data");
delay(100);
}
void loop() {
while (Serial2.available()) {
if (roombaBuffpointer == 0) {
roombaBuffer[0] = Serial2.read();
if (roombaBuffer[0] == 19) roombaBuffpointer++;
}
else roombaBuffer[roombaBuffpointer++] = Serial2.read();
// Serial.println(roombaBuffer[0]);
if ((roombaBuffpointer >= 7) && (roombaBuffer[0] ==19)) {
roombaBuffpointer = 0;
checksum = 0;
for (int i=0; i<7; i++) {
checksum += roombaBuffer;
// Serial.println(roombaBuffer);
}
if (checksum == 256) {
// Serial.println("Checksum good! Let's read the sensor data");
leftBump = (roombaBuffer[3] &2)>>1;
rightBump = (roombaBuffer[3] &1);
virtualWall = (roombaBuffer[5]);
if (virtualWall != 0) digitalWrite(bumperLed, HIGH);
if (leftBump+rightBump != 0){
streamControl(0); //turns the data stream off temporarily
digitalWrite(bumperLed, HIGH);
if (leftBump == 1) {
runScript(turnleft, 15);
Serial2.write(153);
// Serial.println("Left bump!");
}
else if (rightBump == 1) {
runScript(turnright, 15);
Serial2.write(153);
// Serial.println("Right bump!");
}
streamControl(1); //turn stream back on
}
else digitalWrite(bumperLed, LOW);
}
}
}
xbee.readPacket();
if (xbee.getResponse().isAvailable()) {
// Serial.println("Got something");
if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) xbee.getResponse().getZBRxResponse(rx);
length = rx.getData(1);
// Serial.println(rx.getData(0)); // resetting the defaults for the servos
if (rx.getData(0) == 0) {
resting1=(rx.getData(2));
resting2=(rx.getData(3));
resting3=(rx.getData(4));
myservo1.write(resting1);
myservo2.write(resting2);
myservo3.write(resting3);
hit1a=(rx.getData(5));
hit2a=(rx.getData(6));
hit3a=(rx.getData(7));
hit1b=(rx.getData(8));
hit2b=(rx.getData(9));
hit3b=(rx.getData(10));
}
else if (rx.getData(0) == 1) { // giving the Roomba serial commands
for (int i=2; i<length+2;i++) Serial2.write(rx.getData(i));
}
else if (rx.getData(0) == 2){ // tell servo1 to go to position in variable 'pos'
if (rx.getData(2) == 1) myservo1.write(hit1a);
else if (rx.getData(2) == 2) {
myservo1.write(hit1b);
// Serial.write("servo1");
}
servoDelay1 = returnTime;
}
else if (rx.getData(0) == 3)
{
if (rx.getData(2) == 1) myservo2.write(hit2a);
else if (rx.getData(2) == 2) {
myservo2.write(hit2b);
// Serial.write("servo2");
}
servoDelay2 = returnTime;
}
else if (rx.getData(0) == 4)
{
if (rx.getData(2) == 1) myservo3.write(hit3a);
else if (rx.getData(2) == 2) {
myservo3.write(hit3b);
// Serial.write("servo3");
}
servoDelay3 = returnTime;
}
else if (xbee.getResponse().getApiId() == MODEM_STATUS_RESPONSE) xbee.getResponse().getModemStatusResponse(msr);
}
servoReturn();
}
void runScript(byte*command, int numCommands) {
// send script commands
clearScript();
for (int i = 0; i < numCommands; i++) {
Serial2.write(command);
delay(2);
}
delay(5);
}
void clearScript() {
byte nothing[] = {152, 0 };
Serial2.write(nothing[0]);
delay(1);
Serial2.write(nothing[1]);
delay(1);
}
void streamControl(int state) {
Serial2.write(150);
Serial2.write(state);
delay(10);
}
I am currently making a fleet of robotic musical instruments mounted onto iCreate robots (the user friendly Roombas). These devices are supposed to be able to accept XBee commands to activate servos to strike bells. This is all controlled in Max/MSP.
My problem was that I started with an Arduino Leonardo, and learned that I couldn't do these things simultaneously:
(1) use a hardware serial1 to give commands to the Arduino Leo, which then passed via software serial to the Roomba,
(2) run three servos
(3) Send sensor data from the Roomba to the Arduino Leo via software serial
This seems to be because the timing gets messed up by interrupts. As far as I can tell, the software serial is the culprit, making the timed pulses to the servos unstable. This makes the servos flail and spasm rather than operate under my command.
I have tested my system without the streaming content from the Roomba's sensors being fed back to Arduino via software serial, and everything is fine. It's the addition of the streaming content that seems to mess everything up. Also looking at the signal on the servo pins confirms that the pulses are highly irregular in their timing, but the basic shape is still there.
So to me, I thought that using a Teensy 3 should solve this problem: 3 hardware ports, after all! But no, the problem persists.
I could probably solve this problem with a TLC5940 chip but is it necessary to go there? Why is it that this problem persists even after switching out of the software serial?
I have tried detaching and reattaching the servos as well, this isn't helping.
Here's the code in case anyone sees something odd in the coding; the communications with the Xbee are very consistent and the sending of messages is not a problem.
Cheers!
Erin
*~*~**~*~*~*~*~*~**~~**~*~*~*~*~**~*~*~*~**~*~*~**~*~*~*~*~*~*~**~*~~*~*~*
#include <Servo.h>
#include <XBee.h>
Servo myservo1;
Servo myservo2;
Servo myservo3;
void runScript(byte*command, int numCommands);
void streamControl(int state);
int testLed = 13;
XBee xbee = XBee();
XBeeResponse response = XBeeResponse();
ZBRxResponse rx = ZBRxResponse();
ModemStatusResponse msr = ModemStatusResponse();
int length;
int resting1 = 80; //the resting point value for the servos
int resting2 = 80;
int resting3 = 80;
int roombaBuffer [16];
int roombaBuffpointer = 0;
int leftBump = 0;
int rightBump = 0;
int virtualWall = 0;
int bumperLed = 13;
int checksum = 0;
int state = 1;
// radius then velocity
byte turnright[] = { 152, 13, 137, 0, 200, 0, 0, 157, 0, 90, 137, 0, 1, 1, 100 }; //these scripts tell the Roomba to turn when its bumper hits something
byte turnleft[] = { 152, 13, 137, 255, 56, 0, 0, 157, 255, 166, 137, 0, 1, 1, 100 };
char command;
byte commandBuffer[24];
int hit1a;
int hit1b;
int hit2a;
int hit2b;
int hit3a;
int hit3b;
int servoDelay1 = 0;
int servoDelay2 = 0;
int servoDelay3 = 0;
int returnTime = 20;
void servoReturn() { //this tells the servos to return to resting position automatically
if (servoDelay1 != 0) {
servoDelay1--;
if (servoDelay1 == 0) myservo1.write(resting1);
}
if (servoDelay2 != 0) {
servoDelay2--;
if (servoDelay2 == 0) myservo2.write(resting2);
}
if (servoDelay3 != 0) {
servoDelay3--;
if (servoDelay3 == 0) myservo3.write(resting3);
}
delay(1);
}
void setup()
{
delay(2000); //This is to allow the Roomba the chance to boot
// Serial.begin(9600); //debugging serial
// while (!Serial) delay (1000);
// Serial.println("serial began");
myservo1.attach(3);
myservo2.attach(4);
myservo3.attach(5);
delay(10);
myservo1.write(resting1);
myservo2.write(resting2);
myservo3.write(resting3);
// Serial.println("servos set");
pinMode(bumperLed, OUTPUT);
Serial1.begin(9600);
xbee.begin(Serial1);
// Serial.println("xbee serial on");
digitalWrite(testLed, HIGH);
delay(100);
digitalWrite(testLed, LOW);
delay(100);
digitalWrite(testLed, HIGH);
delay(100);
digitalWrite(testLed, LOW);
delay(100);
Serial2.begin(57600); //This is for the Roomba communication. The Roomba can also be set to 9600 but this doesn't seem to solve my problem either.
// Serial.println("Serial2 on");
delay(1000);
Serial2.write(128);
// Serial.println("Passive Mode");
delay(100);
Serial2.write(132);
// Serial.println("Full Control");
delay(100);
commandBuffer [0]=148;
commandBuffer [1]=2;
commandBuffer [2]=7;
commandBuffer [3]=13;
Serial2.write(commandBuffer, 4);
// Serial.println("commence roomba streaming sensor data");
delay(100);
}
void loop() {
while (Serial2.available()) {
if (roombaBuffpointer == 0) {
roombaBuffer[0] = Serial2.read();
if (roombaBuffer[0] == 19) roombaBuffpointer++;
}
else roombaBuffer[roombaBuffpointer++] = Serial2.read();
// Serial.println(roombaBuffer[0]);
if ((roombaBuffpointer >= 7) && (roombaBuffer[0] ==19)) {
roombaBuffpointer = 0;
checksum = 0;
for (int i=0; i<7; i++) {
checksum += roombaBuffer;
// Serial.println(roombaBuffer);
}
if (checksum == 256) {
// Serial.println("Checksum good! Let's read the sensor data");
leftBump = (roombaBuffer[3] &2)>>1;
rightBump = (roombaBuffer[3] &1);
virtualWall = (roombaBuffer[5]);
if (virtualWall != 0) digitalWrite(bumperLed, HIGH);
if (leftBump+rightBump != 0){
streamControl(0); //turns the data stream off temporarily
digitalWrite(bumperLed, HIGH);
if (leftBump == 1) {
runScript(turnleft, 15);
Serial2.write(153);
// Serial.println("Left bump!");
}
else if (rightBump == 1) {
runScript(turnright, 15);
Serial2.write(153);
// Serial.println("Right bump!");
}
streamControl(1); //turn stream back on
}
else digitalWrite(bumperLed, LOW);
}
}
}
xbee.readPacket();
if (xbee.getResponse().isAvailable()) {
// Serial.println("Got something");
if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) xbee.getResponse().getZBRxResponse(rx);
length = rx.getData(1);
// Serial.println(rx.getData(0)); // resetting the defaults for the servos
if (rx.getData(0) == 0) {
resting1=(rx.getData(2));
resting2=(rx.getData(3));
resting3=(rx.getData(4));
myservo1.write(resting1);
myservo2.write(resting2);
myservo3.write(resting3);
hit1a=(rx.getData(5));
hit2a=(rx.getData(6));
hit3a=(rx.getData(7));
hit1b=(rx.getData(8));
hit2b=(rx.getData(9));
hit3b=(rx.getData(10));
}
else if (rx.getData(0) == 1) { // giving the Roomba serial commands
for (int i=2; i<length+2;i++) Serial2.write(rx.getData(i));
}
else if (rx.getData(0) == 2){ // tell servo1 to go to position in variable 'pos'
if (rx.getData(2) == 1) myservo1.write(hit1a);
else if (rx.getData(2) == 2) {
myservo1.write(hit1b);
// Serial.write("servo1");
}
servoDelay1 = returnTime;
}
else if (rx.getData(0) == 3)
{
if (rx.getData(2) == 1) myservo2.write(hit2a);
else if (rx.getData(2) == 2) {
myservo2.write(hit2b);
// Serial.write("servo2");
}
servoDelay2 = returnTime;
}
else if (rx.getData(0) == 4)
{
if (rx.getData(2) == 1) myservo3.write(hit3a);
else if (rx.getData(2) == 2) {
myservo3.write(hit3b);
// Serial.write("servo3");
}
servoDelay3 = returnTime;
}
else if (xbee.getResponse().getApiId() == MODEM_STATUS_RESPONSE) xbee.getResponse().getModemStatusResponse(msr);
}
servoReturn();
}
void runScript(byte*command, int numCommands) {
// send script commands
clearScript();
for (int i = 0; i < numCommands; i++) {
Serial2.write(command);
delay(2);
}
delay(5);
}
void clearScript() {
byte nothing[] = {152, 0 };
Serial2.write(nothing[0]);
delay(1);
Serial2.write(nothing[1]);
delay(1);
}
void streamControl(int state) {
Serial2.write(150);
Serial2.write(state);
delay(10);
}