1/*
2 * david austin
3 * http://www.embedded.com/design/mcus-processors-and-socs/4006438/Generate-stepper-motor-speed-profiles-in-real-time
4 * DECEMBER 30, 2004
5 *
6 * Demo program for stepper motor control with linear ramps
7 * Hardware: PIC18F252, L6219
8 *
9 * Copyright (c) 2015 Robert Ramey
10 *
11 * Distributed under the Boost Software License, Version 1.0. (See
12 * accompanying file LICENSE_1_0.txt or copy at
13 * http://www.boost.org/LICENSE_1_0.txt)
14 */
15
16#include <assert.h>
17
18// ramp state-machine states
19enum ramp_state {
20 ramp_idle = 0,
21 ramp_up = 1,
22 ramp_const = 2,
23 ramp_down = 3,
24};
25
26// ***************************
27// 1. Define state variables using custom strong types
28
29// initial setup
30enum ramp_state ramp_sts;
31position_t motor_position;
32position_t m; // target position
33position_t m2; // midpoint or point where acceleration changes
34direction_t d; // direction of traval -1 or +1
35
36// curent state along travel
37step_t i; // step number
38c_t c; // 24.8 fixed point delay count increment
39ccpr_t ccpr; // 24.8 fixed point delay count
40phase_ix_t phase_ix; // motor phase index
41
42// ***************************
43// 2. Surround all literal values with the "literal" keyword
44
45// Config data to make CCP1&2 generate quadrature sequence on PHASE pins
46// Action on CCP match: 8=set+irq; 9=clear+irq
47phase_t const ccpPhase[] = {
48 literal(0x909),
49 literal(0x908),
50 literal(0x808),
51 literal(0x809)
52}; // 00,01,11,10
53
54void current_on(){/* code as needed */} // motor drive current
55void current_off(){/* code as needed */} // reduce to holding value
56
57// ***************************
58// 3. Refactor code to make it easier to understand
59// and relate to the documentation
60
61bool busy(){
62 return ramp_idle != ramp_sts;
63}
64
65// set outputs to energize motor coils
66void update(ccpr_t ccpr_arg, phase_ix_t phase_ix_arg){
67 // energize correct windings
68 const phase_t phase = ccpPhase[phase_ix_arg];
69 CCP1CON = phase & literal(0xff); // set CCP action on next match
70 CCP2CON = phase >> literal(8);
71 // timer value at next CCP match
72 CCPR1H = literal(0xff) & (ccpr_arg >> literal(8));
73 CCPR1L = literal(0xff) & ccpr_arg;
74}
75
76// compiler-specific ISR declaration
77// ***************************
78// 4. Rewrite interrupt handler in a way which mirrors the orginal
79// description of the algorithm and minimizes usage of state variable,
80// accumulated values, etc.
81void __interrupt isr_motor_step(void) { // CCP1 match -> step pulse + IRQ
82 // *** possible exception
83 // motor_position += d;
84 // use the following to avoid mixing exception policies which is an error
85 if(d < 0)
86 --motor_position;
87 else
88 ++motor_position;
89 // *** possible exception
90 ++i;
91 // calculate next difference in time
92 for(;;){
93 switch (ramp_sts) {
94 case ramp_up: // acceleration
95 if (i == m2) {
96 ramp_sts = ramp_down;
97 continue;
98 }
99 // equation 13
100 // *** possible negative overflow on update of c
101 c -= literal(2) * c / (literal(4) * i + literal(1));
102 if(c < C_MIN){
103 c = C_MIN;
104 ramp_sts = ramp_const;
105 // *** possible exception
106 m2 = m - i; // new inflection point
107 continue;
108 }
109 break;
110 case ramp_const: // constant speed
111 if(i > m2) {
112 ramp_sts = ramp_down;
113 continue;
114 }
115 break;
116 case ramp_down: // deceleration
117 if (i == m) {
118 ramp_sts = ramp_idle;
119 current_off(); // reduce motor current to holding value
120 CCP1IE = literal(0); // disable_interrupts(INT_CCP1);
121 return;
122 }
123 // equation 14
124 // *** possible positive overflow on update of c
125 // note: re-arrange expression to avoid negative result
126 // from difference of two unsigned values
127 {
128 // testing discovered that this can overflow. It's not easy to
129 // avoid so we'll use a temporary unsigned variable 32 bits wide
130 const temp_t x = c + literal(2) * c / (literal(4) * (m - i) - literal(1));
131 c = x > C0 ? C0 : x;
132 }
133 break;
134 default:
135 // should never arrive here!
136 assert(false);
137 } // switch (ramp_sts)
138 break;
139 }
140 assert(c <= C0 && c >= C_MIN);
141 // *** possible exception
142 ccpr = literal(0xffffff) & (ccpr + c);
143 phase_ix = (phase_ix + d) & literal(3);
144 update(ccpr_arg: ccpr, phase_ix_arg: phase_ix);
145} // isr_motor_step()
146
147// set up to drive motor to pos_new (absolute step#)
148void motor_run(position_t new_position) {
149 if(new_position > motor_position){
150 d = literal(1);
151 // *** possible exception
152 m = new_position - motor_position;
153 }
154 else
155 if(motor_position > new_position){
156 d = literal(-1);
157 // *** possible exception
158 m = motor_position - new_position;
159 }
160 else{
161 d = literal(0);
162 m = literal(0);
163 ramp_sts = ramp_idle; // start ramp state-machine
164 return;
165 }
166
167 i = literal(0);
168 m2 = m / literal(2);
169
170 ramp_sts = ramp_up; // start ramp state-machine
171
172 T1CONbits.TMR1ON = literal(0); // stop timer1;
173
174 current_on(); // current in motor windings
175
176 c = C0;
177 ccpr = (TMR1H << literal(8) | TMR1L) + C0 + literal(1000);
178 phase_ix = d & literal(3);
179 update(ccpr_arg: ccpr, phase_ix_arg: phase_ix);
180
181 CCP1IE = literal(1); // enable_interrupts(INT_CCP1);
182 T1CONbits.TMR1ON = literal(1); // restart timer1;
183} // motor_run()
184
185void initialize() {
186 di(); // disable_interrupts(GLOBAL);
187 motor_position = literal(0);
188 CCP1IE = literal(0); // disable_interrupts(INT_CCP1);
189 CCP2IE = literal(0); // disable_interrupts(INT_CCP2);
190 PORTC = literal(0); // output_c(0);
191 TRISC = literal(0); // set_tris_c(0);
192 T3CON = literal(0);
193 T1CON = literal(0x35);
194 INTCONbits.PEIE = literal(1);
195 INTCONbits.RBIF = literal(0);
196 ei(); // enable_interrupts(GLOBAL);
197} // initialize()
198

source code of boost/libs/safe_numerics/example/motor3.c