0

I am trying to control a few (8 for now) servo motors using this 16-channel 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. Where to set delays, the baud rate settings for this application, etc. I couldn't find a good example using all channels with minimum delay. In my case, 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.

Thanks in advance for any suggestions.

#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...

//#define L1 4
//#define L2 7
//#define L3 8
//#define L4 10

#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)
*/

//values for 8 servos:
const uint8_t SERVOMIN[]  = {150, 130, 150, 130, 150, 130, 150, 130};
const uint8_t SERVOMAX[]  = {600, 500, 600, 500, 600, 500, 600, 500};

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

uint8_t servonum = 0;
uint8_t activeServos = 4; //not being used now
char buf [4]; //maybe too long
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[index], SERVOMAX[index]);
    currentPointer[s] = pulselen;
    pwm[0].setPWM(index * 2 + (s - 2), 0, pulselen); //current pointer id * 2 + s (s is 2 or 3)
    //delay(20);
  }
}


// this was plan B - 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);
}
Bontempos
  • 77
  • 8
  • Maybe doing less serial prints will help reduce the latency? You have two potential problems 1) to ensure your servos start moving as close together as possible, you have to output the data to them quickly/closely together - this is limited by the I2C bus speed, perhaps you should use auto-increment so you can output all the values to the PWM chip in one long output action? 2) you should make your serial commands batch the commands together and then an explicit command makes the output happen, then you will send to PWM in one hit once all the moves have been received from serial. – DisappointedByUnaccountableMod Mar 30 '17 at 17:50
  • Thanks for your reply! Motors moves in different timing. The application which sends serial commands to arduino uses timers to calculate each motor trajectory and speed to ensure a motor can't receive new instructions while still moving. But maybe it is better to split their trajectory in small fractions and update all servos at the same time with one single command as you suggest. – Bontempos Mar 31 '17 at 02:14

0 Answers0