Adafruit 16-channel board - controlling all channels by serial - optimisation

Hello,

I am trying to control a few servo motors using this 16-channerl board. I am running to some issues about accuracy, for example, when moving a couple of motors do draw a diagonal line, because of the delay between each servo, each motor will move in different timing resulting in incorrect drawings.

I am not sure about how to drive the motors in the fastest way in therms of code.
Messages are coming from serial, as explained in the code comment.

Is this the right way to drive this board channels?

I am using an arduino uno, but I would like to check if using a Teensy 3.2 results in best performances for this application.

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>

#define SERVOMIN  150
#define SERVOMAX  600


// temporary setting pins for 4 lights - it will be controlled by some decade counter in the future
#define L1 9
#define L2 10
#define L3 11
#define L4 12
/*
 * a "pointer" device includes a light and 2 servos. Parameters from serial are:
 * index,light,servo1,servo2; <- parameters separated by ',' end of pointer is ';'
 * 
 * example of how serial is coming containing instructions for 4 pointers;
  0,0,180,180;1,0,0,0;2,0,180,180;3,0,0,0; 

  0,0,90,90;1,0,90,90;2,0,90,90;3,0,90,90;

  most of the time these instructions doesn't come all for 4 pointers.
  ex:
  1,0,12,12;4,255,100,100;
  
  sometimes it comes only id and light parameter.
  0,255;1,0;
  (instructions only to turn light on/off)
*/

//boards (for now, only one board = 16 servos)
Adafruit_PWMServoDriver pwm [] = {
  Adafruit_PWMServoDriver(0x40)
};

uint8_t servonum = 0;
//uint8_t activeServos = 8; //not being used now
char buf [4]; //maybe can be reduced to a byte, since reading values under 255
uint16_t currentPointer [4]; //index//light//servo1//servo2
byte lightPin [4] = {L1, L2, L3, L4};
uint8_t lightStatus [4] = {0, 0, 0, 0};

//debug

String inputString = "";         // a string to hold incoming data
boolean stringComplete = false;  // whether the string is complete
boolean feedback = false;

void setup() {

  //temporally as digital outputs
  pinMode(L1, OUTPUT);
  pinMode(L2, OUTPUT);
  pinMode(L3, OUTPUT);
  pinMode(L4, OUTPUT);

  Serial.begin(115200);//230400 //115200 //57600 //38400
  for ( uint8_t i = 0; i < sizeof(pwm); i++) {
    pwm[i].begin();
    pwm[i].setPWMFreq(60);
  }
}

void loop() {
  reply();
}

void reply() {
  if (stringComplete) {

    if (feedback) Serial.println(inputString);

    // clear the string:
    inputString = "";
    stringComplete = false;
    for ( int i = 0; i < sizeof(buf);  ++i ) buf[i] = (char)0;
  }
}

void serialEvent() {
  static byte ndx = 0;
  static int s = 0;

  while (Serial.available()) {

    char rc = (char)Serial.read();
    inputString += rc;

    //(2) setting pointer parameter
    if ( rc == ',') {
      setPointer(s);
      s++;
      for ( int i = 0; i < sizeof(buf);  ++i ) buf[i] = (char)0;
      ndx = 0;
    }

    //(3) end of this pointer instruction
    else if (rc == ';') {
      setPointer(s);
      //executePointer(); //plan B
      ndx = 0;
      s = 0;
      for ( int i = 0; i < sizeof(buf);  ++i ) buf[i] = (char)0;
    }

    //(4) end of command line
    else if (rc == '\n') {
      //p = 0;
      s = 0;
      stringComplete = true;
    }

    //(1) buffering
    else {
      buf[ndx] = rc;
      ndx++;
    }

  }
}

void setPointer(int s) {
  //index//light//servo1//servo2

  int value;
  value = atoi(buf);

  //index
  if (s == 0) {
    if (feedback) {
      Serial.print("index:");
      Serial.print(value);
      Serial.print(", buf:");
      Serial.println(buf);
    }
    currentPointer[0] = value;
  }

  //light
  else  if (s == 1) {
    int index = currentPointer[0];
    currentPointer[s] = value;
    //Serial.println(index);
    digitalWrite(lightPin[index], (value > 0) ? HIGH : LOW);
    //    analogWrite( lightPin[currentPointer[0]], currentPointer[1]);  // implement later
    if (feedback) {
      Serial.print("light: ");
      Serial.println(value);
    }

    //servos
  } else {

    int index = currentPointer[0];

    if (feedback) {
      Serial.print("servo ");
      Serial.print(index * 2 + s - 2);
      Serial.print(": ");
      Serial.println(value);
    }
    
    uint16_t pulselen =  map(value, 0, 180, SERVOMIN, SERVOMAX);
    currentPointer[s] = pulselen;
    //execute this servo instruction as soon as possible
    pwm[0].setPWM(index * 2 + (s - 2), 0, pulselen); //current pointer id * 2 + s (s is 2 or 3)
    //delay(20); //not sure about it
  }
}


// this was plan B to execute servo movements in pairs - not using 
void executePointer() {
  int index = currentPointer[0];
  analogWrite( lightPin[index], currentPointer[1]);
  pwm[0].setPWM(index * 2, 0, currentPointer[2]);
  pwm[0].setPWM(index * 2 + 1, 0, currentPointer[3]);
  delay(20);
}

Thanks!

You need to adjust SERVOMIN and SERVOMAX to get the correct throw range for each servo.

You need to adjust SERVOMIN and SERVOMAX to get the correct throw range for each servo.

I see, so maybe it is ok if set them in an array or something, right?

Bontempos:
I see, so maybe it is ok if set them in an array or something, right?

It's your project. You can do it anyway you want.

It's your project. You can do it anyway you want.

...

Bontempos:
I am running to some issues about accuracy, for example, when moving a couple of motors do draw a diagonal line, because of the delay between each servo, each motor will move in different timing resulting in incorrect drawings.

Very interesting project.. For quriosity : where do the input data come from ??

RC-servos will run at almost constant speed when moving from one position to another. Therefore diagonals may distort if they are not 45 degrees on the basic axes.
One solution is to insert a number of interpolated points. This solution will be visible when zooming into the diagonal line drawn.

Thanks for your reply!

where do the input data come from ??

I update the inputs using a processing application.

My questions are about the way loops are happening, I can keep my trial and error, but I would like to understand a little more about the baud rate to be used in this kind of scenario, where i can or should not put delays when reading serial data, etc.

One solution is to insert a number of interpolated points

I understand the solution regarding interpolation. So probably I should constrain the motor movements to shorter distances and more frequent updates