Servos Acting Up When Altering Speeds [Urgent Please]

Hi everyone,

I’m working on a project where I am controlling the motion of several servos based on the oscillatory function:
x(t) = Asin(ωt + φ), where:
A = Amplitude
ω = 2πf (f = frequency)
t = time
φ = phase

I use the COM window to change the value of the frequency. However, if you just simply change the value instantaneously, the position function, x(t), “jumps” from one sine wave to another and does not smoothly transition. To combat this, I creating a small function that will slowly increase the frequency over a given interval (for example 5000 ms [5 seconds] ). The problem I haven’t been able to fix for the past several weeks is that when I increase (or decrease) the value of the frequency from say 0 Hz to 1 Hz, the frequency changes smoothly, however, the x(t) function which controls the position of the servos seems to be almost spazzing out. What I mean by this is that when I increase the frequency from 0 to 1 Hz over a period of 5 seconds, the position jumps back and forth very rapidly until the interval is over, and, when the interval completes the function runs perfectly at the new frequency. It might be difficult to understand exactly what I’m trying to explain, so I will post the code as well as screenshots of what I am trying to describe.

*For the screenshots below, note that the middle position is set to 1500, so with an amplitude of 400 the min value is 1100 while the max is 1900. (Shown in code below)

Input of ‘f100’ increases the frequency from 0 mHz to 100 mHz. In the image below, take note of how quickly the position values are jumping back and forth from max to min while the frequency is less than 1 Hz, meaning it should jump from min to max and back to min in 1 second while the two highlighted lines show about a tenth of a second apart from each other.

In the screenshot below, the new frequency of just about 100 mHz has been reached. The screenshot shows two highlighted lines of about .25 seconds apart, which means that it should travel 1/4 of the full wavelength which it does. It starts from 1100 (min) and increases to roughly 1500 which is the middle value (x axis), which is 1/4 of the full distance (min to max and back to min).

#include <Servo.h>

Servo left;
Servo right;

float Fo;
float delta_F;
float Ff;
float f;

float amplitude = 400;
float pos1 = 1500;
float pos2 = 1500;

unsigned long time_reset;
float time_interval = 5000;

void setup() {
  Serial.begin(57600);
  left.attach(9);
  right.attach(8);
  delay(1000);
  
}

void loop() {

  if(Serial.available() > 0) {
    
    if(Serial.peek() == 'f') { //input f[amount]
      Serial.read();
      f = Serial.parseInt();
      f /= 100; // /100 so that you can input decimals [i.e input 25 for .25 Hz change]
      changeFreq(f);
      //Fo += f;
      Serial.read();
      
      }
    
    
    }

  pos1 = amplitude*sin(2*PI*Fo*millis() ) + 1500; //1500 for motor positioning
  pos2 = amplitude*sin((2*PI*Fo*millis() ) + 90) + 1500; //90 degrees out of phase
  left.write(pos1);
  right.write(pos2);
  
  Serial.print("Pos1 :");
  Serial.print(pos1);
  Serial.print("  Pos2: ");
  Serial.print(pos2);
  Serial.print("  Frequency [mHz]: ");
  Serial.println(Fo*100,4);
  delay(10);
}

float changeFreq(float delta_F) {
  //Ff = Fo;
  time_reset = millis();
  while(millis() - time_reset < time_interval) {
    Ff = ( ( (millis() - time_reset) / time_interval) * (delta_F)) + Fo; //runs for 5000 ms
    pos1 = amplitude*sin(2*PI*Ff*millis() ) + 1500;
    pos2 = amplitude*sin((2*PI*Ff*millis() ) + 90) + 1500;
    left.write(pos1);
    right.write(pos2);
    
    Serial.print("Pos1 :");
    Serial.print(pos1);
    Serial.print("  Pos2: ");
    Serial.print(pos2);
    Serial.print("  Frequency [mHz]: ");
    Serial.println(Ff*100,4);
    delay(10);
  }
  Fo = Ff;
  
  
  }

Any help would be greatly appreciated. If you have any questions leave them for me below. Also, before I did the 5 second time intervals I used to use while/for loops with a set amount of iterations, which is essentially the same thing just increasing a counter variable until it reaches the condition of the loop which it would then exit.

Hello DrewG

I think it's a problem of units.

Maybe you should try something like that :

pos2 = amplitude*sin(( (2.0*PI*Ff*millis() ) /1000.0) ) + (90.0*(2.0*PI/360.0))) + 1500.0; //90 degrees out of phase with seconds and radians

Regards bidouilleelec

Oh no. It’s another “urgent” post. Someone thinks they’re so important they should skip the line.

How do we pin this to the top of page 2?

From the reference section.

“If doing math with floats, you need to add a decimal point, otherwise it will be treated as an int. See the Floating point constants page for details.”

You have a lot of float variables, are you following that rule with these.

float Fo;
float delta_F;
float Ff;
float f;
float amplitude = 400;
float pos1 = 1500;
float pos2 = 1500;
float time_interval = 5000;

You don't need the decimal point when initializing a float variable like that. The compiler will do the conversion. However:

float Fo = 1000*3000;

would require the decimal point.

float Fo = 1000.*3000;

because the rules of integer multiplication would cause an overflow before the assignment.

Pete

The problem I see with you changing frequency is that the phase of (wt + phi) changes when w changes. If you don't want an abrupt change, you need to change the value of phi such that it matches the phase that would have been computed if the frequency had stayed the same.

Hello DaleScott

DaleScott: The problem I see with you changing frequency is that the phase of (wt + phi) changes when w changes. If you don't want an abrupt change, you need to change the value of phi such that it matches the phase that would have been computed if the frequency had stayed the same.

Phase is phi, isn't it? Why do you think that Phi changes when frequency change?

Regards, bidouilleelec