Controlling Servo Motors with .NET MicroFramework

This post illustrates how you can control Servo Motors from .NET MicroFramework using only simple output pins and C# code. 

I have this idea of making a walking robot, so in order to test the basic servo control from .NET MicroFramework, I have used 2 servo motors and built a leg. Each Servo motor controls a joint in a leg.

In my first test I have used the GHI USBizi development board, but no special hardware has been used, only simple GPIO pins. I wanted to use the built-in PWM feature of the GHI board, but the program locked up when I initialized the PWM feature! I don't know if this is an error in the GHI firmware or what, but instead of investigating this further I made my own Servo control class in C#, which implements the PWM control. 

Update 2015.

The code running in the video, is as follows:

using System;
using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;
using System.Threading;

namespace PFJ.NETMF.Hardware.Motor
{
    public class Servo
    {
        OutputPort servoPort;
        int pulseWidth;

        int max;
        int min;
        int mid;

        object lockitem;

        public Servo(Cpu.Pin Pin)
        {
            servoPort = new OutputPort(Pin, false);

            max = 1450;
            min = 300;
            mid = 575; // (max - min) / 2;
            pulseWidth = mid;

            lockitem = new object();

            Thread ServoThread = new Thread(ServoProcessor);
            ServoThread.Start();
        }

        public void Left()
        {
            lock (lockitem)
            {
                pulseWidth = min;
            }
        }

        public void Middle()
        {
            lock (lockitem)
            {
                pulseWidth = mid;
            }
        }

        public void Right()
        {
            lock (lockitem)
            {
                pulseWidth = max;
            }
        }

        public void Position(double Percent)
        {
            double d = System.Math.Round((Percent / 100D) * (max - min) + min);
            lock (lockitem)
            {
                pulseWidth = Convert.ToInt32(d.ToString());
            }
        }

        public void Wait(int Delay)
        {
            Thread.Sleep(Delay);
        }

        protected virtual void ServoProcessor()
        {
            while (true)
            {
                lock (lockitem)
                {
                    ServoHighPulse();
                    ServoLowPulse();
                }
                Thread.Sleep(2);
            }
        }


        private void ServoHighPulse()
        {
            servoPort.Write(true);
            DelayMicroSec(pulseWidth);
        }

        private void ServoLowPulse()
        {
            servoPort.Write(false);
            DelayMicroSec(max - pulseWidth);
        }

        /// <summary>
        /// Blocks thread for given number of microseconds
        /// </summary>
        /// <param name="microSeconds">Delay in microseconds</param>
        private void DelayMicroSec(int microSeconds)
        {
            DateTime startTime = DateTime.Now;

            int stopTicks = microSeconds * 10;

            TimeSpan divTime = DateTime.Now - startTime;
            while (divTime.Ticks < stopTicks)
            {
                divTime = DateTime.Now - startTime;
            }
        }
    }
}