NETMF electronic speed controller (ESC)

Here is a basic speed controller if you want to replace your RC receiver with your .Net Micro Framework  controller.  I don’t have working ESC to test with currently, so this may need some tweaking.  Please let me know if see a change needed.

using System;
using System.Runtime.CompilerServices;
using System.Threading;
using GHIElectronics.NETMF.Hardware;
using Microsoft.SPOT;

namespace MicroFezRobot
{
    // Common ESC Interface.
    interface IESC
    {
        int Speed { get; set; }
    }

    /// <summary>
    /// Electronic Speed Controller (ESC) driver.
    /// </summary>
    public partial class SpeedController : IESC, IDisposable
    {
        PWM pwm;
        byte rampDelay;
        int currentSpeed;
        byte armPeriod;
        int minSpeed;
        int maxSpeed;
        const int Hz = 20 * 1000 * 1000; // 20ms as nanoseconds.
        const int MIN = 1 * 1000 * 1000;
        const int MAX = 2 * 1000 * 1000;

        /// <summary> 
        /// Create a SpeedController.
        /// </summary>
        /// <param name="pin">The speed pin on ESC.</param> 
        public SpeedController(PWM.Pin pin, byte rampDelay=1, byte armPeriod=1, int minSpeed=MIN, int maxSpeed=MAX)
        {
            if (minSpeed < 0 || (maxSpeed < minSpeed))
                throw new ArgumentOutOfRangeException("maxSpeed");
            this.rampDelay = rampDelay;
            this.armPeriod = armPeriod;
            this.minSpeed = minSpeed;
            this.maxSpeed = maxSpeed;
            pwm = new PWM(pin);
            
            // Set speed to neutral and wait armPeriod to arm/start ESC. ESC specific.
            SetSpeed(0);
            Thread.Sleep(armPeriod);
        }

        /// <summary>
        /// Gets or sets the controller speed. Valid values of -100 to 100 inclusive.
        /// </summary>
        public int Speed
        {
            get { return currentSpeed; }
            set { SetSpeed(value); }
        }

        [MethodImpl(MethodImplOptions.Synchronized)]
        private void SetSpeed(int value)
        {
            Debug.Print("\nSetSpeed: " + value + " CurrentSpeed:" + currentSpeed );
            if (value > 100)
                value = 100;
            if (value < -100)
                value = -100;

            // Gradually ramp up/down speed until we match given value.
            int rampDir = (value > currentSpeed) ? 1 : -1;
            
            while (value != currentSpeed)
            {
                uint newSpeed = (uint)ScaleRange(currentSpeed, -100, 100, this.minSpeed, this.maxSpeed);
                pwm.SetPulse(Hz, newSpeed);
                currentSpeed = currentSpeed + rampDir;
                Debug.Print("New Speed:" + currentSpeed);
                Thread.Sleep(rampDelay);
            }
        }

        [MethodImpl(MethodImplOptions.Synchronized)]
        public void Dispose()
        {
            Stop();
            pwm.Dispose();
        }

        /// <summary>
        /// Stop ESC. Check datasheet.
        /// </summary> 
        private void Stop()
        {
            SetSpeed(0);
            pwm.Set(false);
        }

        /// <summary>
        /// Scale a number within a source range to a new range and return new scaled value.
        /// </summary>
        private static int ScaleRange(int oldValue, int oldMin, int oldMax, int newMin, int newMax)
        {
            return ((oldValue - oldMin) * (newMax - newMin) / (oldMax - oldMin)) + newMin;
        }
    }
}
Advertisements
This entry was posted in .Net Micro Framework, C#, FEZ, Motors, Robotics and tagged , , , , . Bookmark the permalink.