writing to serial for multiple servo control

here's a visual of what i'm playing with: iPhone Controlled Robotic Arm on Vimeo

the script i used for the video above works fairly well - except it's not smart and sometimes it will write to the wrong servos and i'd have to manually change the order in which i was writing to the servos in the code. that code can be found here: http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1291012063

so i set off to find a better way of sending multiple variables via serial to the arduino. i found a bunch of posts by PaulS saying search for: "started && ended" in the forums... after some searching i found the "packet" technique he was referring to.

i began to implement.

what WAS a processing script capturing OSC messages from my iphone is now replaced by this ruby script:

# require libraries
%w[ rubygems osc-ruby serialport ].each { |lib| require lib }

# connect to arduino
@sp = SerialPort.new "/dev/tty.usbserial-A800etAZ", 9600, 8, 1, SerialPort::NONE

# initial arm position
@shoulder, @forearm, @elbow, @claw = 180, 50, 60, 35

# setup osc server so we can listen for messages from the multi-touch on my iphone
osc_server = OSC::Server.new 'razic.local', 4000

# these are the osc patterns and their callbacks
# it's callback is triggered when a osc message matching one of our addresses is received
osc_addrs = [
  ["/1/shoulder_forearm", Proc.new { |m| @shoulder, @forearm = *m.instance_variable_get(:@args); }], 
  ["/1/elbow", Proc.new { |m| @elbow = m.instance_variable_get(:@args)[0]; }],
  ["/1/claw", Proc.new { |m| @claw = m.instance_variable_get(:@args)[0]; }]
]
osc_addrs.each { |a| osc_server.add_method a[0], &a[1] }

# run the osc server in a new thread
Thread.new { osc_server.run }

# start talking to the arduino
loop do
  # serially send 4 packets (one for each servo)
  @sp.write "<s#{@shoulder.to_i}><f#{@forearm.to_i}><e#{@elbow.to_i}><c#{@claw.to_i}>"
  
  sleep 1
end

and here is the new arduino code that interprets the packets and writes to the appropriate motor:

#include <Servo.h>

Servo shoulder, forearm, elbow, claw;

int shoulder_pin = 3;
int forearm_pin = 5;
int elbow_pin = 6;
int claw_pin = 9;
int baud = 9600;
char buff[4];
byte i = 0;
boolean started = false;
boolean ended = false;

void setup(){
  Serial.begin(baud);
  
  shoulder.attach(shoulder_pin);
  forearm.attach(forearm_pin);
  elbow.attach(elbow_pin);
  claw.attach(claw_pin);
}

void loop(){
  while(Serial.available() > 0){
    char aChar = Serial.read();

    if(aChar == '<'){ 
      started = true; 
      ended = false; 
    } 
    else if(aChar == '>'){
      ended = true;
      break; // break the while loop
    } 
    else {
      buff[i] = aChar;
      i++;
      buff[i] = '\0'; // this terminates the array
    }
  }

  if(started && ended){
    int intpos;
    char pos[3] = { buff[1], buff[2], buff[3] };
    intpos = atoi(pos);
    
    Serial.print("Writing motor ");
    Serial.print(buff[0]);
    Serial.print(" to ");
    Serial.println(intpos);
    Serial.println();
    
    if(buff[0] == 's') shoulder.write(intpos);
    if(buff[0] == 'f') forearm.write(intpos);
    if(buff[0] == 'e') elbow.write(intpos);
    if(buff[0] == 'c') claw.write(intpos);
    
    i = 0;
    buff[i] = '\0'; // this terminates the array
    started = false;
    ended = false;
  }
}

so heres the big problem: in the ruby script, i put a one second delay between every loop that it writes the servo positions to the arduino.

this works well. but much to slow to emulate my actual fingers movement on the iphone's multitouch. in the old code, (in the link above) i had no delay and it worked really smooth and reacted almost perfectly to realtime.

as soon as i change the delay to 0.1 second the arm flips out and starts moving when it shouldnt etc..

is this because of packet loss/interuption? keep in mind that i am constantly writing the servo position even if it's unchanged since the last time.

is this because of packet loss/interuption?
Almost certainly. The ruby script is sending data faster than the Arduino can process it. There are 3 possible solutions.

The first is a change to this statement.

int baud = 9600;

The Arduino is capable of communicating at much higher rates than this. Try increasing the rate to 57600 or 115200.

The second change would be to have the Arduino send an "I'm ready" message whenever it is ready for more data. This would prevent ruby from sending too much data.

The third, and by far the best, is to have ruby keep track of the previous values sent, and only send deltas. Each packet would then contain exactly 6 bytes - "<", ds, df, de, dc, ">", where ds, df, de, and dc are sent as bytes, not strings. There would be no need to convert strings to integers, or determine which servo the data was for.

This method requires some care, since a byte is unsigned, and the change in servo position is signed. No change would then be sent as 128. Changes would be sent as 128+delta.

On the Arduino, it would need to keep track of the old value, and extract delta from the byte sent (val - 128 = delta), and use the old value and delta (if delta is not 0) to compute the new value to write to the servo.

Is the "sleep 1" in the ruby code 1 sec or 1ms?

@zoomkat that is 1 second in ruby

@PaulS thanks for your generous reply!

after changing the baud to 115200, the arm had responded fairly well and i could get the sleep method in the ruby loop down to basically .00000001 second without any packet loss (however it responds best at .01).

additionally in the ruby script, i recorded the position of the last degree for each ligament (servo) and then only wrote to the serial port if the degree had changed. this cut down on the size of packets sent generously.

the arm seems to experience 0 packet loss. and responds well for communicating via wifi.

do you think any of the other methods you suggested would result in a smoother experience or is this about as good as it gets?

i posted the code here for all to see: https://github.com/razic/arm

thanks again for your help!

You are still sending the position as a string, which requires converting the number to an array of characters, sending the characters one at a time, and converting the array of characters back to a number.

The advantage with this method is that you only need to send data for a joint if the joint changed. If only the elbow moves, you only send data for the elbow - ex, exx, or exxx.

If you changed to sending 4 bytes, one for each joint, every time any joint changed, sometimes this would involve more data (4 bytes vs 2 in ex or 4 bytes vs 3 in exx), sometimes it would involve the same amount of data (4 bytes vs exxx), and sometimes it would result in less data (4 bytes vs sxx and exxx and cx).

If it is working well enough now, I'd leave it alone.

It all depends, I guess, on where the data to send is coming from. If it is from a GUI that has sliders that have to be moved for each joint, then leave it as it is. If it comes from somewhere where all 4 joints can change at the same time, then consider changing.

@PaulS although it was working very well, i was curious to see what changes your new code suggestions would bring...

first, i created an array to put the servos objects in.

then, instead of sending the degree over in string form i figured out how to send that value over as a byte. and since i put the servos in an array, i just send 0, 1, 2 or 3 for which servo instead of "s", "f", "e" and "c". now, instead of a 4 byte buffer - i'm down to 2 per packet.

the new packet format is: "<" (string), 0-3(byte for servo index), 0-180(byte for degree), ">" (string)

this allowed me to get rid of the if statements in my code like:

if(inByte == 's') shoulder.write(pos);
if(inByte == 'f') forearm.write(pos);

and replace with one line of code to write to the motor (thanks to the array):

arm[buff[0]].write[buff[1]];

not to mention there's no need to convert the position to an integer either now.

with all your advice in place:

  • only writing to serial port when a change to the degree is made
  • sending bytes instead of string
  • increased baud rate

the arm now works freakishly smooth!

i have one last final question:

after reading this

If you changed to sending 4 bytes, one for each joint, every time any joint changed, sometimes this would involve more data (4 bytes vs 2 in ex or 4 bytes vs 3 in exx), sometimes it would involve the same amount of data (4 bytes vs exxx), and sometimes it would result in less data (4 bytes vs sxx and exxx and cx).

do you think it would make more sense to send packets with information about every joint each time - or packets with information about a single joint each time?

note the UI for controlling the arm is my iphones multitouch in which i am emulating a 2 axis joystick (x,y) for the shoulder and forearm and a two other sliders: one for the elbow and finally for the claw. using both hands, all four motors can be moved from the UI easily. i'm not sure if this makes a difference for the above question

thanks!

the arm now works freakishly smooth!

Excellent. :slight_smile:

do you think it would make more sense to send packets with information about every joint each time - or packets with information about a single joint each time?

Whether sending <servoNumber,Pos> for each servo or <Pos1,Pos2,Pos3,Pos4> results in less data depends on how often two servos are moving at the same time. If, in general, only one servo moves at a time, then the servoNumber,Pos method works best. If, in general, multiple servos are moving simultaneously, then the Pos1,Pos2,Pos3,Pos4 method works best.

As long as it is working well enough, though, I'd leave it like it is.