I've been working on this piece of code to control a large array of servos on Arduino using Python. The code works, but I've seen some strange behavior surrounding timing. Between each update to the servos, I need a sleep command. If I don't insert a sleep, the program will work the first time I run it, but if the Python code gets stopped, then I try to connect over serial with Python again, the Arduino is non responsive. What could be happening to the serial port and how could I prevent this from happening?
Also, am I parsing the data coming into the Arduino the most efficient way possible using memcpy? Or is there a better or more standard way to do this?
I've read about using serialEvent, is there a benefit to using that command for parsing serial data in this situation?
//ARDUINO CODE
#include <Servo.h>
int servoPins[] = {9, 10};
//int servoPins[] = {2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38};
//int servoPins[] = {2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34};
const int numServos = sizeof(servoPins)/sizeof(int);
Servo servos[numServos];
char serialData[numServos * 3];
char tempData[3];
void setup() {
Serial.begin(9600);
Serial.println("Ready");
for (int i=0; i < numServos; i++) {
servos[i].attach(servoPins[i]);
//servos[i].write(20);
}
}
void loop() {
if (Serial.available()) {
Serial.readBytesUntil('\0', serialData, numServos * 3);
for (int i=0; i < numServos; i++) {
memmove(tempData, serialData + i * 3, 3);
servos[i].write(atoi(tempData));
}
}
}
#PYTHON CODE
""" Control n servos on n arduinos over serial
"""
import glob
import platform
import serial
from time import sleep
import sys
'''
Servo
Id number, angle for servo, Arduino port servo is on
For now the port is used as an index, so please number them going from 0-n
'''
class Servo:
id_num = 0
angle = 0
port = 0
def __init__(self, id_num, angle, port):
self.id_num = id_num
self.angle = angle
self.port = port
'''
ServoDriver
-Stores a list of servos
-Open ports to Arduinos
-Stores a list of those ports
-Creates a map of which servos are on which ports
-Provides a way to update the angle of all servos on all ports
'''
class ServoDriver:
def __init__(self, servos):
self.ports = self.open_ports()
self.servos = servos
# looks for all devices that have the same name pattern as an Arduino and opens them
def open_ports(self):
# find arduinos
# note: currently this is Mac only
devices = glob.glob('/dev/tty.usbmodem*')
print devices
if len(devices) == 0:
print "No Arduinos found"
sys.ext(1)
ports = []
for device in devices:
try:
# connect to serial port
ports.append(serial.Serial(device, 9600))
except:
print 'Failed to open port'
sys.ext(1)
# need a short delay right after serial port is started for the Arduino to initialize
sleep(2)
return ports
# update the angle of all servos on all ports
def update(self, servos):
debug = True
servo_data = []
for p in self.ports:
servo_data.append('')
for servo_update in servos:
for servo_stored in self.servos:
if servo_stored.id_num == servo_update.id_num:
this_port = servo_stored.port
break
# constrain servo angles to avoid damaging servos
if servo_update.angle > 135:
servo_update.angle = 135
else if servo_update.angle < 45:
servo_update.angle = 45
# append angle to the datum for this port
servo_data[this_port] = servo_data[this_port] + str(servo_update.angle).zfill(3)
for servo_datum in servo_data:
# append null byte for arduino to recognize end of data
servo_datum = servo_datum + "\0"
# send data to the Arduinos
for port,servo_datum in zip(self.ports,servo_data):
port.write(servo_datum)
def close_ports(self):
print 'closing ports'
for port in self.ports:
port.close()
# generates values for making a servo sweep back and forth
def servo_iter():
l = []
#for i in range(0,1):
l.append(40)
#for i in range(0,1):
l.append(80)
for pos in l:
yield pos
def servo_iter_2(total):
for i in range(0,total):
yield i
if __name__ == "__main__":
# create a list of servos with mappings to ports
# if you have the wrong number of servos it acts weird
#num_servos = 32
num_servos = 2
servos = []
servos.append(Servo(0, 40, 0))
servos.append(Servo(1, 40, 0))
'''
servos.append(Servo(2, 40, 0))
servos.append(Servo(3, 40, 0))
servos.append(Servo(4, 40, 0))
servos.append(Servo(5, 40, 0))
servos.append(Servo(6, 40, 0))
servos.append(Servo(7, 40, 0))
servos.append(Servo(8, 40, 0))
servos.append(Servo(9, 40, 0))
servos.append(Servo(10, 40, 0))
servos.append(Servo(11, 40, 0))
servos.append(Servo(12, 40, 0))
servos.append(Servo(13, 40, 0))
servos.append(Servo(14, 40, 0))
servos.append(Servo(15, 40, 0))
servos.append(Servo(16, 40, 0))
servos.append(Servo(17, 40, 0))
servos.append(Servo(18, 40, 0))
servos.append(Servo(19, 40, 0))
servos.append(Servo(20, 40, 0))
servos.append(Servo(21, 40, 0))
servos.append(Servo(22, 40, 0))
servos.append(Servo(23, 40, 0))
servos.append(Servo(24, 40, 0))
servos.append(Servo(25, 40, 0))
servos.append(Servo(26, 40, 0))
servos.append(Servo(27, 40, 0))
servos.append(Servo(28, 40, 0))
servos.append(Servo(29, 40, 0))
servos.append(Servo(30, 40, 0))
servos.append(Servo(31, 40, 0))
servos.append(Servo(32, 40, 0))
'''
#if len(servos) != num_servos:
# print 'wrong number of servos'
# sys.ext(1)
# comment out the next two lines if you only have 1 arduino
#servos.append(Servo(2, 40, 1))
#servos.append(Servo(3, 40, 1))
#servos.append(Servo(4, 40, 2))
#servos.append(Servo(5, 40, 2))
angles = []
for i in range(0,len(servos)):
angles.append(40)
try:
# instantiate a driver
# must happen inside try-finally
driver = ServoDriver(servos)
iter1 = False
if iter1:
pos = servo_iter()
else:
pos = servo_iter_2(len(servos))
while True:
try:
x = pos.next()
except StopIteration:
if iter1:
pos = servo_iter()
else:
pos = servo_iter_2(len(servos))
x = pos.next()
# create a list of servos with ids and angles to update positions of servos
if iter1:
for servo in servos:
servo.angle = x
else:
for i,servo in zip(angles,servos):
servo.angle = i
# call the driver with the list of servos
driver.update(servos)
sleep(0.5)
for i in range(0, len(servos)):
if i == x:
angles[i] = 80
else:
angles[i] = 40
# close the serial port on exit, or you will have to unplug the arduinos to connect again
finally:
driver.close_ports()
I think the right answer here was to add a delay to the Arduino.
sleep(0.5)to the end. It seems to me thatserial.writefunction returns early, and thus can be hindered by immediatesleepin its thread. Most likely I am wrong on this.