Animating multiple servos with millis

patrickstefanski · July 21, 2017, 2:01am

Hi, I was following along in this thread: http://forum.arduino.cc/index.php?topic=310054.45

I ran into some issues and thought I would make a new topic.

I’m trying to do some keyframe animation of servo movements for a robotic arm.

This is is the exact animation I’m hoping to achieve:

This is done in 3D Studio Max and I’m just translating the keyframes to milliseconds and for hte most part everything is working.

I have two servos, SHOULDER and SPIN. SHOULDER does a lifting rotation as though raising your arm from the shoulder, and the spin servo spins the arm. Both servos are HS-645MG from Hi-Tec

Here is my code:

#include <Servo.h>

Servo SPIN;
Servo SHOULDER;

int Val;
int SPINdefault = 99;
int SHOULDERdefault = 197;



struct ServoAction
{
  unsigned long ActionTime;  // Number of milliseconds after sequence starts to do this action
  Servo ServoToAct;         // Which servo is it?
  bool ActionDone;           // Is action done yet
  int Degrees;               // Number of degrees to send the designated servo to
};

ServoAction Actions[] = {
  {1000, SHOULDER, false, 161}, 
  {2000, SHOULDER, false, 197},
  {3000, SPIN, false, 0},
  {3500, SPIN, false, 99},
  {4000, SPIN, false, 197},
  {4500, SPIN, false, 99},
  {5500, SHOULDER, false, 161}, 
  {5500, SPIN, false, 0},
  {6000, SPIN, false, 99},
  {6500, SPIN, false, 197},
  {7000, SPIN, false, 99},
  {7500, SHOULDER, false, 197}, 
  {7500, SPIN, false, 0},
  {8000, SPIN, false, 99},
  {8500, SPIN, false, 197},
  {9000, SPIN, false, 99},
  /* etc. etc. */
};

const byte NoActions = sizeof Actions / sizeof Actions[0];
unsigned long StartTime;

void setup()
{
  Serial.begin(9600);
  StartTime = millis();
  SPIN.attach(9, 553, 2520);  // attaches the servo on pin 9 to the servo object, the servo min/max pulse range is 553 to 2520 microseconds
  SHOULDER.attach(10, 553, 2520);


  SPIN.write(SPINdefault);
  SHOULDER.write(SHOULDERdefault);
  delay (2000);
}

void loop() 
{
for(int i=0;i<NoActions;i++)
  if(!Actions[i].ActionDone)
    if(millis()-StartTime > Actions[i].ActionTime)
      {
      Actions[i].ActionDone=true;
      Actions[i].ServoToAct.write(Actions[i].Degrees);
      }
}

With that code I can achieve this:

There is one problem I am experiencing and a few things I’d like to add to the functionality if possible.

PROBLEM: If you look in my setup, when I attach the servos I change the pulse-rate range to match the specs of my servo per the manufacturer. Before I did this, I was only getting 0-180 degree rotation instead of 0-197. When I added in that pulse rate range it started working as it should. If you watch the video of the actual part, you can see during the setup function it does go to the proper spot (the arm being totally flat) but when it enters the loop function the arm immediately jumps up to about 180 range as if I never put those values in at all. It also never returns to full flat as it should at the end. Why are those pulse rate changes being ignored in the loop function?

As far as functionality, Is there a way I can add a parameter to ServoAction to slow down the speed of the servo, like add in a ‘delay’ between each step? In the CG animation the arms move slowly and are timed differently, the code seems to just make them move to their next point as fast as possible. I’m not sure how to add in a “speed control”

I plan on adding in the elbow servo which should work similarly to these, but I’m also hoping to control the claw at the end with a solenoid. The solenoid will be controlled by a cirucuit with a transistor. The transistor just needs a high/low signal from a digital pin. How can I add that into the ServoAction animation chain so I can incorporate it with the full animation?

I know it’s a lot that I am asking, but any help you can give would be amazing!
Thanks!

edgemoron · July 21, 2017, 3:48am

If you want 197 degrees, write 197 *= 180 / 197, read through these functions from servo.cpp.

uint8_t Servo::attach(int pin, int min, int max)
{
  if(this->servoIndex < MAX_SERVOS ) {
    pinMode( pin, OUTPUT) ;                                   // set servo pin to output
    servos[this->servoIndex].Pin.nbr = pin;
    // todo min/max check: abs(min - MIN_PULSE_WIDTH) /4 < 128
    this->min  = (MIN_PULSE_WIDTH - min)/4; //resolution of min/max is 4 uS
    this->max  = (MAX_PULSE_WIDTH - max)/4;
    // initialize the timer if it has not already been initialized
    timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex);
    if(isTimerActive(timer) == false)
      initISR(timer);
    servos[this->servoIndex].Pin.isActive = true;  // this must be set after the check for isTimerActive
  }
  return this->servoIndex ;
}

void Servo::write(int value)
{
  if(value < MIN_PULSE_WIDTH)
  {  // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
    if(value < 0) value = 0;
    if(value > 180) value = 180; // <<<<<<<<<<<<<<<<<<<<<<<<
    value = map(value, 0, 180, SERVO_MIN(),  SERVO_MAX());
  }
  this->writeMicroseconds(value);
}

I would do everything in microseconds.
And take a look at this library for speed.
https://playground.arduino.cc/ComponentLib/Servotimetimer1

slipstick · July 21, 2017, 8:48am

To control the speed of movements try using library VarSpeedServo.h instead of plain Servo.h. See https://forum.arduino.cc/index.php?topic=61586.60

Steve

slipstick · July 21, 2017, 1:16pm

Yes you can add another element Speed to your struct. No problem. You just need to remember to initialise it in your Actions array.

Oh and you should change the name of Degrees, it looks silly having an element called Degrees with values like 2520. It's not degrees it's pulse length.

Then you need to finish your changes. You're still using write() everywhere when it should be writeMicroseconds().

Steve

patrickstefanski · July 21, 2017, 1:32pm

Thank you so much edgemoron and slipstick. I implented advice on both of your posts.

I converted everything to microseconds.
I started using VarSpeedServo.h instead of Servo.h

I also updated the max pulse value of VarSpeedServo.h which cleared up the issue of the servo not returning home.

All problems are solved!

#include <VarSpeedServo.h> 

VarSpeedServo SPIN;
VarSpeedServo SHOULDER;
VarSpeedServo ELBOW;
int buttonPin = 0;
int Val;
int SPINdefault = 1536;
int SHOULDERdefault = 2520;
int ELBOWdefault = 0;
int Solenoid = 2;


struct ServoAction
{
  unsigned long ActionTime;  // Number of milliseconds after sequence starts to do this action
  VarSpeedServo ServoToAct;         // Which servo is it?
  bool ActionDone;           // Is action done yet
  int Degrees;               // Number of degrees to send the designated servo to
  int Speed;  //VarSpeedServoSpeed 0-255
  bool Wait; //true to wait until done, false to run in background
};

ServoAction Actions[] = {
  {0, SHOULDER, false, 2520, 255, false}, 
  {2000, SHOULDER, false, 1536, 30, false}, 
  {4000, SHOULDER, false, 2520, 30, false}, 
  {8000, SHOULDER, false, 1536, 90, false}, 
  {10000, SHOULDER, false, 2520, 90, false},
  {14000, SHOULDER, false, 1536, 120, false}, 
  {16000, SHOULDER, false, 2520, 120, false},
  {20000, SHOULDER, false, 1536, 150, false}, 
  {22000, SHOULDER, false, 2520, 150, false},
  {26000, SHOULDER, false, 1536, 255, false}, 
  {28000, SHOULDER, false, 2520, 255, false},

};

const byte NoActions = sizeof Actions / sizeof Actions[0];
unsigned long StartTime;

void setup()
{
  Serial.begin(9600);
  StartTime = millis();
  SPIN.attach(9, 553, 2520);  // attaches the servo on pin 9 to the servo object, the servo min/max pulse range is 553 to 2520 microseconds
  SHOULDER.attach(10, 553, 2520);
  ELBOW.attach(11, 610, 2360);

  SPIN.write(SPINdefault);
  SHOULDER.write(SHOULDERdefault);
  ELBOW.write(ELBOWdefault);
  delay (2000);
}

void loop() 
{
for(int i=0;i<NoActions;i++)
  if(!Actions[i].ActionDone)
    if(millis()-StartTime > Actions[i].ActionTime)
      {
      Actions[i].ActionDone=true;
      Actions[i].ServoToAct.write(Actions[i].Degrees,Actions[i].Speed,Actions[i].Wait);
      }
}

I just have one more bit of functionality to implement and I am not sure how to do it.

The claw you see in my original animation is going to use a solenoid. All I need to trigger the solenoid is to send a HIGH signal to a pin and be able to control the length of time it does that.

Ideally, I can make it a paramter in my servo action struct so I can just keep animating. Alternatively I can have a separate struct for it but I’d rather not do that if there is a way around it, any ideas?

patrickstefanski · July 21, 2017, 1:55pm

slipstick, I updated the Degrees and ‘write’

#include <VarSpeedServo.h> 

VarSpeedServo SPIN;
VarSpeedServo SHOULDER;
VarSpeedServo ELBOW;
int buttonPin = 0;
int Val;
int SPINdefault = 1536;
int SHOULDERdefault = 2520;
int ELBOWdefault = 0;
int Solenoid = 2;


struct ServoAction
{
  unsigned long ActionTime;  // Number of milliseconds after sequence starts to do this action
  VarSpeedServo ServoToAct;         // Which servo is it?
  bool ActionDone;           // Is action done yet
  int Pulse;               // Number of Pulse to send the designated servo to
  int Speed;  //VarSpeedServoSpeed 0-255
  bool Wait; //true to wait until done, false to run in background
};

ServoAction Actions[] = {
  {0, SHOULDER, false, 2520, 255, false}, 
  {2000, SHOULDER, false, 1536, 30, false}, 
  {4000, SHOULDER, false, 2520, 30, false}, 
  {8000, SHOULDER, false, 1536, 90, false}, 
  {10000, SHOULDER, false, 2520, 90, false},
  {14000, SHOULDER, false, 1536, 120, false}, 
  {16000, SHOULDER, false, 2520, 120, false},
  {20000, SHOULDER, false, 1536, 150, false}, 
  {22000, SHOULDER, false, 2520, 150, false},
  {26000, SHOULDER, false, 1536, 255, false}, 
  {28000, SHOULDER, false, 2520, 255, false},

};

const byte NoActions = sizeof Actions / sizeof Actions[0];
unsigned long StartTime;

void setup()
{
  Serial.begin(9600);
  StartTime = millis();
  SPIN.attach(9, 553, 2520);  // attaches the servo on pin 9 to the servo object, the servo min/max pulse range is 553 to 2520 microseconds
  SHOULDER.attach(10, 553, 2520);
  ELBOW.attach(11, 610, 2360);

  SPIN.write.Microseconds(SPINdefault);
  SHOULDER.write.Microseconds(SHOULDERdefault);
  ELBOW.write.Microseconds(ELBOWdefault);
  delay (2000);
}

void loop() 
{
for(int i=0;i<NoActions;i++)
  if(!Actions[i].ActionDone)
    if(millis()-StartTime > Actions[i].ActionTime)
      {
      Actions[i].ActionDone=true;
      Actions[i].ServoToAct.write(Actions[i].Pulse,Actions[i].Speed,Actions[i].Wait);
      }
}

system · May 5, 2021, 10:38pm