Multiple Servo Delays

[mar]

Hello!!

I am trying to control 4 servos at the same time. The program I wrote is pretty basic but I can't seem to get my head around the results I'm seeing.

I'm just trying to input a change in position (in microseconds) in the serial monitor and have all four servos move to their new position at the same time.

I was initially using no delays between servo.writes but they were all out of order and really funky looking. I started adding small delays and eventually settled on a 15ms delay between writes. This eliminated the lag between the servo position changes!

I don't understand why this is happening as I've never had this problem before.

For reference, I am using these servos https://flightcomp.com/products/bms-207wv-mini-digital-high-torque-high-speed-coreless and I am running them at 7.4 volts 1A max. I am using the teensy 3.2.

Any help would be appreciated!!

#include <Servo.h>

Servo s1;
Servo s2;
Servo s3;
Servo s4;

int s1Pin = 21;
int s2Pin = 20;
int s3Pin = 23;
int s4Pin = 22;

int s1Start = 1520;
int s2Start = 1400;
int s3Start = 1520;
int s4Start = 1520;

int s1Pos;
int s2Pos;
int s3Pos;
int s4Pos;

int sDelay = 15;

void setup() {

  Serial.begin(115200);

  s1.attach(s1Pin);
  s2.attach(s2Pin);
  s3.attach(s3Pin);
  s4.attach(s4Pin);

  s1.writeMicroseconds(s1Start);   // Move servo to the intermediate position (30 deg) instead of it moving on its own to its neutral position of 1500 us
  s2.writeMicroseconds(s2Start);
  s3.writeMicroseconds(s3Start);
  s4.writeMicroseconds(s4Start);

  delay(1000);

}

void loop() {


  if (Serial.available()) {

    int val = Serial.parseInt();

    s1Pos = s1Start + val;
    s2Pos = s2Start + val;
    s3Pos = s3Start + val;
    s4Pos = s4Start + val;

//    delay(1000);
    s1.writeMicroseconds(s1Pos);
    delay(sDelay);
    s2.writeMicroseconds(s2Pos);
    delay(sDelay);
    s3.writeMicroseconds(s3Pos);
    delay(sDelay);
    s4.writeMicroseconds(s4Pos);
//    delay(1000);

    Serial.println(val);
  }


}

 

[brtaylor]

The specs you link to for that servo mention a frequency of 333 Hz. Most standard servos, and indeed the servo library, are designed to work at 50 Hz. Do you know if that servo can operate at the lower rate? In any case, I really don't like the servo library - it writes commands to the servos in round robin fashion (i.e. writing to one servo, then the next servo, etc), which mimics early RC receivers, but really most of us expect the servos to move simultaneously. I highly recommend using analogWriteFrequency instead since it will start the PWM pulse for all servos simultaneously. Below is some example I wrote to command servos through a sine wave. It's well commented and you should see where I set the frequency to 50 Hz, you may want to try running this example at 50 Hz, to see if your servos can run at the lower rate, and at 333 Hz. Hopefully you can easily adapt it to your needs.

/* Redefine the pin numbers to match the PWM Backpack numbering */
const unsigned int PWM[8] = {20,21,22,23,5,6,10,9};
/* PWM update frequency, 50 Hz */
const unsigned int Freq = 50;
/* PWM update period */
const unsigned int Period_us = 1000000 / Freq;
/* PWM resolution */
const unsigned int PWM_Resolution = 16;
/* Time, ms */
elapsedMillis time_ms;

void setup()
{
  /* Serial for printing the command */
  Serial.begin(115200);
  while (!Serial && time_ms < 5000) {}
  /* setting the analog frequency to 50 Hz and resolution to 16 bit */
  for (unsigned int i = 0; i < sizeof(PWM) / sizeof(unsigned int); ++i) {
    analogWriteFrequency(PWM[i], Freq);
    analogWriteResolution(PWM_Resolution);
  }
}

void loop()
{
  /* 1 Hz sine wave */
  float Cmd = sinf(2.0f * M_PI * time_ms / 1000.0f);
  /* Scale from +/- 1 to a range of 1000 us to 2000 us */
  Cmd = Cmd * 500.0f + 1500.0f;
  /* Command channels */
  for (unsigned int i = 0; i < sizeof(PWM) / sizeof(unsigned int); ++i) {
    analogWrite(PWM[i],Cmd / Period_us * powf(2,PWM_Resolution));
  }
  /* Print command */
  Serial.println(Cmd);
  /* Run at 50 Hz for better printing */
  delay(20);
}

 

[mar]

Thank you so much brtaylor!! I didn't realize the frequency was something I had to consider. I will try it out and do more research.