servo module

#ifndef SCM_H_
#define SCM_H_

  #include "ModuleCapacityOptions.h"
  #include "ModuleSelectOptions.h"

  #include "AVRBase.h"
  #include "Blink.h"
  #include "SPIProtocol.h"

  #define BOARD_BUILD     BUILD_NUMBER_STR

  #include "PCA9685.h"

  #define SERVO_TIMING_RANGE 4096
  #define SERVOMIN_TIMING    150
  #define SERVOMAX_TIMING    3500

  #define SERVO_COMMAND_MAX  10000
  #define SERVO_COMMAND_MIN -10000

  #define NUM_SERVO_CHANNELS 16

  class SCM : public AVRBase {
    public:
      SCM();
      ~SCM();

      void ManageSlaveSPIInterrupt();

      void SetOutputSelectorCallback(volatile bool (* pCallback)(uint8_t)) { m_cSPIProtocol.SetOutputSelectorCallback(pCallback); }
      void SetInputSelectorCallback(volatile void (pCallback)(uint8_t))    { m_cSPIProtocol.SetInputSelectorCallback(pCallback);  }

      void Run();

      void CustomSetup();
      void CustomLoop();

      bool OutputSelector(uint8_t);
      void InputSelector(uint8_t);

      void SetServoPos(uint8_t, float);

    private:
      SPIProtocol m_cSPIProtocol;
      Blink m_cBlink;

      volatile uint8_t  m_nModuleStatus;
      volatile uint32_t m_nModuleCapacities;
      volatile uint8_t  m_eSystemStatus;
      volatile uint8_t  m_eMissionStage;

      PCA9685 * m_pcPCA9685;

      // System data
      int8_t m_nNumServoChannels;

      volatile int16_t m_anServoPos[NUMSERVOS];
      volatile int16_t m_anServoZero[NUMSERVOS];
      volatile int16_t m_anServoMax[NUMSERVOS];
      volatile int16_t m_anServoMin[NUMSERVOS];
      volatile bool    m_abServoEnable[NUMSERVOS];
      volatile bool    m_abServoChanged[NUMSERVOS];
      // volatile uint8_t m_anServoFrequency[NUMSERVOS];

      int16_t m_fServoRange;
      uint8_t m_nSelectedServo;
  };

#endif

#include "SCM.h"

SCM::SCM() {
  m_nNumServoChannels = 16;
  m_nModuleCapacities = BIT17_SERVO_CONTROLER;

  m_nModuleStatus = 0;
  // sbi(m_nModuleStatus, MODULE_STATUS_BIT_NONZERO);

  m_nSelectedServo = 0;

  GPIO1_IS_OUTPUT;
  GPIO1_HIGH;

  m_pcPCA9685 = new(PCA9685);

  m_pcPCA9685->Reset();

  m_pcPCA9685->SetPWMFreq(60);

  for (uint8_t nIndex = 0; nIndex < NUMSERVOS; nIndex++) {
    m_anServoZero[nIndex] = 0;
    m_anServoMax[nIndex] = SERVO_COMMAND_MAX;
    m_anServoMin[nIndex] = SERVO_COMMAND_MIN;
    m_anServoPos[nIndex] = SERVO_COMMAND_MIN;
    m_abServoEnable[nIndex] = false;
    m_abServoChanged[nIndex] = true;
  }

  GPIO1_LOW; // PWM OUTPUT ENABLE

  m_cBlink.SetLEDGPIO(7);
  m_cBlink.SetLEDOnCount(50000);
  m_cBlink.SetLEDOffCount(50000);

  m_cBlink.BlinkLED();
}

SCM::~SCM() {

}

void SCM::ManageSlaveSPIInterrupt() {
  m_cSPIProtocol.ManageSlaveSPIInterrupt();
}

void SCM::Run() {

  CustomSetup();

  while(1) {
    m_cBlink.ManageLED();

    CustomLoop();
  }
}

void SCM::CustomSetup() {
  m_cSPIProtocol.RunAsSlave();
  m_cSPIProtocol.InitSPI();
}

void SCM::CustomLoop() {
  uint8_t nServoNum;

  for (nServoNum = 0; nServoNum < NUM_SERVO_CHANNELS; nServoNum++) {
    if (m_abServoChanged[nServoNum]) {
      if (m_abServoEnable[nServoNum]) {
        SetServoPos(nServoNum);
      }
      else {
        m_anServoPos[nServoNum] = SERVO_COMMAND_MIN;
        SetServoPos(nServoNum);
      }
      m_abServoChanged[nServoNum] = false;
    }
  }
}

bool SCM::OutputSelector(uint8_t nSelect) {

  switch (nSelect) {
    case B00_MODULE_STATUS: m_cSPIProtocol.SendByteViaSPI(NOT_APPLICABLE, NOT_APPLICABLE, (uint8_t) m_nModuleStatus);
    case B00_SYSTEM_STATUS: m_cSPIProtocol.ExpectByte(); break;
    case B00_MISSION_STAGE: m_cSPIProtocol.ExpectByte(); break;

    case B17_NUM_SERVO_CHANNELS: m_cSPIProtocol.SendByteViaSPI(NOT_APPLICABLE, NOT_APPLICABLE, m_nNumServoChannels);

    case B17_SERVO_SELECT: m_cSPIProtocol.ExpectByte(); break;

    case B17_SERVO_MIN:    m_cSPIProtocol.ExpectInt(); break;
    case B17_SERVO_MAX:    m_cSPIProtocol.ExpectInt(); break;
    case B17_SERVO_ZERO:   m_cSPIProtocol.ExpectInt(); break;
    case B17_SERVO_POS:    m_cSPIProtocol.ExpectInt(); break;

    case B17_SERVO_ENABLE: m_cSPIProtocol.ExpectByte(); break;

    default: SPDR = NOT_UNDERSTOOD; return false;
  }

  return true;
}

void SCM::InputSelector(uint8_t nSelect) {

  switch (nSelect) {
    case B00_SYSTEM_STATUS: m_eSystemStatus = m_cSPIProtocol.ReadByteViaSPI(NOT_APPLICABLE, NOT_APPLICABLE); break;
    case B00_MISSION_STAGE: m_eMissionStage = m_cSPIProtocol.ReadByteViaSPI(NOT_APPLICABLE, NOT_APPLICABLE); break;

    case B17_SERVO_SELECT: m_nSelectedServo = m_cSPIProtocol.ReadByteViaSPI(NOT_APPLICABLE, NOT_APPLICABLE); break;

    case B17_SERVO_MIN:    m_nServoMin[m_nSelectedServo]    = m_cSPIProtocol.ReadLongViaSPI(NOT_APPLICABLE, NOT_APPLICABLE);
                           m_abServoChanged[m_nSelectedServo] = true;
                           break;

    case B17_SERVO_MAX:    m_nServoMax[m_nSelectedServo]    = m_cSPIProtocol.ReadLongViaSPI(NOT_APPLICABLE, NOT_APPLICABLE);
                           m_abServoChanged[m_nSelectedServo] = true;
                           break;
    case B17_SERVO_ZERO:   m_nServoZero[m_nSelectedServo]   = m_cSPIProtocol.ReadLongViaSPI(NOT_APPLICABLE, NOT_APPLICABLE);
                           m_abServoChanged[m_nSelectedServo] = true;
                           break;
    case B17_SERVO_POS:    m_nServoPos[m_nSelectedServo]    = m_cSPIProtocol.ReadLongViaSPI(NOT_APPLICABLE, NOT_APPLICABLE);
                           m_abServoChanged[m_nSelectedServo] = true;
                           break;
    case B17_SERVO_ENABLE: m_nServoEnable[m_nSelectedServo] = m_cSPIProtocol.ReadByteViaSPI(NOT_APPLICABLE, NOT_APPLICABLE);
                           m_abServoChanged[m_nSelectedServo] = true;
                           break;

    default: SPDR = NOT_UNDERSTOOD;
  }
}

void SCM::SetServoPos(uint8_t nServoNum, int16_t nServoPos) {

  if (nServoNum < NUMSERVOS) {

    if (m_abServoEnable[nServoNum]) {

      uint16_t nServoOffTime;

      if (nServoPos < m_anServoMin[nServoNum]) {
        nServoPos = m_anServoMin[nServoNum];
      }

      if (nServoPos > m_anServoMax[nServoNum]) {
        nServoPos = m_anServoMax[nServoNum];
      }

      uint16_t nZeroPos = m_anServoZero[nServoNum];
      uint16_t nPositiveRange = 4096 - nZeroPos;
      uint16_t nNegativeRange = nZeroPos;

      if (nServoPos >= 0) {
        nServoOffTime = (uint16_t) (fZeroPos + fServoPos * fPositiveRange);
      }
      else {
        nServoOffTime = (uint16_t) (fZeroPos + fServoPos * fNegativeRange);
      }

      m_pcPCA9685->SetPWM(nServoNum, 0, nServoOffTime);

      m_afServoPos[nServoNum] = nServoOffTime;
    }
  }
}