Hello All, I am trying to write a relatively simple code for servo easing (it does not have to be smooth). The goal is to start servo motion slowly and ramp up servo speed to mid travel point, and then decay the speed at inverse rate of ramp up until end point reached. Peak ramp up will always be in the middle of the travel, as is the beginning of the decay. I have read through servoEasing.h (https://docs.arduino.cc/libraries/servoeasing/) , but it is much more than I need. I have read through many posts and written many changes into my code, and I can't figure out how to decay the quadratic equation. it works just fine 0-180 (positive integers) as written, or 0-90 with adjusments. But it does not work when I try 180 to 0 or 90 to 0. Below is the base code I am working with. Any suggesions much appreciated!!!
BTW: feedbackPin is because I have modified my servos to give pot info....
#include <Servo.h>
Servo myServo;
const float a = 0.01;
const float b = 0.5;
const float c = 10;
const int feedbackPin = 14;
void setup() {
myServo.attach(9);
Serial.begin(9600);
}
void loop() {
for (int x = 0; x <= 180; x++) {
float y = a * x * x + b * x + c;
int servoPos = constrain(y, 0, 180);
myServo.write(servoPos);
Serial.print("x: ");
Serial.println(x);
Serial.print("servo Pos: ");
Serial.println(servoPos);
delay(10);
}
}
Hi, Thanks for replies!! Maximo...VarSpeedServo is great for speed setting, but I would have to code to get speed changes over time and location which is what I am trying to do with the quad equation.
sherzaad: Thank you! your functions work great! Works fine for travel 0-180, and 180-0. But I am still having the same issue. So for my servo action, the peak speed should be mid travel 0-180 ..at 90, and then decay. And same for 180 back to 0, peak speed should be at 90, and then decay after that. I see how you normalized with the 360 code line. When I try to normalize from 90, I get messy jumps before the decay begins.... I will work on my code a bit and post.
Added: tl;dr: Or don't. "non-linear 4PL" makes alotta hair. Maybe one of the other methods would be better, I'll experminet now that I am legit.
and enter data like I did
0 0
45 15
90 90
135 165
180 180
and use "non-linear 4PL" fitting to get a nice curve equation to use the way @sherzaad used the quadratic in the demonstration.
I'd share my results, which were quickly obtained, except I overstayed my "guest" welcome at the site. I'll sign up later (sigh) and see if I can fit something into @sherzaad' code.
Which worked as you have found to ease the approach to 180.
#include <cmath>
double fourPL(double x, double a, double b, double c, double d) {
return d + (a - d) / (1 + pow(x / c, b));
}
not sure how this is any easier than the quad equation??
I'm still stuck on trying to ramp up servo speed to mid position (90) and then ramp down to zero speed at 180 (end pos), and then alter the functions for similar action in reverse.
I get a nice smooth accel to 90, but it's the decel/decay 90 to 180 that is giving me quite a challenge...
#include <Servo.h>
// Create servo object
Servo myServo;
const float a = (-1.0 / 180);// + /- for parabola direction
const float b = .5;//2.... 0.5 gives a smoother accel/attack curve
const float c = 0;
void setup() {
myServo.attach(9);
Serial.begin(9600);
}
void loop() {
for (int x = 0; x <= 90; x++) {
float y = (-a * x * x) + (b * x) + c;//leading coef - to start off slowly
int servoPos = constrain(y, 0, 90);
myServo.write(servoPos);
Serial.print("x: ");
Serial.println(x);
Serial.print("servo Pos: ");
Serial.println(servoPos);
delay(10);
}
}
IMO, a quadratic is not the correct fit for those requirements. Consider a sinusoid (properly shifted and scaled). Such a curve will have maximum instantaneous slope (i.e. derivative) at the mid point with zero slope at the end points.
It isn't whether one equation is easier than another, it's that one works to do what you want and the other does not, at least not how it is currently coded.
As you move form 0 to 180, feed that number into the function and use the function return as the value to write to the servo.
Thx Gfvalvo!.... I thought it would have been easier to get the peak value from parabola apex, but I have to account for + /- x/y coords. I will look into sinusoid wavform. I was just looking at the servo arm action, and i thought that it's like points on a circle, just modify that circle into a parabola and adjust servoWrite pos accordingly...not so easy after all....
Ok...WOW! Thx 777, that code is fantastic! I just finished reading a bit on sinusoidal control, but this is as smooth as servoEasing.h...impresssive! Thank you
Time to read more on 4Pl!!
and thx Gfvalvo! I found a bit of starter code on this forum:
for (int degrees = 0; degrees < 180; degrees ++)
{
float radians = (degrees * M_PI) / 180.0;
for (int servo = 0; servo < 16; servo++)
{
// Map the sin() function to the servomin..servomax range
int position = (sin(radians) + 1.0) * ((servomax - servomin) / 2.0) + servomin;
pwm.setPWM(servo, 0, position);
//myServo.write(position);
radians += (2.0 * M_PI) / 16.0; // Advance 1/16th of a circle
}
delay(10); // 3600 milliseconds per cycle
}
I wanted to post the rest of this for anyone else. Thx to alto777 and Gfvalvo for directing me to sinusoid math instead of quadratic calcs. So I went back to the suggested 4Pl website above. After scrolling down, I saw the spot to enter your sinusoid x,y data. Then use drop down "fitMethod" to select output equation. I am using non-linear 4PL. click on that and then on the graph, your calculations will be generated. Put them into the fourPl equation in the sketch. Super COOL!! never even knew about this site. So for anyone else looking for a (now easier to understand) method (which can be made non-blocking!!) for realistic servo operations, read this thread!
One thing to keep in mind as you experiment is that the servo signal is only new every 20 milliseconds.
Right now with all the printing you do and
Serial.begin(9600);
using your grandfather's idea of high speed plus the 10 milliseconds additional delay you are writing to the servo at a rate that makes sense.
When/if you remove the printing or move to a 21st century baud rate the delay may need to be adjusted.
In a more elaborate sketch, the servo would be updated periodically with the time between updates fixed neither by how long printing takes nor any use of delay().
I suspect that the servo libraries that do smoothing or easing or whatever they call it operate on such a basis, probably using a time peripheral and interrupt service routines.
BTW, where did you find the function you posted in #6?
lol...grandfather's baud....nice! I'm using teensy 4.0 so I can use much more.
All the serial.prints are just for debugging. all commented out on final code assembly. I wrote a very nice non-blocking function for all 4 servos so the delay()'s will all be gone. but you are RIGHT. nearly all of the servo smoothing easing codes use some interrupt, millis(0, delay or 'while' and most are code cumbersome. And just too much code for my project. I have only 4 servos. three are normal with added feedback , and fourth is a modified standard to 360. I wanted to get a more realistic servo motion for the 3 standard servos.
code from #6 was just a basic fourPL function I found on the net. Did not bookmark it....
just a bit of final condensed code for this thread using different servo sweep angles. You will have to calculate the sine wave variables before implementing them into fourPl() in the code. But, there is some room for sweep angle adjustments within the sine wave zero slope limits, depending on how much attack, decay, or primary slope you want to have. Y variable is used for float to int for servo degree.
#include <Servo.h>
Servo myServo;
void setup() {
myServo.attach(9);
Serial.begin(115200);
}
double fourPL60to130(double x) {
return 136.0903 + (58.83364 - 136.0903) / (1 + pow(x / 96.06687, 8.339759)); //for 60-130 sweep
}
double fourPL0to180(double x) {
return 191.7609 + (3.237289 - 191.7609) / (1 + pow(x / 92.42119, 4.382869)); //for 0-180 sweep
}
void loop() {
for (int x = 60; x <= 130; x++) {
float y = fourPL60to130(x);
int servoPos = y;
myServo.write(servoPos);
Serial.print("x: ");
Serial.println(x);
Serial.print("servo Pos: ");
Serial.println(servoPos);
delay(20);
}
for (int x = 130; x > 60; x--) {
float y = fourPL60to130(x);
int servoPos = y;
myServo.write(servoPos);
Serial.print("x: ");
Serial.println(x);
Serial.print("servo Pos: ");
Serial.println(servoPos);
delay(20);
}
for (int x = 0; x <= 180; x++) {
float y = fourPL0to180(x);
int servoPos = y;
myServo.write(servoPos);
Serial.print("x: ");
Serial.println(x);
Serial.print("servo Pos: ");
Serial.println(servoPos);
delay(10);
}
for (int x = 180; x > 0; x--) {
float y = fourPL0to180(x);
int servoPos = y;
myServo.write(servoPos);
Serial.print("x: ");
Serial.println(x);
Serial.print("servo Pos: ");
Serial.println(servoPos);
delay(10);
}
}
NP. I looked and did not see that mycurvefit.com had any code-ready output. If you hadn't posted the function, I would not have messed with it further, so thanks.
I signed up with them, now I get 100 free of something per month. Otherwise it's real money. I suppose it would be worth it to someone, not me. Cheap, on fixed income, waiting to die.
I had wondered if you'd need to make other 4PL functions. Now it looks like you could use a 60 degree sweep and a 50 degree sweep.
Any swept angle can be played out from any starting position, just add the base angle to an additional angle caculated from the offset to that initial angle.
A curve based on the sin function might be easier to use if you do plan on many ranges of angles. It looks like one general function to scale and shift a sim curve would suffice.
It looks and sounds like you know what you are doing.
I was just looking at my code and thinking along similar lines. I can use the function I posed above in #6, and maybe create a look up table or calculating function for the changed degrees as they would appear on the 'x' 'y' coordinate sinusoidal chart. Then it would be much more portable for different servo ranges. It's just a 180x x 180y box so in theory, using a local calculation to derive the new a,b,c,d is doable....
On the other hand, I only have a few motions that require either 0-180 or the shorter range. the rest are just stepped to position with non blocking at various speeds.
but I do think that the three main goals I had set out for my servo control have been met:
non blocking simple function (not posted)
soft start (not posted..requires servo POT feedback..no EEPROM needed)
today's solution of sinusoidal motion
I'll post a link to project and code on Adafruit when it's all done.
