stepper_controller/arduino/stepper/qm_code/stepper.c

   1 /*****************************************************************************
   2 * Model: C:/Documents and Settings/Harry McNally/My Documents/project/arduino/stepper/arduino/stepper/stepper.qm
   3 * File:  ./qm_code/stepper.c
   4 *
   5 * This file has been generated automatically by QP Modeler (QM).
   6 * DO NOT EDIT THIS FILE MANUALLY.
   7 *
   8 * Please visit www.state-machine.com/qm for more information.
   9 *****************************************************************************/
  10 //
  11 // Product: Radiotelescope - Stepper Motor Controller
  12 // Version: 0.1
  13 // Date:    25 November 2010
  14 //
  15 //                      +-----------------------+
  16 //                      |   d e c i s i o n s   |
  17 //                      +-----------------------|
  18 //                      | a n d   d e s i g n s |
  19 //                      +-----------------------+
  20 //
  21 // Copyright (C) 2009 Decisions and Designs Pty Ltd. All rights reserved.
  22 //
  23 // This software may be distributed and modified under the terms of the GNU
  24 // General Public License version 2 (GPL) as published by the Free Software
  25 // Foundation and appearing in the file GPL.TXT included in the packaging of
  26 // this file. Please note that GPL Section 2[b] requires that all works based
  27 // on this software must also be made publicly available under the terms of
  28 // the GPL ("Copyleft").
  29 //
  30 // Contact information:
  31 // Decisions and Designs Web site: http://www.decisions-and-designs.com.au
  32 // e-mail:                         [email protected]
  33 //
  34
  35 #include "radiotelescope.h"
  36 #include "signals.h"
  37 #include "stepper.h"
  38 #include "qpn_port.h"
  39 #include "bsp.h"
  40
  41 /* Typedefs for pointers to (timer and direction) driver functions */
  42 typedef uint8_t (*FPQueueStepTime)(uint32_t);
  43 typedef void (*FPSetDirection)(int8_t);
  44
  45 /* declaration of the Stepper active object --------------------------------*/
  46 /* $(AOs::Stepper) .........................................................*/
  47 typedef struct StepperTag {
  48 /* protected: */
  49     QActive super;
  50
  51 /* private: */
  52     double basePeriod;
  53     uint32_t slewTimer;
  54     uint32_t totalSlewSteps;
  55     double accelerationFactor;
  56     double stepPeriod;
  57     uint32_t stepTimer;
  58     uint32_t slewStepCount;
  59     uint32_t decelerationStep;
  60     int32_t stepPosition;
  61     int8_t direction;
  62     FPQueueStepTime queueStepTime;
  63     FPSetDirection setDirection;
  64 } Stepper;
  65
  66 /* public: */
  67 void Stepper_ctor(void);
  68
  69 /* protected: */
  70 QState Stepper_initial(Stepper *me);
  71 QState Stepper_operational(Stepper *me);
  72 QState Stepper_LinearAccelerator(Stepper *me);
  73 QState Stepper_LinearDecelerate(Stepper *me);
  74 QState Stepper_LinearAccelerate(Stepper *me);
  75 QState Stepper_LinearConstantSpeed(Stepper *me);
  76 QState Stepper_Stopped(Stepper *me);
  77
  78
  79 /* FP for support code */
  80 void initialiseStepperData(Stepper *me);
  81 void setupLinearAccelerator(Stepper *me);
  82 void accelerationStepPeriod(Stepper *me);
  83 void decelerationStepPeriod(Stepper *me);
  84
  85 /* global objects ----------------------------------------------------------*/
  86 Stepper AO_azimuth;
  87 Stepper AO_elevation;
  88
  89 /* Active object definition ------------------------------------------------*/
  90 /* $(AOs::Stepper) .........................................................*/
  91 /* $(AOs::Stepper::ctor) ...................................................*/
  92 void Stepper_ctor(void) {
  93     /* Configure AO_azimuth Stepper */
  94     AO_azimuth.queueStepTime = BSP_nextStepTimer3;
  95     AO_azimuth.setDirection = BSP_azimuthDirection;
  96     QActive_ctor((QActive *)&AO_azimuth, (QStateHandler)&Stepper_initial);
  97     BSP_initStepTimer3((QActive *)&AO_azimuth);
  98     BSP_initAzimuthFault((QActive *)&AO_azimuth);
  99     /* Configure AO_elevation Stepper */
 100     AO_elevation.queueStepTime = BSP_nextStepTimer4;
 101     AO_elevation.setDirection = BSP_elevationDirection;
 102     QActive_ctor((QActive *)&AO_elevation, (QStateHandler)&Stepper_initial);
 103     BSP_initStepTimer4((QActive *)&AO_elevation);
 104     BSP_initElevationFault((QActive *)&AO_elevation);
 105 }
 106 /* $(AOs::Stepper::Statechart) .............................................*/
 107 /* @(/2/2/13/0) */
 108 QState Stepper_initial(Stepper *me) {
 109     return Q_TRAN(&Stepper_operational);
 110 }
 111 /* $(AOs::Stepper::Statechart::operational) ................................*/
 112 QState Stepper_operational(Stepper *me) {
 113     switch (Q_SIG(me)) {
 114         /* @(/2/2/13/1) */
 115         case Q_ENTRY_SIG: {
 116             initialiseStepperData(me);
 117
 118             return Q_HANDLED();
 119         }
 120         /* @(/2/2/13/1/0) */
 121         case Q_INIT_SIG: {
 122             return Q_TRAN(&Stepper_Stopped);
 123         }
 124     }
 125     return Q_SUPER(&QHsm_top);
 126 }
 127 /* $(AOs::Stepper::Statechart::operational::LinearAccelerator) .............*/
 128 QState Stepper_LinearAccelerator(Stepper *me) {
 129     switch (Q_SIG(me)) {
 130         /* @(/2/2/13/1/1) */
 131         case Q_ENTRY_SIG: {
 132             setupLinearAccelerator(me);
 133             (*me->setDirection)(me->direction);
 134
 135             return Q_HANDLED();
 136         }
 137         /* @(/2/2/13/1/1/0) */
 138         case Q_INIT_SIG: {
 139             return Q_TRAN(&Stepper_LinearAccelerate);
 140         }
 141         /* @(/2/2/13/1/1/1) */
 142         case STEPPER_FAULT_SIG: {
 143             return Q_TRAN(&Stepper_Stopped);
 144         }
 145     }
 146     return Q_SUPER(&Stepper_operational);
 147 }
 148 /* $(AOs::Stepper::Statechart::operational::LinearAccelerator::LinearDecelerate) */
 149 QState Stepper_LinearDecelerate(Stepper *me) {
 150     switch (Q_SIG(me)) {
 151         /* @(/2/2/13/1/1/2) */
 152         case Q_ENTRY_SIG: {
 153             decelerationStepPeriod(me);
 154             (*me->queueStepTime)(me->stepTimer);
 155             return Q_HANDLED();
 156         }
 157         /* @(/2/2/13/1/1/2/0) */
 158         case NEXT_STEP_TIME_SIG: {
 159             me->slewStepCount++;
 160             me->stepPosition += me->direction;
 161             /* @(/2/2/13/1/1/2/0/0) */
 162             if (me->slewStepCount <  me->totalSlewSteps) {
 163                 decelerationStepPeriod(me);
 164                 (*me->queueStepTime)(me->stepTimer);
 165                 return Q_HANDLED();
 166             }
 167             /* @(/2/2/13/1/1/2/0/1) */
 168             else {
 169                 return Q_TRAN(&Stepper_Stopped);
 170             }
 171         }
 172     }
 173     return Q_SUPER(&Stepper_LinearAccelerator);
 174 }
 175 /* $(AOs::Stepper::Statechart::operational::LinearAccelerator::LinearAccelerate) */
 176 QState Stepper_LinearAccelerate(Stepper *me) {
 177     switch (Q_SIG(me)) {
 178         /* @(/2/2/13/1/1/3) */
 179         case Q_ENTRY_SIG: {
 180             (*me->queueStepTime)(me->stepTimer);
 181             accelerationStepPeriod(me);
 182             return Q_HANDLED();
 183         }
 184         /* @(/2/2/13/1/1/3/0) */
 185         case NEXT_STEP_TIME_SIG: {
 186             me->slewStepCount++;
 187             me->stepPosition += me->direction;
 188             /* @(/2/2/13/1/1/3/0/1) */
 189             if (me->slewStepCount < me->decelerationStep) {
 190                 /* @(/2/2/13/1/1/3/0/1/0) */
 191                 if (me->stepTimer > me->slewTimer) {
 192                     (*me->queueStepTime)(me->stepTimer);
 193                     accelerationStepPeriod(me);
 194                     return Q_HANDLED();
 195                 }
 196                 /* @(/2/2/13/1/1/3/0/1/1) */
 197                 else {
 198                     me->stepTimer = me->slewTimer;
 199                     me->decelerationStep =
 200                     me->totalSlewSteps - me->slewStepCount;
 201                     return Q_TRAN(&Stepper_LinearConstantSpeed);
 202                 }
 203             }
 204             /* @(/2/2/13/1/1/3/0/0) */
 205             else {
 206                 return Q_TRAN(&Stepper_LinearDecelerate);
 207             }
 208         }
 209     }
 210     return Q_SUPER(&Stepper_LinearAccelerator);
 211 }
 212 /* $(AOs::Stepper::Statechart::operational::LinearAccelerator::LinearConstantSpeed) */
 213 QState Stepper_LinearConstantSpeed(Stepper *me) {
 214     switch (Q_SIG(me)) {
 215         /* @(/2/2/13/1/1/4) */
 216         case Q_ENTRY_SIG: {
 217             (*me->queueStepTime)(me->stepTimer);
 218             return Q_HANDLED();
 219         }
 220         /* @(/2/2/13/1/1/4/0) */
 221         case NEXT_STEP_TIME_SIG: {
 222             me->slewStepCount++;
 223             me->stepPosition += me->direction;
 224             /* @(/2/2/13/1/1/4/0/0) */
 225             if (me->slewStepCount <  me->decelerationStep) {
 226                 (*me->queueStepTime)(me->stepTimer);
 227                 return Q_HANDLED();
 228             }
 229             /* @(/2/2/13/1/1/4/0/1) */
 230             else {
 231                 return Q_TRAN(&Stepper_LinearDecelerate);
 232             }
 233         }
 234     }
 235     return Q_SUPER(&Stepper_LinearAccelerator);
 236 }
 237 /* $(AOs::Stepper::Statechart::operational::Stopped) .......................*/
 238 QState Stepper_Stopped(Stepper *me) {
 239     switch (Q_SIG(me)) {
 240         /* @(/2/2/13/1/2/0) */
 241         case SET_BASE_SPEED_SIG: {
 242             /* Calculate and store (double)basePeriod */
 243             UParam p; p.param = Q_PAR(me);
 244             me->basePeriod =
 245             AZIMUTH_CLOCK_DBL / p.dbl;
 246             return Q_HANDLED();
 247         }
 248         /* @(/2/2/13/1/2/1) */
 249         case SET_SLEW_SPEED_SIG: {
 250             /* Calculate and store (uint32_t)slewPeriod */
 251             UParam p; p.param = Q_PAR(me);
 252             me->slewTimer = (uint32_t)
 253             (AZIMUTH_CLOCK_DBL / p.dbl);
 254             return Q_HANDLED();
 255         }
 256         /* @(/2/2/13/1/2/2) */
 257         case SET_ACCELERATION_SIG: {
 258             /* Calculate and store derived factor */
 259             UParam p; p.param = Q_PAR(me);
 260             me->accelerationFactor = p.dbl
 261             / AZIMUTH_CLOCK_DBL
 262             / AZIMUTH_CLOCK_DBL;
 263             return Q_HANDLED();
 264         }
 265         /* @(/2/2/13/1/2/3) */
 266         case SET_TOTAL_STEPS_SIG: {
 267             UParam p; p.param = Q_PAR(me);
 268             me->totalSlewSteps = p.uint32;
 269             return Q_HANDLED();
 270         }
 271         /* @(/2/2/13/1/2/4) */
 272         case START_LINEAR_SLEW_SIG: {
 273             /* @(/2/2/13/1/2/4/0) */
 274             if (me->totalSlewSteps > 0) {
 275                 return Q_TRAN(&Stepper_LinearAccelerator);
 276             }
 277             break;
 278         }
 279         /* @(/2/2/13/1/2/5) */
 280         case SET_DIRECTION_UP_CW_SIG: {
 281             me->direction = 1;
 282             return Q_HANDLED();
 283         }
 284         /* @(/2/2/13/1/2/6) */
 285         case SET_DIRECTION_DOWN_CCW_SIG: {
 286             me->direction = -1;
 287             return Q_HANDLED();
 288         }
 289     }
 290     return Q_SUPER(&Stepper_operational);
 291 }
 292
 293
 294 /* Support code */
 295 void initialiseStepperData(Stepper *me)
 296 {
 297     me->basePeriod = (double)SLOW_STEP_PERIOD;
 298     me->slewTimer = SLOW_STEP_PERIOD;
 299     me->totalSlewSteps = 0;
 300     me->accelerationFactor = 0.0;
 301 }
 302
 303 void setupLinearAccelerator(Stepper *me)
 304 {
 305     me->stepPeriod = me->basePeriod;
 306     me->stepTimer = (uint32_t)me->stepPeriod;
 307     me->slewStepCount = 0;
 308     me->decelerationStep = (me->totalSlewSteps + 1) / 2;
 309 }
 310
 311 void accelerationStepPeriod(Stepper *me)
 312 {
 313     me->stepPeriod -=   me->accelerationFactor
 314                       * me->stepPeriod
 315                       * me->stepPeriod
 316                       * me->stepPeriod;
 317     me->stepTimer = (uint32_t)me->stepPeriod;
 318 }
 319
 320 void decelerationStepPeriod(Stepper *me)
 321 {
 322     me->stepPeriod +=   me->accelerationFactor
 323                       * me->stepPeriod
 324                       * me->stepPeriod
 325                       * me->stepPeriod;
 326     me->stepTimer = (uint32_t)me->stepPeriod;
 327 }