Hi guys!
I'm making an animatronic robot head that will move using 16 servos with many of the servos moving at the same time. It needs to be controllable from different sources/clients (computers) that choose to connect to it or not. It doesn't have to be wireless but it would be great to have that possibility.
For my prototype now I am using an ESP8266 to receive Open Sound Control (OSC) messages that are sent from SuperCollider via a gamepad. This controls a PCA9685 16 channel servo controller which is connected to the ESP via I2C has gets a separate power source to drive the servos.
I am quite familiar with OSC and enjoy working with it, so if I can use it to control servos in a network I'd be very happy as it would mean interfacing would be very easy to adapt from many different platforms.
Now to the issue:
When controlling only one servo it works fine, all the values seem to get from the joystick to the servo, but when I try to control two or more servos at once I loose a lot of values, so many that mostly one servo moves and the other just stutters. It doesn't look like just a couple of packets are lost but more like it freezes up for ~500ms at a time, constantly, when moving more servos than one.
I have ruled out that it's a fault of power as I'm using a 5V/16A PSU for the servos on the PCA9685 (which also have a 1000uF capacitor) and have tried other PSUs). I did some troubleshooting that made me believe something around the OSC protocol, udp or wifi is the bottleneck. By removing the servo updating part from the arduino code (pwm.setPWM(0, 0)) and replacing it with sending a OSC message back to SuperCollider with the same values that I send, then I could visualize a very similar loss of values on a GUI as that what I saw on the servos.
This behavior is the same if the ESP clock is set to 80mhz or 160mhz.
I tried with a ESP32 board instead of ESP8266 and also got identical results.
Any help with this would be greatly appreciated. At this point I'm not sure if there is faulty/unoptimized code or if wifi or OSC or udp itself is the bottleneck, if the boards are not good enough, or how to troubleshoot this further.
Might be worth mentioning that my experience with arduino/code is somewhere between noob and intermediate, learning as I go 
#include <ESP8266WiFi.h>
#include <WiFiUdp.h>
#include <OSCMessage.h>
#include <OSCData.h>
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#define ID 10 //EDIT device id (also static ip)
#define PINMIC 0
#define PINBUT 2
//#define PINPWM 5
#define PORT 8888
#define UPDATERATE 16
#define TRIES 100 //how many times try check for wifi connection at startup
#define PINGRATE 600
#define STATICIP 0 //optional - 0 or 1
#define SERVOMIN 125 // this is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX 575 // this is the 'maximum' pulse length count (out of 4096)
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
const char *ssid = "*********"; //EDIT your accessPoint network name
const char *password = "*********"; //EDIT your password
const char *espname = "16chServo01";
const unsigned int outPort = 57120;
#if STATICIP==1
IPAddress ip(192, 168, 0, ID); //EDIT optional static ip
IPAddress gateway(192, 168, 0, 1); //EDIT optional router
IPAddress subnet(255, 255, 255, 0);
#endif
//Servo variables
float val[15], valTarget[15], lastVal[15];
int servoNumBuf = 0;
float valBuf = 0;
unsigned long nextTime;
int cnt;
IPAddress outIp;
WiFiUDP Udp;
OSCMessage msgPing("/16chServo01");
void setup() {
delay(10);
WiFi.mode(WIFI_STA);
WiFi.hostname(espname);
WiFi.begin(ssid, password);
#if STATICIP==1
WiFi.config(ip, gateway, subnet);
#endif
cnt = 0;
while ((WiFi.status() != WL_CONNECTED) && (cnt < TRIES)) {
delay(100);
cnt++;
}
outIp = WiFi.localIP();
outIp[3] = 2; //send to ip x.x.x.99 on same network (e.g. 192.168.1.99)
Udp.begin(PORT);
//Serial.begin(115200, SERIAL_8N1, SERIAL_TX_ONLY);
msgPing.add(ID);
msgPing.add("ping");
sendOscBut(1); //
pwm.begin(); // start servo board
pwm.setPWMFreq(60); // Analog servos run at ~60 Hz updates
delay(100);
for ( int i = 0; i <= 15; i++ ) { //intialize all 16 servos to 90
pwm.setPWM(i, 0, angleToPulse(90) );
delay(100);
}
}
void oscServo(OSCMessage &msg) { // Get values from OSC msg and store in variable
servoNumBuf = msg.getInt(0);
valBuf = msg.getFloat(1);
valTarget[servoNumBuf] = valBuf;
}
void sendOscBut(float val) { // Send return values to SC
OSCMessage msg("/16chServo01");
msg.add(ID);
msg.add("but");
msg.add(val);
Udp.beginPacket(outIp, outPort);
msg.send(Udp);
Udp.endPacket();
msg.empty();
}
void sendOscPing() { // Tell the world that you are still alive
Udp.beginPacket(outIp, outPort);
msgPing.send(Udp);
Udp.endPacket();
}
int angleToPulse(int ang) {
int pulse = map(ang, 0, 180, SERVOMIN, SERVOMAX); // map angle of 0 to 180 to Servo min and Servo max
//Serial.print("Angle: ");Serial.print(ang);
//Serial.print(" pulse: ");Serial.println(pulse);
return pulse;
}
void loop() {
//--OSC input
int packetSize = Udp.parsePacket();
if (packetSize) {
OSCMessage oscMsg;
while (packetSize--) {
oscMsg.fill(Udp.read());
}
if (!oscMsg.hasError()) {
oscMsg.dispatch("/servo", oscServo);
}
}
// PING
if (millis() >= nextTime) {
nextTime = millis() + UPDATERATE;
if (cnt % PINGRATE == 0) {
sendOscPing();
}
cnt++;
//SERVO
val[servoNumBuf] = valTarget[servoNumBuf];
if (val[servoNumBuf] != lastVal[servoNumBuf] ) {
lastVal[servoNumBuf] = valTarget[servoNumBuf];
pwm.setPWM(servoNumBuf, 0, val[servoNumBuf] );
//sendOscBut(val[servoNumBuf]);
}
}
}
