1 /*****************************************************************************
2 * Model: C:/Documents and Settings/Harry McNally/My Documents/project/arduino/stepper/arduino/stepper/stepper.qm
3 * File: ./qm_code/stepper.c
5 * This file has been generated automatically by QP Modeler (QM).
6 * DO NOT EDIT THIS FILE MANUALLY.
8 * Please visit www.state-machine.com/qm for more information.
9 *****************************************************************************/
11 // Product: Radiotelescope - Stepper Motor Controller
13 // Date: 25 November 2010
15 // +-----------------------+
16 // | d e c i s i o n s |
17 // +-----------------------|
18 // | a n d d e s i g n s |
19 // +-----------------------+
21 // Copyright (C) 2009 Decisions and Designs Pty Ltd. All rights reserved.
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").
30 // Contact information:
31 // Decisions and Designs Web site: http://www.decisions-and-designs.com.au
35 #include "radiotelescope.h"
41 /* Typedefs for pointers to (timer and direction) driver functions */
42 typedef uint8_t (*FPQueueStepTime)(uint32_t);
43 typedef void (*FPSetDirection)(int8_t);
45 /* declaration of the Stepper active object --------------------------------*/
46 /* $(AOs::Stepper) .........................................................*/
47 typedef struct StepperTag {
54 uint32_t totalSlewSteps;
55 double accelerationFactor;
58 uint32_t slewStepCount;
59 uint32_t decelerationStep;
62 FPQueueStepTime queueStepTime;
63 FPSetDirection setDirection;
67 void Stepper_ctor(void);
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);
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);
85 /* global objects ----------------------------------------------------------*/
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);
106 /* $(AOs::Stepper::Statechart) .............................................*/
108 QState Stepper_initial(Stepper *me) {
109 return Q_TRAN(&Stepper_operational);
111 /* $(AOs::Stepper::Statechart::operational) ................................*/
112 QState Stepper_operational(Stepper *me) {
116 initialiseStepperData(me);
122 return Q_TRAN(&Stepper_Stopped);
125 return Q_SUPER(&QHsm_top);
127 /* $(AOs::Stepper::Statechart::operational::LinearAccelerator) .............*/
128 QState Stepper_LinearAccelerator(Stepper *me) {
132 setupLinearAccelerator(me);
133 (*me->setDirection)(me->direction);
137 /* @(/2/2/13/1/1/0) */
139 return Q_TRAN(&Stepper_LinearAccelerate);
141 /* @(/2/2/13/1/1/1) */
142 case STEPPER_FAULT_SIG: {
143 return Q_TRAN(&Stepper_Stopped);
146 return Q_SUPER(&Stepper_operational);
148 /* $(AOs::Stepper::Statechart::operational::LinearAccelerator::LinearDecelerate) */
149 QState Stepper_LinearDecelerate(Stepper *me) {
151 /* @(/2/2/13/1/1/2) */
153 decelerationStepPeriod(me);
154 (*me->queueStepTime)(me->stepTimer);
157 /* @(/2/2/13/1/1/2/0) */
158 case NEXT_STEP_TIME_SIG: {
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);
167 /* @(/2/2/13/1/1/2/0/1) */
169 return Q_TRAN(&Stepper_Stopped);
173 return Q_SUPER(&Stepper_LinearAccelerator);
175 /* $(AOs::Stepper::Statechart::operational::LinearAccelerator::LinearAccelerate) */
176 QState Stepper_LinearAccelerate(Stepper *me) {
178 /* @(/2/2/13/1/1/3) */
180 (*me->queueStepTime)(me->stepTimer);
181 accelerationStepPeriod(me);
184 /* @(/2/2/13/1/1/3/0) */
185 case NEXT_STEP_TIME_SIG: {
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);
196 /* @(/2/2/13/1/1/3/0/1/1) */
198 me->stepTimer = me->slewTimer;
199 me->decelerationStep =
200 me->totalSlewSteps - me->slewStepCount;
201 return Q_TRAN(&Stepper_LinearConstantSpeed);
204 /* @(/2/2/13/1/1/3/0/0) */
206 return Q_TRAN(&Stepper_LinearDecelerate);
210 return Q_SUPER(&Stepper_LinearAccelerator);
212 /* $(AOs::Stepper::Statechart::operational::LinearAccelerator::LinearConstantSpeed) */
213 QState Stepper_LinearConstantSpeed(Stepper *me) {
215 /* @(/2/2/13/1/1/4) */
217 (*me->queueStepTime)(me->stepTimer);
220 /* @(/2/2/13/1/1/4/0) */
221 case NEXT_STEP_TIME_SIG: {
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);
229 /* @(/2/2/13/1/1/4/0/1) */
231 return Q_TRAN(&Stepper_LinearDecelerate);
235 return Q_SUPER(&Stepper_LinearAccelerator);
237 /* $(AOs::Stepper::Statechart::operational::Stopped) .......................*/
238 QState Stepper_Stopped(Stepper *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);
245 AZIMUTH_CLOCK_DBL / p.dbl;
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);
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
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;
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);
279 /* @(/2/2/13/1/2/5) */
280 case SET_DIRECTION_UP_CW_SIG: {
284 /* @(/2/2/13/1/2/6) */
285 case SET_DIRECTION_DOWN_CCW_SIG: {
290 return Q_SUPER(&Stepper_operational);
295 void initialiseStepperData(Stepper *me)
297 me->basePeriod = (double)SLOW_STEP_PERIOD;
298 me->slewTimer = SLOW_STEP_PERIOD;
299 me->totalSlewSteps = 0;
300 me->accelerationFactor = 0.0;
303 void setupLinearAccelerator(Stepper *me)
305 me->stepPeriod = me->basePeriod;
306 me->stepTimer = (uint32_t)me->stepPeriod;
307 me->slewStepCount = 0;
308 me->decelerationStep = (me->totalSlewSteps + 1) / 2;
311 void accelerationStepPeriod(Stepper *me)
313 me->stepPeriod -= me->accelerationFactor
317 me->stepTimer = (uint32_t)me->stepPeriod;
320 void decelerationStepPeriod(Stepper *me)
322 me->stepPeriod += me->accelerationFactor
326 me->stepTimer = (uint32_t)me->stepPeriod;