AdaFruitPWMDriver

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

namespace ReefinDotNet.i2cDevices
{
    class Adafruit_PWMServoDriver
    {
        private MultiI2C _Device;

        private const byte PCS9685_SUBADR1 = 0x2;
        private const byte PCA9685_SUBADR2 = 0x3;
        private const byte PCA9685_SUBADR3 = 0x4;

        private const byte PCA9685_MODE1 = 0x0;
        private const byte PCA9685_PRESCALE = 0xFE;

        private const byte LED0_ON_L = 0x6;
        private const byte LED0_ON_H = 0x7;
        private const byte LED0_OFF_L = 0x8;
        private const byte LED0_OFF_H = 0x9;

        private const byte ALLLED_ON_L = 0xFA;
        private const byte ALLLED_ON_H = 0xFB;
        private const byte ALLLED_OFF_L = 0xFC;
        private const byte ALLLED_OFF_H = 0xFD;

        private int[] on = new int[15];
        private int[] off = new int[15];

        private byte _i2caddr;

        public bool Override;
        public int Step;

        public Adafruit_PWMServoDriver(byte addr = 0x40, int ClockRateKhz = 200, int step=0)
        {
            _i2caddr = addr;
            this._Device = new MultiI2C(new I2CDevice.Configuration(0x40, 200));
            this.Step = step;
            Override = false;
        }

        public void Begin()
        {
            Reset();
        }
        public void Reset()
        {
            write8(PCA9685_MODE1, 0x0);
        }
        public void setPWMFreq(float freq)
        {
            float prescaleval = 25000000;
            prescaleval /= 4096;
            prescaleval /= freq;
            prescaleval -= 1;
            // Debug.Print("Estimated pre-scale: " + prescaleval);
            byte prescale = (byte)System.Math.Floor(prescaleval + 0.5);
            // Debug.Print("Final Pre-scale: " + prescale);
            byte oldmode = read8(PCA9685_MODE1);
            byte newmode = (byte)((oldmode & 0x7F) | 0x10); // sleep
            write8(PCA9685_MODE1, newmode); // go to sleep
            write8(PCA9685_PRESCALE, prescale); // set the prescaler
            write8(PCA9685_MODE1, oldmode);
            Thread.Sleep(5);
            write8(PCA9685_MODE1, (byte)(oldmode | 0x80));

        }
        public void setPWM(byte num, UInt16 on, UInt16 off)
        {
            write8((byte)(LED0_ON_L + 4 * num), (byte)on);
            write8((byte)(LED0_ON_H + 4 * num), (byte)(on >> 8));
            write8((byte)(LED0_OFF_L + 4 * num), (byte)off);
            write8((byte)(LED0_OFF_H + 4 * num), (byte)(off >> 8));
            this.on[num] = on;
            this.off[num] = off;
        }
        public int getPower(byte num)
        {
            return this.off[num];
        }
        public void fadeTo(int ch1, int ch2, int ch3, int ch4, int ch5, int seconds, int offset=0)
        {
            seconds = seconds * 10;
            int step1 = (int)Math.Ceiling((double)(Math.Abs(this.off[0 + offset] - ch1)) / (double)(seconds));
            int step2 = (int)Math.Ceiling((double)(Math.Abs(this.off[1 + offset] - ch2)) / (double)(seconds));
            int step3 = (int)Math.Ceiling((double)(Math.Abs(this.off[2 + offset] - ch3)) / (double)(seconds));
            int step4 = (int)Math.Ceiling((double)(Math.Abs(this.off[3 + offset] - ch4)) / (double)(seconds));
            int step5 = (int)Math.Ceiling((double)(Math.Abs(this.off[4 + offset] - ch5)) / (double)(seconds));
            while (seconds > 1)
            {
                    if (this.off[0 + offset] > ch1)
                    {
                        this.setPWM((byte) (0 + offset), 0, (ushort)(this.off[0+offset] - step1));
                    }
                    else
                    {
                        setPWM((byte)(0 + offset), 0, (ushort)(this.off[0 + offset] + step1));
                    }
                    //ch2
                    if (this.off[1 + offset] > ch2)
                    {
                        this.setPWM((byte)(1 + offset), 0, (ushort)(this.off[1 + offset] - step2));
                    }
                    else
                    {
                        this.setPWM((byte)(1 + offset), 0, (ushort)(this.off[1 + offset] + step2));
                    }
                    //ch3
                    if (this.off[2 + offset] > ch3)
                    {
                        this.setPWM((byte)(2 + offset), 0, (ushort)(this.off[2 + offset] - step3));
                    }
                    else
                    {
                        this.setPWM((byte)(2 + offset), 0, (ushort)(this.off[2 + offset] + step3));
                    }
                    //ch4
                    if (this.off[3 + offset] > ch4)
                    {
                        this.setPWM((byte)(3 + offset), 0, (ushort)(this.off[3 + offset] - step4));
                    }
                    else
                    {
                        this.setPWM((byte)(3 + offset), 0, (ushort)(this.off[3 + offset] + step4));
                    }
                    //ch5
                    if (this.off[4 + offset] > ch5)
                    {
                        this.setPWM((byte)(4 + offset), 0, (ushort)(this.off[4 + offset] - step5));
                    }
                    else
                    {
                        this.setPWM((byte)(4 + offset), 0, (ushort)(this.off[4 + offset] + step5));
                    }
                    Thread.Sleep(100);
                    //Debug.Print(this.off[0 + offset] + " - " + this.off[1 + offset] + " - " + this.off[2 + offset]);
                seconds--;
            }
            setPWM((byte)(0 + offset), 0, (ushort)(ch1));
            setPWM((byte)(1 + offset), 0, (ushort)(ch2));
            setPWM((byte)(2 + offset), 0, (ushort)(ch3));
            setPWM((byte)(3 + offset), 0, (ushort)(ch4));
            setPWM((byte)(4 + offset), 0, (ushort)(ch5));
        }

        private byte read8(byte addr)
        {
            byte[] ReadBuffer = new byte[2];
            this._Device.ReadRegister(addr, ReadBuffer);
            return ReadBuffer[0];
        }
        private void write8(byte addr, byte d)
        {
            this._Device.WriteRegister(addr, d);
        }

    }
}