Hello. I am having troubles with using a sine wave equation to drive my servo motor. There are three major terms in this equation: Amplitude, frequency and phase difference. However, all but frequency terms work well when I am changing them continuously. When change frequency my servo starts jerking for a bit and then comes back to its normal oscillating movement. I have tried many options such as making frequency fractional and changing by very small amounts, also I tried introducing frequency only when it is top or bottom of the sine wave, however, nothing helped me. Please comment if you know any other ways to make my frequency transition smooth. Here is my simplified code to make it going:
#include <Servo.h>
Servo servo_1;
int frequency = 0;
int Ampl = 180;
int pos1 = 90;
void setup()
{
servo_1.attach(9, 800, 2200);
}
void loop()
{
float value = millis()/(V*10000.0);
pos1 = 90.0-(Ampl/2)*sin(2*PI*frequency*value-((Pd*PI)/180));
servo_1.write(pos1);
}
Sorry for the error in my simplify code. Here it is corrected:
#include <Servo.h>
Servo servo_1;
int frequency = 0;
int Ampl = 180; //amplitude of oscillation
int pos1 = 90; //angular position for the servo
PD = 0; //phase difference
int V = 1;
void setup()
{
servo_1.attach(9, 800, 2200);
}
void loop()
{
float value = millis()/(V*10000.0);
pos1 = 90.0-(Ampl/2)*sin(2*PI*frequency*value-((Pd*PI)/180));
servo_1.write(pos1);
}
Your debugging will go much better if you print out the key values so you can see when they leave the tracks and deduce what is driving it.
It looks like there is an awful lot of integer arithmetic going on inside that big expression calculating pos1. You might find it works better if you make all the constants and variables participating in that calculation floating point. Put ".0" after the integer-looking constants, at least.
sketch_apr17a:6: error: expected constructor, destructor, or type conversion before '=' token
sketch_apr17a.ino: In function 'void loop()':
sketch_apr17a:16: error: 'Pd' was not declared in this scope
The problem is that when you change the frequency the functions become out of phase, even though your phase shift is the same that doesn't mean they will be the same (when I've got issues like this I like to quickly put my function into excel and see what it actually looks like when I introduce different frequencies). Trying set the frequency at the top/bottom of the wave is the right kind of idea, but the wave you are changing to will not be the maximum so you won't get the smooth transition.
What you need to do is work out the phase difference between the two waves when you change frequency and offset the new wave by this. Another method is to interpolate between the two waves, e.g:
const int I=100;
int oldFrequency=0;
int frequency=50;
int i=0;
void setFrequency(int freq){
oldFrequency=frequency;
frequency=freq;
i=I;
}
void loop(){
float v = 2*PI*millis()/(V*10000.0);
if(i>0){
pos1 = 90.0-(Ampl/2)*(i*sin(oldFrequency*v-phase)+(I-i)*sin(frequency*v-phase))/I;
i--;
}
else{
pos1 = 90.0-(Ampl/2)*sin(frequency*v-phase));
}
servo_1.write(pos1);
}
Be aware that if you change the frequency during the interpolation then it won't work, but you can tweak I to adjust the rate that it interpolates between the frequencies.
This is about the least efficient way to generate a sine wave, and you have the problem of discontinuities on
changing frequency.
I'd highly recommend going over to a technique called "direct digital synthesis", where the key variable is
phase. You don't calculate the phase from the frequency directly, you calculate the change in phase
from the time difference and frequency - thus if the frequency changes suddenly the phase is continuous,
which is what you need to keep your servo happy. In pseudo-code:
long time_diff = millis () - prev_time ;
prev_time += time_diff ;
phase += frequency * time_diff ;
value = amplitude * sine_table [phase >> shift] ;
Note that a look-up table using the top bits of the phase variable provides a value in a few machine instructions,
rather than the 10,000's needed by using floating point and sin(). You will need to scale the frequency values
according to the size of the phase variable and the fact that time units are milliseconds (or whatever you choose).
You can apply a change to the phase, frequency or amplitude variables between running this code and the expected
thing will happen. You could run this kind of code in a timer interrupt routine to automate it, then you would need
to use volatile variables and inhibit interrupts around any such changes.
This technique is so powerful that its used in special purpose chips to generate RF signals upto 100's MHz such as the
AD9951. You can apply amplitude, phase and frequency modulation directly in the digital domain.
MarkT, thank you for your reply! It appears to be truly effective way of managing my task, however, I have no idea how to integrate your advice into my code. Could you please give me an example of using direct digital synthesis in my code? I would greatly appreciate any more help you can offer.
So far my frequency range will vary from 0 up to 1.5 Hz, in increments of 0.1 Hz. I am running a robot using these sine waves and continuous update is desired, however, twice a cycle will do. Frequency controls speed of movement of my robot and ideally I want to vary it pretty quickly but smoothly within the desired range. Also, using this method would I still be able to control Amplitude and Phase difference? Thank you very much for your time!
This is not DDS, but this is so far what I have done. It simply calculates the phase shift of the new sine wave compare to the old sine wave and puts it in. However, this is still making the servo jump sometimes, but sometimes transition goes smoothly. I am not sure why it makes the servo jump (some kind of discontinuity perhaps), but on excel it works perfectly fine. Could you please tell me what might be the reason for that, or how I could fit DDS in here? Thank you very much in advance. Note that in this simplified code I did not write the bit where I change frequency from the serial while running the code.
#include <Servo.h>
Servo servo_1;
int Ampl = 180;
int V = 1;
int frequency = 0;
int frequency_old = 0;
double PD_transition_old = 0.0;
double PD_transition = 0.0;
int pos1 = 90;
void setup()
{
servo_1.attach(9, 800, 2200);
}
void loop()
{
float value = millis()/(V*10000.0);
if(frequency != frequency_old)
{
PD_transition = 2*PI*value*(frequency-frequency_old)-PD_transition_old;
}
pos1 = 90.0+(Ampl/2)*sin(2*PI*frequency*value-PD_transition);
pos1 = constrain(pos1, 0, 180);
servo_1.write(pos1);
frequency_old = frequency;
PD_transition_old = PD_transition;
}
dvl12:
So far my frequency range will vary from 0 up to 1.5 Hz, in increments of 0.1 Hz. I am running a robot using these sine waves and continuous update is desired, however, twice a cycle will do. Frequency controls speed of movement of my robot and ideally I want to vary it pretty quickly but smoothly within the desired range. Also, using this method would I still be able to control Amplitude and Phase difference? Thank you very much for your time!
twice a cycle? how long i a cycle? Also how much precision are we talking for this servo, 8 bits enough?
Length of the cycle depends on frequency and is found by 1/f. The maximum amplitude is 180, so I guess 8 bits will provide resolution of up to 1 degree out of those 180. May be I am misunderstanding your question, please tell me if so.