C:/Documents and Settings/gw/My Documents/Other projects/flightos/Modules/servo50hz.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/>.
*/




#include "Servo/Servo.h"

#if SERVO50HZCHANNELS > 8
#error "too many 50Hz servos"
#endif


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

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

MAKENAME(1);
MAKENAME(2);
MAKENAME(3);
MAKENAME(4);
MAKENAME(5);
MAKENAME(6);
MAKENAME(7);
MAKENAME(8);
MAKEARRAY(Servo50HzNames);

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


MAKENAME(Min,1);
MAKENAME(Min,2);
MAKENAME(Min,3);
MAKENAME(Min,4);
MAKENAME(Min,5);
MAKENAME(Min,6);
MAKENAME(Min,7);
MAKENAME(Min,8);
MAKEARRAY(Servo50HzMin);

MAKENAME(Max,1);
MAKENAME(Max,2);
MAKENAME(Max,3);
MAKENAME(Max,4);
MAKENAME(Max,5);
MAKENAME(Max,6);
MAKENAME(Max,7);
MAKENAME(Max,8);
MAKEARRAY(Servo50HzMax);

MAKENAME(Gain,1);
MAKENAME(Gain,2);
MAKENAME(Gain,3);
MAKENAME(Gain,4);
MAKENAME(Gain,5);
MAKENAME(Gain,6);
MAKENAME(Gain,7);
MAKENAME(Gain,8);
MAKEARRAY(Servo50HzGain);

MAKENAME(Offset,1);
MAKENAME(Offset,2);
MAKENAME(Offset,3);
MAKENAME(Offset,4);
MAKENAME(Offset,5);
MAKENAME(Offset,6);
MAKENAME(Offset,7);
MAKENAME(Offset,8);
MAKEARRAY(Servo50HzOffset);
#undef MAKENAME
#undef MAKEARRAY




class Servo50Hz : public Module
{
    DECLARE_MODULE(Servo50Hz,Module);
    public:


    //Only use this to expose sockets and parameters. Do most of your
    //initialisastion in Init()
    Servo50Hz()
    {

        AddSocketArray(Servo50HzNames, m_servoSockets, SERVO50HZCHANNELS);
        AddSocketArray(Servo50HzMin, m_mins, SERVO50HZCHANNELS);
        AddSocketArray(Servo50HzMax, m_maxs, SERVO50HZCHANNELS);
        AddSocketArray(Servo50HzGain, m_gains, SERVO50HZCHANNELS);
        AddSocketArray(Servo50HzOffset, m_offsets, SERVO50HZCHANNELS);

        for (int ct=0; ct< SERVO50HZCHANNELS; 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)
    {
        for (int ct=0; ct< SERVO50HZCHANNELS; ct++)
        {
            if (param == &m_mins[ct] || param == &m_maxs[ct])// || param == &m_maxs[ct])
            {
                m_servos[ct].detach();
                m_servos[ct].attach(servo50HzPins[ct],m_mins[ct],m_maxs[ct]);
                break;
            }
        }
    }

    //Initialise the module
    //set up hardware etc
    virtual void Init()
    {
        for (int ct=0; ct< SERVO50HZCHANNELS; ct++)
        {
            m_servos[ct].attach(servo50HzPins[ct],m_mins[ct],m_maxs[ct]);
        }

        //Set how often Loop() should be executed (in milliseconds)
        SetInterval(20);
        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< SERVO50HZCHANNELS; ct++)
        {

            float socket = m_servoSockets[ct];
            if (socket != m_servoWas[ct])
            {
                m_servoWas[ct] = socket;
                m_servos[ct].writeMicroseconds(((int)(socket * m_gains[ct])) + m_offsets[ct]);
            }
        }
    }

private:

    SocketF m_servoSockets[SERVO50HZCHANNELS];
    ParameterF m_gains[SERVO50HZCHANNELS];
    Parameter16 m_offsets[SERVO50HZCHANNELS];
    Parameter16 m_mins[SERVO50HZCHANNELS];
    Parameter16 m_maxs[SERVO50HZCHANNELS];
    float m_servoWas[SERVO50HZCHANNELS];
    Servo m_servos[SERVO50HZCHANNELS];
};

Servo50Hz g_Servo50Hz;