Hello all!
I’m in a robotics class and trying to do a 5 DOF robot. We’re doing some fairly advanced stuff like forward/reverse kinematics, etc. We’re using python for the matrix/linear algebra but I need a way to control the servos. I’ve watched several YouTube videos and seen a couple different ways: 1) Serial Communication and 2) using pyfirmata. I’m wondering if there’s a preference.
Any thoughts? I need to be able to command the angle as it changes or even the pulsewidth in necessary. Would like to keep the heavy lifting in the python code.
you could basically do all the heavy lifting on the python side as you say and just send commands through the Serial line. You "just" need to decide on a command language like
"SxAy" ➜ Servo #x to Angle y
"SxPy" ➜ Servo #x to pulsewidth y
The command could be followed by a new line as a separator. The Arduino code to read these commands is pretty straightforward. I would suggest to study Serial Input Basics to get the basics on how to handle this.
something like this would do
bool processCommand(const char* buf) {
if (buf[0] != 'S') return false; // wrong syntax
int servo, value;
char mode;
if (sscanf(buf, "S%d%c%d", &servo, &mode, &value) != 3) return false; // could not parse
if (mode != 'A' && mode != 'P') return false;
if (mode == 'A') Serial.printf("Servo #%d to Angle %d\n", servo, value);
else Serial.printf("Servo #%d to Pulsewidth %d\n", servo, value);
return true;
}
bool handleCommand() {
char buf[32];
int pos = 0;
while (Serial.available()) {
char c = Serial.read();
if (c == '\n') {
buf[pos] = 0;
return processCommand(buf);
} else if (pos < sizeof(buf) - 1) {
buf[pos++] = c;
}
}
return false;
}
void setup() {
Serial.begin(115200);
Serial.println("READY");
}
void loop() {
if (handleCommand()) {
// command processed
}
}
Thanks for the replies! @J-M-L Thanks for posting the code. That helps get me started. Like you note, I would just need to come up with a simple ‘scheme’ in the commands for telling the servo how far to turn but that’s easy enough. For the serial communications, would I have to run a separate FTDI/Serial cable to the Arduino board or could I use the same port that is used by to program it with? Wondering if the Arduino will tie up the serial port?? I will take a look at your link. Thx. @xfpd - That’s a interesting choice! I’ve never seen that. So do I have to get a pyBoard or can I use a normal Uno? All the code is done in Python with that? I’m just now starting to look through the link on how this works.
You can use the same cable - no problem. Just make sure the Serial Monitor is not open in the IDE as only one process can claim / grab the Serial line.
If you want to see debug messages then you’ll need two communication ports - one for the commands from your Python code to the Arduino and one for the debug interface .
Some boards like the Arduino Mega have multiple hardware ports, it would be good to use one like this as software emulation of serial ports tend to put pressure on interrupts and the board and the communication can get flaky.
The mega has also more capabilities (more ram, more timers,…) so it could be a good choice too for those reasons depending on what you do and need (An Arduino UNO can reliably control up to 12 servos using the Servo library. An Arduino Mega 2560 can control up to 48 servos. Both limits come from the Servo library design, not the physical PWM pins. Each servo uses one timer interrupt, and the Mega has more available timers and pins, allowing more concurrent servos..)
Thanks guys for the responses. I’m going to try just using a Mega and send serial commands down to it from the Python code. The Mega will only have to de-code the message and then set the PWM’s for the servos accordingly. So another question:
If I want to use a second serial port for the python-to-mega. Would I use pins TX2/RX2 and Gnd. Would I need the FTDI cable to convert down to 5V TTL signal from the PC? I’m assuming so.
I’m looking through J-M-L’s example code. Let me know if I understand this correctly. It does the following:
a) It creates a char buffer of 32 bits and a int for position tracking
b) If there’s data on the serial buss, read it one byte at a time while its available and add it to the buffer.
c) If you read a new line character (\n), set that char in the buffer to 0 and then send the buffer to the ‘processCommand’ function.
d) In that function, it uses a pointer. Using sscanf, it assigns updates servo ID, mode, and value. Then you’re just repeating it to the serial port.
e) For me, I would have servo, mode, and value as global variables so I can use them outside that function and then have code to take them and move the appropriate servo the appropriate value. Correct?
f) What does returning a True or False do for it?
Sure, or you can use TX1/RX1 or TX3/RX3 if you prefer.
Yes, you do need a USB to serial (TTL UART) adapter. The "FTDI cable" will work fine. It is actually more common these days to use one of the modules that are sold for a very reasonable price at all the usual sources of parts for electronics projects. Something like this:
@ptillisch Thanks for confirming what I figured. I happen to have a couple FTDI cables so I’m going to use one of those.
I’m having some trouble though. Right now I’m trying to just send data from my PC to my Mega’s Serial1 port and then print it to the normal Serial port so I can see it on the Serial Monitor.
Below is the code I’m using. When I enter something, it continuously prints out -1 over and over. I tried reversing it and sending data out to Serial1 port and that works just fine. I just can’t read anything in on it. I’m not sure why. If anyone has any ideas, I’d appreciate it!
Also, I got a question on how I’m going to send servo positions over. I’d like to send just a character array of the positions for all 6 servos in one shot such as: 100 25 9 210 150 230.
My concern is that the angles can be 1, 2, or 3 digits. If I use the sscanf() function, could I do this? How does that function determine when one number ends and the next one starts? Does it need spaces between the numbers? Is this a good idea or is there a better way?
Thanks!
int incomingByte = 0; // for incoming serial data
void setup() {
Serial.begin(9600); // opens serial port, sets data rate to 9600 bps
Serial1.begin(9600);
}
void loop() {
// send data only when you receive data:
if (Serial1.available() > 0) {
// read the incoming byte:
incomingByte = Serial.read();
// say what you got:
Serial.print("I received: ");
Serial.println(incomingByte, DEC);
}
}
Your understanding is correct. I used local variables but it’s OK to keep the info global if that makes your life easier.
Returning true or false was a way for the loop to know that a command had been processed if you wanted to have some follow up activity depending on that. It’s not necessary.
You have space separators so sscanf() will do the right thing just use the space in the format string "%d %d %d %d %d %d" and check the returned value as it will tell you how many fields were decoded - if it’s not 6 then the incoming buffer was not properly formatted.
Make sure also that the receiving buffer is large enough for your max possible length. 6 numbers of up to 3 characters plus 5 spaces plus potentially other stuff you send and a trailing null (32 was a good guess to be fine)
After fighting wiring issues and switching to my Uno board because my Mega’s serial port seemed messed up, I FINALLY am able to control the servos by sending one message with all 6 positions like “90 120 30 45 8 210”. I’ve attached my code for reference at the bottom. A follow up question. I put spaces between my angle values because I thought I read that you need to do that in order for sscanf() to work? I don’t see how it would be able to decide when one number ends and the other one starts. Is this true? In any case, I did what you said and added spaces between my %d’s and it works so I’m happy. Just trying to get a better feel for things.
Now I got to try to get PySerial to work. Is that something that is supported on this forum?
Thanks!
-#include <Servo.h>
char c;
char buf[32];
int pos = 0;
int pos1, pos2, pos3, pos4, pos5, pos6 = 0;
//Create Servo Objects
Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;
Servo servo5;
Servo servo6;
void setup()
{
Serial.begin(9600); // opens serial port, sets data rate to 9600 bps
servo1.attach(3); //Servo 1 is on pin 3
servo2.attach(5); //Servo 2 is on pin 5
servo3.attach(6); //Servo 3 is on pin 6
servo4.attach(9); //Servo 4 is on pin 9
servo5.attach(10); //Servo 5 is on pin 10
servo6.attach(11); //Servo 6 is on pin 11
}
void loop()
{
while (Serial.available())
{
c = Serial.read();
if (c == '\n')
{
Serial.println(buf);
sscanf(buf, "%d %d %d %d %d %d", &pos1, &pos2, &pos3, &pos4, &pos5, &pos6);
Serial.println(pos1);
Serial.println(pos2);
Serial.println(pos3);
Serial.println(pos4);
Serial.println(pos5);
Serial.println(pos6);
servo1.write(pos1);
delay(15); //Need small delay
servo2.write(pos2);
delay(15); //Need small delay
servo3.write(pos3);
delay(15); //Need small delay
servo4.write(pos4);
delay(15); //Need small delay
servo5.write(pos5);
delay(15); //Need small delay
servo6.write(pos6);
pos = 0; //reset pos pointer for buffer
}
else if (pos < sizeof(buf) - 1)
{
buf[pos++] = c;
}
}
}
The communication is in ascii so the number of bytes used to represent an int between 0 and 180 will have 1 to 3 character. If you send two numbers without a separator line 1123 how would you (as a human) know if I meant 1 and 123 or 11 and 23 or 112 and 3? The sscanf function would be like you and wouldn’t have a clue either. So the space is there to say that’s where the number representation ends.
Note that sscanf() reruns the number of data it was able to extract according to the format. You should really validate it got the 6 numbers before sending blindly the info to the servos
@J-M-L Thanks for the explanation. I figured it was along the lines of not knowing where one number ends the other one starts. I’ll take a look at your link for communicating with Python.
If you look at my code, I do print out the values I get for each position before I write them to the servos. I had this first to confirm I was getting the right numbers. It seems like the very first time the data is sent I sometimes get the last number repeated so instead of getting say 90, I got a 9090. But afterwards it seems stable. I’ll keep an eye on it. Thanks for showing my the sscanf() function. That is nice since I don’t have to worry if the angles are 1,2 or 3 digits long. It parses them based on the space.
Ok. I need to modify this code some. Instead of sending over desired angles, I need to send over desired pulse widths in usec. I need to do this because even if I increment motion by 1deg every 50ms, the robot is ‘jerky’ in its motion. I’d like if I send over the desire pulse widths in usec, I gain more resolution.
I tried sending over 1500 for the 1st value and it didn’t like it. Is an Int 1-byte or 2-bytes? I thought it was 2 so not sure why it didn’t like it.
Below is my code. Once I can read over larger numbers. I can then change the servo.write()’s to servo.writeMicroseconds(). What do I need to chane in order to send over integers ranging 1000 to 2000?
Thanks!
#include <Servo.h>
char c;
char buf[50];
int pos = 0;
int pos1, pos2, pos3, pos4, pos5, pos6 = 0;
//Create Servo Objects
Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;
Servo servo5;
Servo servo6;
void setup()
{
Serial.begin(115200); // opens serial port, sets data rate to 9600 bps
servo1.attach(3); //Servo 1 is on pin 3
servo2.attach(5); //Servo 2 is on pin 5
servo3.attach(6); //Servo 3 is on pin 6
servo4.attach(9); //Servo 4 is on pin 9
servo5.attach(10); //Servo 5 is on pin 10
servo6.attach(11); //Servo 6 is on pin 11
}
void loop()
{
while (Serial.available())
{
c = Serial.read();
if (c == '\n')
{
Serial.println(buf);
sscanf(buf, "%d %d %d %d %d %d", &pos1, &pos2, &pos3, &pos4, &pos5, &pos6);
Serial.println(pos1);
Serial.println(pos2);
Serial.println(pos3);
Serial.println(pos4);
Serial.println(pos5);
Serial.println(pos6);
//servo1.write(pos1);
servo1.write(pos1);
delay(1); //Need small delay
servo2.write(pos2);
delay(1); //Need small delay
servo3.write(pos3);
delay(1); //Need small delay
servo4.write(pos4);
delay(1); //Need small delay
servo5.write(pos5);
delay(1); //Need small delay
servo6.write(pos6);
pos = 0; //reset pos pointer for buffer
}
else if (pos < sizeof(buf) - 1)
{
buf[pos++] = c;
}
}
}
The issue is not with the Arduino int type itself. On an Arduino Uno (ATmega328P), int is 2 bytes, so it can hold values from -32,768 to 32,767. Sending numbers like 1500 is fine. The problem is likely with the way the Python code sends the data.
Right now, your Arduino code expects a the base 10 textual representation of the numbers separated by spaces and ending with \n, like 1500 1500 1500 1500 1500 1500\n.
If the Python code is sending raw bytes, Arduino's sscanf will fail ➜ can you clarify how you are testing and what code is sending the data ?
With 6 4-character numbers, five spaces, and(arguably) a needed command that you’ve not incorporated yet, plus the terminator(sum is now 30 characters + command), you’re getting quite close to this buffer size. It would be wise to extend this, before you run into problems without realizing it. Why? Let’s presume your python code, for whatever reason, calculates a maximum int value for one of the servo positions - likely, it’s a 32 bit int on a desktop, not 16, so let’s use 2147483647, the maximum positive 32 bit integer. So now you have a 10-char integer instead of a 4-char int in the middle of your command. You have a problem. You can mitigate that by bounds-checking the servo position values in Python, before building the send string. I’d also check for negative values in that case, unless you have unsigned ints as an option. I’m no python programmer.
Even with validating the data bounds at the python end, I would still bounds-check all received values before writing them to the servos; it’s just good practice, as there is always the possibility of a serial hiccup trashing a value. If you do incorporate a command structure that allows angle or pulse-width commands, bounds-check both options.
You do have one fallback protection, though - Servo.h happily limits the incoming position/angle values to the default, or user defined, limits. So nothing critical will happen, provided you have either set those limits to the limits your hardware design will tolerate, or your hardware design will tolerate the full swing of the servo (0→180) without damage.
The command structure referenced early in this thread would prevent that. It’s also good practice to do a serial flush() in setup, to prevent ‘hangover’ data from a previous run; however, your first priority should be a command structure that frames the data.
Where does the 50ms come from? Servo.h updates at 50 hz, which is 20 ms. Getting smooth motion is still difficult, though, because you have (at least) two async processes going on - the communication stream/parsing/updating Servo.h with position values is not sync’d to the interrupt process outputting those pulses to the servos, so even at 50 hz, using pulse widths, you can sometimes encounter glitches.
The cleanest way forward is to sync your write-to-Servo.h cycle to the interrupt cycle; that requires that you monitor your last servo’s digital output for end-of-pulse, and update all the servo positions at that time (while the interrupt cycle is asleep, waiting for the next 20 ms update to start).
I have done this, and it works very nicely; however, if you detach() your servos at end of move, be aware that the digital output writes go away, which can stall your process. I’ve gone as far as to add a phantom servo that gets attached, and never detached, just to circumvent this. The other option is to modify Servo.h to provide a readable status(“updating, please wait” sort of thing), and only update the servo positions when that flag says it’s safe to do so. Technically, there’s a variable within Servo.h that already indicates this, it’s just not exposed to the user. I guess no one ever thought users might want to know.
Respectfully, suggest you clean up your comments, I thought for a minute you were running at 9600!!! At 115200, you’ll receive about 11 chars/ms, so a buffer length of data should take < 3 ms, presuming the transmit end keeps up; processing that shouldn’t take anywhere near another ms, so you easily have the ability to receive a frame of data for every servo update cycle. That should allow for maximum stability, as you’ll consistently change all positions every servo cycle.
