Want to introduce function into Servo control

 :fearful: 
I want to introduce function into servo control so the rotating speed can change.

If I want to run the servos such that output follows: 
pos = 90 sin (a*t) + 90    //position
where  a is value of your choice a t is time. 

Could anyone give me some advice?

Thx in advance.

pos = 90 sin (a*t) + 90 It looks like you have everything you need. What units will the time be in ? Can you please post an example program that you wish to use this in ?

I like the movement this sort of equation will give a servo. The servo moves quickly through the center section of the servo’s range and slows near the extremes of motion.

I have an aversion to mixing float and integers but apparently the Arduino can handle this without a problem.

I haven’t wired up a servo to test it yet, but below is my attempt of turning the equation into a function.

“TIME_CONSTANT” is a floating point value but I’m not sure if it needs to be.

void setServo(float t)
{
  int pos = 90.0 * sin(TIME_CONSTANT * t) + 90.0;
  myservo.write(pos);
}

I thought it would be good to try a servo sweep with this equation. Here’s what I came up with. The code should be non-blocking. The comment “// do other stuff” shows where the program could be expanded.

While this code is non-blocking, the main program loop should be less than 20ms in order to keep the servo motion smooth.

/*  SineSweep
 *  by Duane Degn
 *  November 5, 2015
*/ 

#include <Servo.h> 

const float TIME_CONSTANT = 2.0; 
const float MAX_TIME = 360.0 / TIME_CONSTANT; 
float timeParameter = 0.0;

unsigned long lastRefresh;
 
Servo myservo;  // create servo object to control a servo 
                // twelve servo objects can be created on most boards

void setup() 
{ 
  myservo.attach(9);  // attaches the servo on pin 9 to the servo object 

  lastRefresh = micros();
} 
 
void loop() 
{ 
  void checkServoTime();

  // do other stuff
  
} 

float checkServoTime()
{
  if (micros() - lastRefresh > REFRESH_INTERVAL)
  {
    lastRefresh += REFRESH_INTERVAL; //REFRESH_INTERVAL defined in servo library as 20000 us
    timeParameter += 1.0;
    
    if (timeParameter > MAX_TIME)
    {
      timeParameter -= MAX_TIME;
    }
    setServo(timeParameter);
  }

}

void setServo(float t)
{
  int pos = 90.0 * sin(TIME_CONSTANT * t) + 90.0;
  myservo.write(pos);
}

I used the one letter variable “t” in the function but I used “TIME_CONSTANT” instead of “a”.

With the TIME_CONSTANT equalling 2.0, and the timeParameter being incremented by one every 20ms, a full sweep period of the servo movement should take 3.6 seconds.

To increase the speed of the sweep, you could change either the value of “TIME_CONSTANT” or you could change the amount “timeParameter” is incremented with reach refresh interval.

I think a better way to code this would be to get rid of the “TIME_CONSTANT” and instead change the amount “timeParameter” is incremented.

I hope people let me know if there a better way to declare variables and I also hope you let me know if I mixed floats and integers appropriately.

I used a whole number for “TIME_CONSTANT” and also incremented “timeParameter” by a whole number but I thought it would be good to use floats for these values in order to allow greater flexibility in the speed of the servo sweep.

I did something similar to this sort of motion using a different microcontroller. If you watch the way the servos move in this video, you can see how there are times the servos slow near the extremes of motion and speed as they pass through the center position.

void checkServoTime();

This is a function prototype, not a call to the function. It should be

checkServoTime();

There could usefully be a "delay" between each write to the servo to allow it to reach its target position. You might also consider reading the pos value from an array of bytes instead of calculating it in the program.

UKHeliBob:
This is a function prototype, not a call to the function. It should be

Thanks for checking the code. I had meant to call the function and I copied and pasted the line from the function itself to make the call. I didn’t clean it up as I should have. (I copied a version without a return value, hence the “void”.)

UKHeliBob:
There could usefully be a “delay” between each write to the servo to allow it to reach its target position.

My intention was to have the code calculate the position at 50Hz. The code does not use a delay but a new position is only calculated once every “REFRESH_INTERVAL” (20ms). The “checkServoTime” function checks to see if it’s time to update the position. “setServo” is only called if the required time interval has passed.

Since the new positions are calculated so frequently, the distance between the old position and the new position is (or should be) very small. The idea is the new position should be reached in the 20ms refresh period.

UKHeliBob:
You might also consider reading the pos value from an array of bytes instead of calculating it in the program.

If one didn’t need to change the motion of the servo then an array seems like a good idea but my intention was to allow the “// do other stuff” part be filled with ways the motion parameters of the servo could be changed. A knob could be read to change the speed etc…

Here’s the code again with the error you found fixed. I think it would be great if you were willing to take another look at it.

/*  SineSweep
 *  by Duane Degn
 *  November 6, 2015
*/ 

#include <Servo.h> 

const float TIME_CONSTANT = 2.0; 
const float MAX_TIME = 360.0 / TIME_CONSTANT; 
float timeParameter = 0.0;

unsigned long lastRefresh;
 
Servo myservo;  // create servo object to control a servo 
                // twelve servo objects can be created on most boards

void setup() 
{ 
  myservo.attach(9);  // attaches the servo on pin 9 to the servo object 

  lastRefresh = micros();
} 
 
void loop() 
{ 
  checkServoTime();

  // do other stuff
  
} 

void checkServoTime()
{
  if (micros() - lastRefresh > REFRESH_INTERVAL)
  {
    lastRefresh += REFRESH_INTERVAL; //REFRESH_INTERVAL defined in servo library as 20000 us
    timeParameter += 1.0;
    
    if (timeParameter > MAX_TIME)
    {
      timeParameter -= MAX_TIME;
    }
    setServo(timeParameter);
  }

}

void setServo(float t)
{
  int pos = 90.0 * sin(TIME_CONSTANT * t) + 90.0;
  myservo.write(pos);
}

I’ll try to find some time later today to hook up a servo and test it.

Thanks again for pointing out the error.

My intention was to have the code calculate the position at 50Hz. The code does not use a delay but a new position is only calculated once every "REFRESH_INTERVAL" (20ms)

Sorry, I missed that.

I dug out a servo and tested the code with my Uno and it did not work as I had hoped. The servo kind convulsed.

The equation to calculate position was the main culprit.

void setServo(float t)
{
  int pos = 90.0 * sin(TIME_CONSTANT * t) + 90.0;
  myservo.write(pos);
}

Since the “write” call takes a parameter in degrees, I was using degrees in the calculation. The trig functions use radians.

I’m sure there’s a better way to write this, but here’s my first attempt to convert the angles to radians expected by the sine function.

void setServo(float t)
{
  int pos = 90.0 * sin(PI * (TIME_CONSTANT * t) / 180.0) + 90.0;
  myservo.write(pos);
}

Here’s the complete code.

/*  SineSweep
 *  by Duane Degn
 *  November 7, 2015
*/ 

#include <Servo.h> 

const float TIME_CONSTANT = 3.6; 
const float MAX_TIME = 360.0 / TIME_CONSTANT; 
float timeParameter = 0.0;

unsigned long lastRefresh;
 
Servo myservo;  // create servo object to control a servo 
                // twelve servo objects can be created on most boards

void setup() 
{ 
  myservo.attach(9);  // attaches the servo on pin 9 to the servo object 
  lastRefresh = micros();
} 
 
void loop() 
{ 
  checkServoTime();
  //delay(500);
  // do other stuff
  
} 

void checkServoTime()
{
  if (micros() - lastRefresh > REFRESH_INTERVAL)
  {
    lastRefresh += REFRESH_INTERVAL; //REFRESH_INTERVAL defined in servo library as 20000 us
    timeParameter += 1.0;
  
    if (timeParameter > MAX_TIME)
    {
      timeParameter -= MAX_TIME;
    }
    setServo(timeParameter);
  }

}

void setServo(float t)
{
  int pos = 90.0 * sin(PI * (TIME_CONSTANT * t) / 180.0) + 90.0;
  myservo.write(pos);
}

In order to give the sweep a bit more speed, I change the “TIME_CONSTANT” to 3.6.

const float TIME_CONSTANT = 3.6;

This gives a nice 0.5Hz frequency. It takes the servo horn one second to travel from one extreme to the other.

As hoped the movement has a nice sinusoidal look to it.