Teensy 3/teensyduino - Issue with Servo library and streaming data input on Serial

Status
Not open for further replies.

eringee

Active member
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);
}
 
This really should work.

I could investigate and get to the bottom of this, if only I had it in a form that is just two boards talking to each other (so I can reproduce the problem here with 3 servo motors). I just can't reproduce this if it depends on the XBee and Roomba.
 
Hey Paul -

From what I have noticed, the Xbee works with the servos fine. I have a video of all three things (the roomba, the servos, the Xbee) working. So I can send commands to the Roomba at the same time as the Arduino receives info for the servos. I think the streaming data is what messes it up.

I don't need the Xbee attached to have this servo twitch problem. Even when I remove the Xbee, the twitch happens.

Maybe one could simulate a Roomba, if you can simulate streaming serial data every 15ms or so going into the Arduino via serial port...

This is the information on the Roomba

http://www.irobot.com/filelibrary/pdfs/hrd/create/Create Open Interface_v2.pdf - page 13/14 talks about streaming data.

I am very curious - yes I thought that this situation should work as well so I have been very baffled! Thanks for thinking about this! Let me know if there is anything I can do, short of sending you a vacuum robot in the mail!
 
oh, also the streaming data I am receiving is "bumps and wheel drops" and "virtual wall sensor", described on page 17 of that irobot pdf.
 
Maybe one could simulate a Roomba, if you can simulate streaming serial data every 15ms or so going into the Arduino via serial port...

This is the information on the Roomba

http://www.irobot.com/filelibrary/pdfs/hrd/create/Create Open Interface_v2.pdf - page 13/14 talks about streaming data.

Maybe something like this running on another Teensy or the Arduino Leonardo could substitute for the Roomba?

Code:
unsigned char roomba[] = {19, 152, 17, 158, 5, 158, 251, 139, 2, 0, 0, 158, 5, 158, 251, 139, 0, 255};

void loop() {
  Serial.write(roomba, sizeof(roomba));
  delay(15);
}

I am very curious - yes I thought that this situation should work as well so I have been very baffled! Thanks for thinking about this! Let me know if there is anything I can do, short of sending you a vacuum robot in the mail!

LOL, don't ship me a Roobma!

I'm curious too. I really want to investigate.

If you can just capture some of the data, maybe with Serial.print() to Arduino's Serial Monitor, and then put it into a tiny program running on another board to take the place of the Roomba, then I can connect the same setup here.

If you use the 5V Arduino Leonardo to send the data, use a 10K resistor in series with the data 5V signal before it goes into the 3.3V pin.
 
unsigned char roomba[] = { 19, 4, 7, 0, 13, 0, 213 };
void setup()
{
Serial1.begin(57600);
while (!Serial1) delay(1000); //needed on Leonardo boards
}

void loop() {
for (int i = 0; i < 6; i++) {
Serial1.write(roomba);
}
delay(15);
}


This should work for simulating the data.

Will test this more later tonight, my initial tests are actually successful with the Xbee, but I haven't been able to Serial.println(roombaBuffer) in order to well, make sure that the Teensy is "getting" what the Leonardo is sending out.

Thanks again
 
Ugh sorry I am just looking at this now and this is the real code...

unsigned char roomba[] = { 19, 4, 7, 0, 13, 0, 213 };
void setup()
{
Serial1.begin(57600);
while (!Serial1) delay(1000); //needed on Leonardo boards
}

void loop() {
Serial1.write(roomba, 7);
delay(15);
}

I can report that after reproducing this on a Leonardo, with a Xbee sending wireless controls, everything works fine. So I guess the problem is in fact with the Roomba. Aie aie aie....
 
And amazingly enough, now the problem has disappeared. Paul I'm really sorry to have bothered you with this but after two months of this NOT WORKING it suddenly is working. Which is great because it means I'm going to buy a whackload of Teensy 3.0 and Xbee shields now for my fleet of robotic minions :)
 
Glad it's working!

If the problem does reappear, please try to capture a set of code I can use to reproduce it here. If I can reproduce a problem, I can usually get to the bottom of it.
 
Status
Not open for further replies.
Back
Top