API tools faq
paste
enhering

Untitled

enhering
Sep 16th, 2017
139
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Python 7.66 KB | None | 0 0
raw download clone embed print report
  1. #include "SCM.h"
  2.  
  3. SCM  g_cSCM;
  4.  
  5. ISR(SPI_STC_vect) {
  6.   g_cSCM.ManageSlaveSPIInterrupt();
  7. }
  8.  
  9. ISR(TIMER1_COMPA_vect) {
  10.   g_cSCM.ManagePCAEnable();
  11. }
  12.  
  13. volatile bool OutputSelector(uint8_t nSelect) {
  14.   return g_cSCM.OutputSelector(nSelect);
  15. }
  16.  
  17. volatile void InputSelector(uint8_t nSelect) {
  18.   g_cSCM.InputSelector(nSelect);
  19. }
  20.  
  21.  
  22. int main(void) {
  23.  
  24.   g_cSCM.SetOutputSelectorCallback(&OutputSelector);
  25.   g_cSCM.SetInputSelectorCallback(&InputSelector);
  26.  
  27.   sei();
  28.   g_cSCM.Run();
  29. }
  30.  
  31. —————————————————————————————
  32. SCM:
  33. #ifndef SCM_H_
  34. #define SCM_H_
  35.  
  36.   #include "ModuleCapacityOptions.h"
  37.   #include "ModuleSelectOptions.h"
  38.  
  39.   #include "AVRBase.h"
  40.   #include "Blink.h"
  41.   #include "SPIProtocol.h"
  42.  
  43.   #define BOARD_BUILD     BUILD_NUMBER_STR
  44.  
  45.   #include "PCA9685.h"
  46.  
  47.   #define SERVO_TIMING_RANGE 4096
  48.   #define SERVOMIN_TIMING    150
  49.   #define SERVOMAX_TIMING    3500
  50.  
  51.   #define SERVO_COMMAND_MAX  10000
  52.   #define SERVO_COMMAND_MIN -10000
  53.   #define SERVO_COMMAND_RANGE 20000 //SERVO_COMMAND_MAX-SERVO_COMMAND_MIN
  54.  
  55.   #define NUM_SERVO_CHANNELS 16
  56.  
  57.   class SCM : public AVRBase {
  58.     public:
  59.       SCM();
  60.       ~SCM();
  61.  
  62.       void ManageSlaveSPIInterrupt();
  63.  
  64.       void SetOutputSelectorCallback(volatile bool (* pCallback)(uint8_t)) { m_cSPIProtocol.SetOutputSelectorCallback(pCallback); }
  65.       void SetInputSelectorCallback(volatile void (pCallback)(uint8_t))    { m_cSPIProtocol.SetInputSelectorCallback(pCallback);  }
  66.  
  67.       void Run();
  68.  
  69.       void CustomSetup();
  70.       void CustomLoop();
  71.  
  72.       bool OutputSelector(uint8_t);
  73.       void InputSelector(uint8_t);
  74.      
  75.       void SetServoPos(uint8_t, float);
  76.  
  77.     private:
  78.       SPIProtocol m_cSPIProtocol;
  79.       Blink m_cBlink;
  80.  
  81.       volatile uint8_t  m_nModuleStatus;
  82.       volatile uint32_t m_nModuleCapacities;
  83.       volatile uint8_t  m_eSystemStatus;
  84.       volatile uint8_t  m_eMissionStage;
  85.      
  86.       PCA9685 * m_pcPCA9685;
  87.  
  88.       // System data
  89.       int8_t m_nNumServoChannels;
  90.  
  91.       volatile int16_t m_anServoPos[NUMSERVOS];
  92.       volatile int16_t m_anServoZero[NUMSERVOS];
  93.       volatile int16_t m_anServoMax[NUMSERVOS];
  94.       volatile int16_t m_anServoMin[NUMSERVOS];
  95.       volatile bool    m_abServoEnable[NUMSERVOS];
  96.       volatile bool    m_abServoChanged[NUMSERVOS];
  97.       // volatile uint8_t m_anServoFrequency[NUMSERVOS];
  98.  
  99.       int16_t m_fServoRange;
  100.       uint8_t m_nSelectedServo;
  101.  
  102.       volatile bool m_bPCAOutputEnabled;
  103.       volatile uint8_t m_nPCAEnableWindowCountdown;
  104.   };
  105.  
  106. #endif
  107.  
  108. SCM::SCM() {
  109.   m_nNumServoChannels = 16;
  110.   m_nModuleCapacities = BIT17_SERVO_CONTROLER;
  111.  
  112.   m_nModuleStatus = 0;
  113.   // sbi(m_nModuleStatus, MODULE_STATUS_BIT_NONZERO);
  114.  
  115.   m_nSelectedServo = 0;
  116.  
  117.   m_bPCAOutputEnabled = false;
  118.  
  119.   m_nPCAEnableWindowCountdown = 3;
  120.  
  121.   GPIO1_IS_OUTPUT;
  122.   GPIO1_HIGH;
  123.  
  124.   m_pcPCA9685 = new(PCA9685);
  125.  
  126.   m_pcPCA9685->Reset();
  127.  
  128.   m_pcPCA9685->SetPWMFreq(500);
  129.  
  130.   for (uint8_t nIndex = 0; nIndex < NUMSERVOS; nIndex++) {
  131.     m_anServoZero[nIndex] = 0;
  132.     m_anServoMax[nIndex] = SERVO_COMMAND_MAX;
  133.     m_anServoMin[nIndex] = SERVO_COMMAND_MIN;
  134.     m_anServoPos[nIndex] = SERVO_COMMAND_MIN;
  135.     m_abServoEnable[nIndex] = false;
  136.     m_abServoChanged[nIndex] = true;
  137.   }
  138.  
  139.   GPIO1_LOW; // PWM OUTPUT ENABLE
  140.  
  141.   m_cBlink.SetLEDGPIO(7);
  142.   m_cBlink.SetLEDOnCount(50000);
  143.   m_cBlink.SetLEDOffCount(50000);
  144.  
  145.   m_cBlink.BlinkLED();
  146. }
  147.  
  148. SCM::~SCM() {
  149.  
  150. }
  151.  
  152. void SCM::ManageSlaveSPIInterrupt() {
  153.   m_cSPIProtocol.ManageSlaveSPIInterrupt();
  154. }
  155.  
  156. void SCM::Run() {
  157.  
  158.   CustomSetup();
  159.  
  160.   while(1) {
  161.     m_cBlink.ManageLED();
  162.  
  163.     CustomLoop();  
  164.   }
  165. }
  166.  
  167. void SCM::CustomSetup() {
  168.   m_cSPIProtocol.RunAsSlave();
  169.   m_cSPIProtocol.InitSPI();
  170.  
  171.  
  172.   TCCR1B |= (1 << WGM12); // Configure timer 1 for CTC mode
  173.   TIMSK |= (1 << OCIE1A); // Enable CTC interrupt
  174.   OCR1A = 155;
  175.   TCCR1B |= ((1 << CS10) | (1 << CS12)); // Start timer at Fcpu/1024
  176.  
  177. }
  178.  
  179. void SCM::CustomLoop() {
  180.   uint8_t nServoNum;
  181.  
  182.   for (nServoNum = 0; nServoNum < NUM_SERVO_CHANNELS; nServoNum++) {
  183.     if (m_abServoChanged[nServoNum]) {
  184.       if (m_abServoEnable[nServoNum]) {
  185.         SetServoPos(nServoNum);
  186.       }
  187.       else {
  188.         m_anServoPos[nServoNum] = SERVO_COMMAND_MIN;
  189.         SetServoPos(nServoNum);
  190.       }
  191.       m_abServoChanged[nServoNum] = false;
  192.     }
  193.   }
  194. }
  195.  
  196. void SCM::ManagePCAEnable() {
  197.   if (m_bPCAOutputEnabled) {
  198.     GPIO1_HIGH;
  199.     m_bPCAOutputEnabled = false;
  200.     OCR1A = 155;
  201.   }
  202.   else {
  203.     m_nPCAEnableWindowCountdown--;
  204.  
  205.     // Protection against loss of communication from COM module.
  206.     if (m_nPCAEnableWindowCountdown > 0) {
  207.       GPIO1_LOW;
  208.       m_bPCAOutputEnabled = true;
  209.     }
  210.     OCR1A = 15;
  211.   }
  212. }
  213.  
  214. bool SCM::OutputSelector(uint8_t nSelect) {
  215.  
  216.   switch (nSelect) {
  217.     case B00_MODULE_STATUS: m_cSPIProtocol.SendByteViaSPI(NOT_APPLICABLE, NOT_APPLICABLE, (uint8_t) m_nModuleStatus); break;
  218.     case B00_SYSTEM_STATUS: m_cSPIProtocol.ExpectByte(); break;
  219.     case B00_MISSION_STAGE: m_cSPIProtocol.ExpectByte(); break;
  220.  
  221.     case B17_NUM_SERVO_CHANNELS: m_cSPIProtocol.SendByteViaSPI(NOT_APPLICABLE, NOT_APPLICABLE, m_nNumServoChannels); break;
  222.  
  223.     case B17_SERVO_SELECT: m_cSPIProtocol.ExpectByte(); break;
  224.  
  225.     case B17_SERVO_MIN:    m_cSPIProtocol.ExpectInt(); break;
  226.     case B17_SERVO_MAX:    m_cSPIProtocol.ExpectInt(); break;
  227.     case B17_SERVO_ZERO:   m_cSPIProtocol.ExpectInt(); break;
  228.     case B17_SERVO_POS:    m_cSPIProtocol.ExpectInt(); break;
  229.  
  230.     case B17_SERVO_ENABLE: m_cSPIProtocol.ExpectByte(); break;
  231.  
  232.     default: SPDR = NOT_UNDERSTOOD; return false;
  233.   }
  234.  
  235.   return true;
  236. }
  237.  
  238. void SCM::InputSelector(uint8_t nSelect) {
  239.  
  240.   switch (nSelect) {
  241.     case B00_SYSTEM_STATUS: m_eSystemStatus = m_cSPIProtocol.ReadByteViaSPI(NOT_APPLICABLE, NOT_APPLICABLE); break;
  242.     case B00_MISSION_STAGE: m_eMissionStage = m_cSPIProtocol.ReadByteViaSPI(NOT_APPLICABLE, NOT_APPLICABLE); break;
  243.  
  244.     case B17_SERVO_SELECT: m_nSelectedServo = m_cSPIProtocol.ReadByteViaSPI(NOT_APPLICABLE, NOT_APPLICABLE);
  245.                            m_nPCAEnableWindowCountdown = 3;
  246.                            break;
  247.  
  248.     case B17_SERVO_MIN:    m_nServoMin[m_nSelectedServo]    = m_cSPIProtocol.ReadLongViaSPI(NOT_APPLICABLE, NOT_APPLICABLE);
  249.                            m_abServoChanged[m_nSelectedServo] = true;
  250.                            break;
  251.  
  252.     case B17_SERVO_MAX:    m_nServoMax[m_nSelectedServo]    = m_cSPIProtocol.ReadLongViaSPI(NOT_APPLICABLE, NOT_APPLICABLE);
  253.                            m_abServoChanged[m_nSelectedServo] = true;
  254.                            break;
  255.     case B17_SERVO_ZERO:   m_nServoZero[m_nSelectedServo]   = m_cSPIProtocol.ReadLongViaSPI(NOT_APPLICABLE, NOT_APPLICABLE);
  256.                            m_abServoChanged[m_nSelectedServo] = true;
  257.                            break;
  258.     case B17_SERVO_POS:    m_nServoPos[m_nSelectedServo]    = m_cSPIProtocol.ReadLongViaSPI(NOT_APPLICABLE, NOT_APPLICABLE);
  259.                            m_abServoChanged[m_nSelectedServo] = true;
  260.                            break;
  261.     case B17_SERVO_ENABLE: m_nServoEnable[m_nSelectedServo] = m_cSPIProtocol.ReadByteViaSPI(NOT_APPLICABLE, NOT_APPLICABLE);
  262.                            m_abServoChanged[m_nSelectedServo] = true;
  263.                            break;
  264.  
  265.     default: SPDR = NOT_UNDERSTOOD;
  266.   }
  267. }
  268.  
  269. void SCM::SetServoPos(uint8_t nServoNum) {
  270.  
  271.   if (nServoNum < NUMSERVOS) {
  272.  
  273.     if (m_abServoEnable[nServoNum]) {
  274.  
  275.       uint16_t nServoPos = m_anServoPos[nServoNum];
  276.  
  277.       uint16_t nServoOffTime;
  278.  
  279.       if (nServoPos < m_anServoMin[nServoNum]) {
  280.         nServoPos = m_anServoMin[nServoNum];
  281.       }
  282.  
  283.       if (nServoPos > m_anServoMax[nServoNum]) {
  284.         nServoPos = m_anServoMax[nServoNum];
  285.       }
  286.  
  287.       nServoOffTime = ((-1 * SERVO_COMMAND_MIN) + nServoPos) * (SERVO_TIMING_RANGE / SERVO_COMMAND_RANGE); // make the second term a constant!
  288.  
  289.       m_pcPCA9685->SetPWM(nServoNum, 0, nServoOffTime);
  290.     }
  291.   }
  292. }
Advertisement
Add Comment
Please, Sign In to add comment
Public Pastes
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!