NETMF Servo classes (PWM and Software versions)

Just posting some servo classes.  One for hardware PWM control and one for software PWM using OutputCompare. Never know when you may need.

using System;
using GHIElectronics.NETMF.FEZ;
using GHIElectronics.NETMF.Hardware;

namespace MicroFezRobot
{
    public class ServoHardPWM : IServo
    {
        readonly PWM pwm;
        readonly uint periodUs;     // Number of us in period.
        readonly uint periodNs;     // Number of ns in period.
        readonly uint lowDegree;    // Low degree of servo; 0 default.
        readonly uint highDegree;   // High degree of servo; 180 default.
        uint lowUs = 690;           // microsecond (us) range. Find your specific values with servo range testing.
        uint highUs = 2422;         // microsecond (us) range.
        bool invert;                // Invert/reverse rotation.
        DateTime lastChanged;

        public ServoHardPWM(FEZ_Pin.PWM pin, uint periodUs=20000, uint lowUs=1000, uint highUs=2000, uint lowDegree=0, uint highDegree=180)
        {
            this.periodUs = periodUs;
            this.periodNs = this.periodUs * 1000;
            this.lowUs = lowUs;
            this.highUs = highUs;
            this.lowDegree = lowDegree;
            this.highDegree = highDegree;
            this.pwm = new PWM((PWM.Pin)pin);
            pwm.Set(false);
            this.lastChanged = DateTime.Now;
        }

        /// <summary>
        /// Gets or sets value to invert direction.
        /// </summary>
        public bool Invert
        {
            get { return invert; }
            set { this.invert = value; }
        }

        /// <summary>
        /// Gets number of microseconds (us) in the period.
        /// </summary>
        public uint Period
        {
            get { return periodUs / 1000; }
        }
        
        /// <summary>
        /// Gets or sets number of microseconds (us) in low servo range.
        /// </summary>
        public uint LowRange
        {
            get { return this.lowUs; }
            set { this.lowUs = value; }
        }

        /// <summary>
        /// Gets or sets number of microseconds (us) in high servo range.
        /// </summary>
        public uint HighRange
        {
            get { return this.highUs; }
            set { this.highUs = value; }
        }

        /// <summary>
        /// Gets datetime position was last changed.
        /// </summary>
        public DateTime LastChanged
        {
            get { return this.lastChanged; }
        }

        /// <summary>
        /// Sets current servo angle degree.
        /// </summary>
        /// <param name="degree">Degree to set. (i.e. 0-180)</param>
        public void SetDegree(uint degree)
        {
            if (degree < lowDegree || degree > highDegree)
                throw new ArgumentOutOfRangeException("angleDegree");

            uint posUs = ScaleRange(degree, lowDegree, highDegree, lowUs, highUs);
            SetPosition(posUs);
        }

        /// <summary>
        /// Sets current servo position.
        /// </summary>
        /// <param name="positionUs">High time in microseconds.</param>
        public void SetPosition(uint positionUs)
        {
            if (invert)
                positionUs = (highUs - positionUs) + lowUs;

            uint posNs = positionUs * 1000; // convert us to ns as SetPulse uses ns scale.
            pwm.SetPulse(periodNs, posNs);
            this.lastChanged = DateTime.Now;
        }

        /// <summary>
        /// Stops holding servo at current position by setting PWM low.
        /// </summary>
        public void Stop()
        {
            pwm.Set(false);
        }

        /// <summary>
        /// Dispose servo instance.
        /// </summary>
        public void Dispose()
        {
            // Try to stop servo clean on a pulse low.
            Stop();
            pwm.Dispose();
        }

        private static uint ScaleRange(uint oldValue, uint oldMin, uint oldMax, uint newMin, uint newMax)
        {
            return ((oldValue - oldMin) * (newMax - newMin) / (oldMax - oldMin)) + newMin;
        }
    }
}

using System;
using GHIElectronics.NETMF.FEZ;
using GHIElectronics.NETMF.Hardware;
using Microsoft.SPOT.Hardware;

namespace MicroFezRobot
{
    interface IServo : IDisposable
    {
        bool Invert { get; set; }
        uint Period { get; }
        uint LowRange { get; set; }
        uint HighRange { get; set; }
        DateTime LastChanged { get; }
        void SetDegree(uint angleDegree);
        void SetPosition(uint position);
        void Stop();
    }

    public class ServoSoftPWM : IServo
    {
        readonly OutputCompare oc;
        readonly uint periodUs;     // Normally 20,000us (i.e. 20ms)
        readonly uint lowDegree;    // Low degree of servo; 0 default.
        readonly uint highDegree;   // High degree of servo; 180 default.
        readonly uint[] timings;    // Used for setting OC timings.
        uint lowUs;                 // Small servo (e.g. 690)
        uint highUs;                // Small servo (e.g. 2422)
        bool invert;                // Invert servo direction control.
        DateTime lastChanged;

        public ServoSoftPWM(FEZ_Pin.Digital pin, uint periodUs=20000, uint lowUs=1000, uint highUs=2000, uint lowDegree=0, uint highDegree=180)
        {
            this.oc = new OutputCompare((Cpu.Pin)pin, true, 2);
            this.timings = new uint[2];
            this.periodUs = periodUs;
            this.lowUs = lowUs;
            this.highUs = highUs;
            this.lowDegree = lowDegree;
            this.highDegree = highDegree;
            this.lastChanged = DateTime.Now;
        }

        /// <summary>
        /// Gets or sets a value to invert the servo direction.
        /// </summary>
        public bool Invert
        {
            get { return this.invert; }
            set { this.invert = value; }
        }

        /// <summary>
        /// Gets the period in microseconds.
        /// </summary>
        public uint Period
        {
            get { return this.periodUs; }
        }

        /// <summary>
        /// Gets or sets the low servo range in microseconds.
        /// </summary>
        public uint LowRange
        {
            get { return this.lowUs; }
            set { this.lowUs = value; }
        }

        /// <summary>
        /// Gets or sets the high servo range in microseconds.
        /// </summary>
        public uint HighRange
        {
            get { return this.highUs; }
            set { this.highUs = value; }
        }

        /// <summary>
        /// Gets datetime of last position change.
        /// </summary>
        public DateTime LastChanged
        {
            get { return this.lastChanged; }
        }

        /// <summary>
        /// Sets the current servo angle in degrees.
        /// </summary>
        /// <param name="degree">Degree angle to set.</param>
        public void SetDegree(uint degree)
        {
            if (degree < lowDegree || degree > highDegree)
                throw new ArgumentOutOfRangeException("angleDegree");

            uint highTime = ScaleRange(degree, lowDegree, highDegree, lowUs, highUs);
            SetPosition(highTime);
        }

        /// <summary>
        /// Sets the current position of the servo.
        /// </summary>
        /// <param name="positionUs">Position in microseconds</param>
        public void SetPosition(uint positionUs)
        {
            if (positionUs < lowUs || positionUs > highUs)
                throw new ArgumentOutOfRangeException("positionUs");

            if (invert)
                positionUs = (highUs - positionUs) + lowUs;

            // OutputCompare uses microsecond (us) scale.
            timings[0] = positionUs;
            timings[1] = periodUs - positionUs;
            oc.Set(true, timings, 0, 2, true);
            this.lastChanged = DateTime.Now;
        }

        /// <summary>
        /// Stops holding servo at current position by setting PWM low.
        /// </summary>
        public void Stop()
        {
            oc.Set(false);
        }

        /// <summary>
        /// Dispose servo instance.
        /// </summary>
        public void Dispose()
        {
            Stop();
            oc.Dispose();
        }

        private static uint ScaleRange(uint value, uint oldMin, uint oldMax, uint newMin, uint newMax)
        {
            uint v = ((value - oldMin) * (newMax - newMin) / (oldMax - oldMin)) + newMin; // same good.
            return v;
        }
    }
}

-William

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