C:/Documents and Settings/gw/My Documents/Other projects/flightos/Modules/fastservo.h

Go to the documentation of this file.

/*
  Copyright (c) 2010 Les Newell. All rights reserved

  This program is free software: you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation, either version 3 of the License, or
  (at your option) any later version.

  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  GNU General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with this program. If not, see <http://www.gnu.org/licenses/>.
*/



#if FASTSERVO_CHANNELS > 6
#error "too many fast servos"
#endif

//this ugly stuff is here because of the need to put the strings in program memory

#define MAKENAME(n) const char fastServoNames##n[] PROGMEM = "FastServo."#n
#define MAKEARRAY(s) static const char* s[] = {\
    s##1,s##2,s##3,s##4,s##5,s##6};

MAKENAME(1);
MAKENAME(2);
MAKENAME(3);
MAKENAME(4);
MAKENAME(5);
MAKENAME(6);
MAKEARRAY(fastServoNames);

#undef MAKENAME
#define MAKENAME(s,n) const char fastServo##s##n[] PROGMEM = "FastServo."#n"."#s


MAKENAME(Min,1);
MAKENAME(Min,2);
MAKENAME(Min,3);
MAKENAME(Min,4);
MAKENAME(Min,5);
MAKENAME(Min,6);
MAKEARRAY(fastServoMin);

MAKENAME(Max,1);
MAKENAME(Max,2);
MAKENAME(Max,3);
MAKENAME(Max,4);
MAKENAME(Max,5);
MAKENAME(Max,6);
MAKEARRAY(fastServoMax);

MAKENAME(Gain,1);
MAKENAME(Gain,2);
MAKENAME(Gain,3);
MAKENAME(Gain,4);
MAKENAME(Gain,5);
MAKENAME(Gain,6);
MAKEARRAY(fastServoGain);

MAKENAME(Offset,1);
MAKENAME(Offset,2);
MAKENAME(Offset,3);
MAKENAME(Offset,4);
MAKENAME(Offset,5);
MAKENAME(Offset,6);
MAKEARRAY(fastServoOffset);
#undef MAKENAME
#undef MAKEARRAY


#define US2CLKS ((F_CPU / 8)/1000000)

#define sbi(reg,bit) reg |= _BV(bit)
#define cbi(reg,bit) reg &= ~(_BV(bit));

//define the module.
class FastServo : public Module
{
    DECLARE_MODULE(FastServo,Module)


    //Only use this to expose sockets and parameters. Do most of your
    //initialisastion in Init()
    FastServo()
    {
        /*Expose sockets to the outside world
        NOTE: the socket name must be a static string.
        Bad Things will happen if you use a string allocated on the stack
        You can use a pointer to a static string.*/
        AddSocketArray(fastServoNames, m_servoSockets, FASTSERVO_CHANNELS);
        AddSocketArray(fastServoMin, m_mins, FASTSERVO_CHANNELS);
        AddSocketArray(fastServoMax, m_maxs, FASTSERVO_CHANNELS);
        AddSocketArray(fastServoGain, m_gains, FASTSERVO_CHANNELS);
        AddSocketArray(fastServoOffset, m_offsets, FASTSERVO_CHANNELS);


        for (int ct=0; ct< FASTSERVO_CHANNELS; ct++)
        {
            m_mins[ct] = 1000;
            m_maxs[ct] = 2000;
            m_gains[ct] = 500;
            m_offsets[ct] = 1500;
        }
    }

    //A parameter has changed. This happens at bootup if the EEPROM
    //contains valid data and at run-time if the user changes a
    //parameter over the serial link
    virtual void ParamChanged(Socket * param)
    {
    }

    //Initialise the module
    //set up hardware etc
    virtual void Init()
    {
        for (int ct=0; ct< FASTSERVO_CHANNELS; ct++)
        {
            InitPin(fastServoPins[ct],ct);
        }
        //Set how often Loop() should be executed (in milliseconds)
        SetInterval(5);
        SetPriority(20);
    }

    //Here is where you do your work. It is called at the rate defined by SetInterval
    virtual void Loop(const unsigned long& interval)
    {
        for (int ct=0; ct< FASTSERVO_CHANNELS; ct++)
        {

            float socket = m_servoSockets[ct];
            if (socket != m_servoWas[ct])
            {
                m_servoWas[ct] = socket;
                int value = m_servoSockets[ct] * m_gains[ct];
                value += m_offsets[ct];
                if(value > m_maxs[ct])
                {
                    value = m_maxs[ct];
                }else if(value < m_mins[ct])
                {
                    value = m_mins[ct];
                }
                *m_outputs[ct] = value * 2;
            }
        }
    }



private:

    bool InitPin(int pin, int index)
    {
        pinMode(pin, OUTPUT);
        bool found = true;
        byte timer = 0;

        //Only use timers 3 and 4
        //TODO: Use timer 2

    /*
        if (digitalPinToTimer(pin) == TIMER2A) {
            // connect pwm to pin on timer 2, channel A
            sbi(TCCR2A, COM2A1);
            // set pwm duty
            m_outputs[index] = &OCR2A;
        } else if (digitalPinToTimer(pin) == TIMER2B) {
            // connect pwm to pin on timer 2, channel B
            sbi(TCCR2A, COM2B1);
            // set pwm duty
            m_outputs[index] = &OCR2B;

        } else */if (digitalPinToTimer(pin) == TIMER3A) {
            // connect pwm to pin on timer 3, channel A
            sbi(TCCR3A, COM3A1);
            // set pwm duty
            m_outputs[index] = &OCR3A;
            timer = 3;
        } else if (digitalPinToTimer(pin) == TIMER3B) {
            // connect pwm to pin on timer 3, channel B
            sbi(TCCR3A, COM3B1);
            // set pwm duty
            m_outputs[index] = &OCR3B;
            timer = 3;
        } else if (digitalPinToTimer(pin) == TIMER3C) {
            // connect pwm to pin on timer 3, channel C
            sbi(TCCR3A, COM3C1);
            // set pwm duty
            m_outputs[index] = &OCR3C;
            timer = 3;
        } else if (digitalPinToTimer(pin) == TIMER4A) {
            // connect pwm to pin on timer 4, channel A
            sbi(TCCR4A, COM4A1);
            // set pwm duty
            m_outputs[index] = &OCR4A;
            timer = 4;
        } else if (digitalPinToTimer(pin) == TIMER4B) {
            // connect pwm to pin on timer 4, channel B
            sbi(TCCR4A, COM4B1);
            // set pwm duty
            m_outputs[index] = &OCR4B;
            timer = 4;
        } else if (digitalPinToTimer(pin) == TIMER4C) {
            // connect pwm to pin on timer 4, channel C
            sbi(TCCR4A, COM4C1);
            // set pwm duty
            m_outputs[index] = &OCR4C;
            timer = 4;
        }

        switch(timer)
        {
        case 3:
            TCCR3B = _BV(WGM33)|_BV(WGM32)|_BV(CS31); //clk/8, fast pwm
            sbi(TCCR3A,WGM31);
            cbi(TCCR3A,WGM30);
            ICR3 = ((F_CPU / 8) / FASTSERVO_RATE);
            break;

        case 4:
            TCCR4B = _BV(WGM43)|_BV(WGM42)|_BV(CS41); //clk/8, fast pwm
            sbi(TCCR4A,WGM41);
            cbi(TCCR4A,WGM40);
            ICR4 = ((F_CPU / 8) / FASTSERVO_RATE);
            break;

        default:
            m_outputs[index] = &scrapVal; //we can safely write to this value without doing any harm
            return(false);
        }
        return(true);
    }

    SocketF m_servoSockets[FASTSERVO_CHANNELS];
    ParameterF m_gains[FASTSERVO_CHANNELS];
    Parameter16 m_offsets[FASTSERVO_CHANNELS];
    Parameter16 m_mins[FASTSERVO_CHANNELS];
    Parameter16 m_maxs[FASTSERVO_CHANNELS];
    float m_servoWas[FASTSERVO_CHANNELS];
    volatile uint16_t* m_outputs[FASTSERVO_CHANNELS];
    static uint16_t scrapVal;
};

uint16_t FastServo::scrapVal;

FastServo g_FastServo;








/*





void analogWrite(uint8_t pin, int val)
{
    // We need to make sure the PWM output is enabled for those pins
    // that support it, as we turn it off when digitally reading or
    // writing with them.  Also, make sure the pin is in output mode
    // for consistenty with Wiring, which doesn't require a pinMode
    // call for the analog output pins.
    pinMode(pin, OUTPUT);

    bool error = true;

    if (digitalPinToTimer(pin) == TIMER1A) {
        // connect pwm to pin on timer 1, channel A
        sbi(TCCR1A, COM1A1);
        // set pwm duty
        OCR1A = val;
        error = false;
    } else if (digitalPinToTimer(pin) == TIMER1B) {
        // connect pwm to pin on timer 1, channel B
        sbi(TCCR1A, COM1B1);
        // set pwm duty
        OCR1B = val;
#if defined(__AVR_ATmega8__)
    } else if (digitalPinToTimer(pin) == TIMER2) {
        // connect pwm to pin on timer 2, channel B
        sbi(TCCR2, COM21);
        // set pwm duty
        OCR2 = val;
#else
    } else if (digitalPinToTimer(pin) == TIMER0A) {
        if (val == 0) {
            digitalWrite(pin, LOW);
        } else {
            // connect pwm to pin on timer 0, channel A
            sbi(TCCR0A, COM0A1);
            // set pwm duty
            OCR0A = val;
        }
    } else if (digitalPinToTimer(pin) == TIMER0B) {
        if (val == 0) {
            digitalWrite(pin, LOW);
        } else {
            // connect pwm to pin on timer 0, channel B
            sbi(TCCR0A, COM0B1);
            // set pwm duty
            OCR0B = val;
        }
    } else if (digitalPinToTimer(pin) == TIMER2A) {
        // connect pwm to pin on timer 2, channel A
        sbi(TCCR2A, COM2A1);
        // set pwm duty
        OCR2A = val;
    } else if (digitalPinToTimer(pin) == TIMER2B) {
        // connect pwm to pin on timer 2, channel B
        sbi(TCCR2A, COM2B1);
        // set pwm duty
        OCR2B = val;
#endif
#if defined(__AVR_ATmega1280__)
    // XXX: need to handle other timers here
    } else if (digitalPinToTimer(pin) == TIMER3A) {
        // connect pwm to pin on timer 3, channel A
        sbi(TCCR3A, COM3A1);
        // set pwm duty
        OCR3A = val;
    } else if (digitalPinToTimer(pin) == TIMER3B) {
        // connect pwm to pin on timer 3, channel B
        sbi(TCCR3A, COM3B1);
        // set pwm duty
        OCR3B = val;
    } else if (digitalPinToTimer(pin) == TIMER3C) {
        // connect pwm to pin on timer 3, channel C
        sbi(TCCR3A, COM3C1);
        // set pwm duty
        OCR3C = val;
    } else if (digitalPinToTimer(pin) == TIMER4A) {
        // connect pwm to pin on timer 4, channel A
        sbi(TCCR4A, COM4A1);
        // set pwm duty
        OCR4A = val;
    } else if (digitalPinToTimer(pin) == TIMER4B) {
        // connect pwm to pin on timer 4, channel B
        sbi(TCCR4A, COM4B1);
        // set pwm duty
        OCR4B = val;
    } else if (digitalPinToTimer(pin) == TIMER4C) {
        // connect pwm to pin on timer 4, channel C
        sbi(TCCR4A, COM4C1);
        // set pwm duty
        OCR4C = val;
    } else if (digitalPinToTimer(pin) == TIMER5A) {
        // connect pwm to pin on timer 5, channel A
        sbi(TCCR5A, COM5A1);
        // set pwm duty
        OCR5A = val;
    } else if (digitalPinToTimer(pin) == TIMER5B) {
        // connect pwm to pin on timer 5, channel B
        sbi(TCCR5A, COM5B1);
        // set pwm duty
        OCR5B = val;
#endif
    }

    */