Simple servo control from FEZ

If you just need the easiest possible way to hook up a servo to FEZ, it does not get easier then this.  No breadboards, mosfets, or resistors.  Just hook the servo wires into the fez and control.  If your servo has a normal 3 pin female connector, then just get some male-to-male jumper wires and your done.  Use a PWM pin to control the servo’s signal wire.  The 5v on the FEZ should be enough for small to medium servos.  If you need more, then power from external supply.

const uint PERIOD = 20 * 1000 * 1000; // 20ms is 20000000ms(nanoseconds)
const uint LowUs = 690;   // min range of servo as microseconds.
const uint HighUs = 2422; // max range of servo as microseconds.

/// <summary>
/// Set servo position.
/// </summary>
/// <remarks>
/// The standard range for 180degree servos is 1 to 2 milliseconds high in a 20ms period, with
/// neutral in the middle at 1.5ms. Exact range varies between servos. Find exact range with
/// a servo tester or PWM range testing.
/// </remarks>
/// <param name="servo">PWM pin of servo control.</param>
/// <param name="highUs">High time in microseconds (e.g. 1500us is 1.5ms or neutral)</param>
public static void SetSimpleServo(PWM servo, uint highUs)
{
    Debug.Print("Set servo nanoseconds: " + highUs);
    servo.SetPulse(PERIOD, highUs * 1000);
}

public static void SetSimpleServoByDegree(PWM servo, byte degree, bool invert)
{
    if (degree < 0 || degree > 180)
        throw new ArgumentOutOfRangeException("angleDegree");

    if (invert)
        degree = (byte)(180 - degree);
            
    // Scale degree (0-180) to servo position. Mult 1K to get to nanoseconds.
    uint pos = ScaleRange((uint)degree, 0, 180, LowUs, HighUs) * 1000;
    servo.SetPulse(PERIOD, pos);

    Debug.Print("Degree:" + degree + " Position ms:" + pos / (double)1000000);
}
        
private static uint ScaleRange(uint oldValue, uint oldMin, uint oldMax, uint newMin, uint newMax)
{
    return ((oldValue - oldMin) * (newMax - newMin) / (oldMax - oldMin)) + newMin;
}

-William

Advertisements
This entry was posted in .Net Micro Framework, C#, FEZ, Robotics, Servo and tagged , , , , . Bookmark the permalink.