How to make a servo go to a certain angle, 60 degrees for instance

I've got the following code that turns the servo back and forth, over and over. I got it working, but how can I create a function that sets it to a certain degree... 60 for instance?

My current code:

public static void Main()
{
    PWM servo = new PWM(PWMChannels.PWM_PIN_D5, 100, .5, false);
    servo.Start();

    while (true)
    {
        double startValue, endValue;

        for (startValue = 4.712; startValue < 10.995; startValue = startValue + 0.0005)
        {
            endValue = System.Math.Sin(startValue) * 0.5 + 0.5;
            servo.DutyCycle = endValue;
        }
    }

}

How can I for instance set 60 degrees and later be able to set it to 164?

Solution

You need to run your code, make some measurements of the servo position, then give these measurements back to your program, so it can properly adjust itself. This is known as adjustment and calibration.

You should end up with a function who's input is a desired angle, and who's output is the PWM value you need to command to the servo. Call this function just prior to moving the servo.

Note: The loop in your code is a little strange, so I don't know exactly how your particular servo is controlled. PWM controls work based on their duty cycle, which is the ratio of time they are high compared to low, or 0 to 100%. For this answer, this equates to a PWM value of 0.0 to 1.0. You'll have to integrate the ideas and equations into your code yourself, of course.

To determine your calibration coefficients (which would be gain and offset in the simplest transformation, assuming the servo control is linear. y = mx + b, where gain is m and offset is b.):

Fire it up, and set your PWM to the minimum (let's say that minimum value is 0).
Measure the angle of the servo.
Set the PWM to maximum (let's say that maximum value is 1.0).
Measure the angle of the servo.

I'm making some assumptions and picking numbers arbitrarily, but let's say the measurements are:

-10 degrees at PWM setting of 0 (minimum).
+80 degrees at PWM setting of 1 (maximum).

Let's call these measurements:

x1 = angle1 = -10;
y1 = pwm1 = 0;
x2 = angle2 = 80;
y2 = pwm2 = 1;

We have two values for x, and two values for y, so you can solve the simultaneous equations y1 = x1 * m + b and y2 = x2 * m + b. Since m is gain and b is offset, we end up with the following equation:

y = (1/90) * x + (1/9)

gain = 1/90;
offset = 1/9;
dacValue = gain * angle + offset;

Now, assuming the servo is repeatable, and that the PWM control is linear, when 45 degrees is requested, you'll apply the coefficients using the equation above and get an answer of 0.6111. You command your PWM with this, and your servo should move. If you measure the servo position, it should be at 45 degrees.

Note that the gain and offset will probably be different for every servo in your system, due to variations in the servo, electronic circuitry, mechanical linkages, etc. If any part of the system is modified, you likely will have to recalibrate that servo system.

This gets more complicated if the range isn't linear (the servo moves more near maximum and less at minimum, for example). You'll need to use a quadratic equation, an equation of higher order, or a totally different equation altogether. The choice comes down to how much error you're able to tolerate in the servo position.

If the servo isn't repeatable (e.g. every time to go to a specific PWM value, the servo stops at a different spot), control will be difficult as well.

Also note that this is open loop control, at least of your system. Within the servo, a very similar control loop is present. Instead of angle to PWM, it's PWM to position, where position probably isn't calibrated to some real world angle. That's why you need to take care of that job in your program.