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
19 enum 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
30 enum ramp_state ramp_sts;
31 position_t motor_position;
32 position_t m; // target position
33 position_t m2; // midpoint or point where acceleration changes
34 direction_t d; // direction of traval -1 or +1
35
36 // curent state along travel
37 step_t i; // step number
38 c_t c; // 24.8 fixed point delay count increment
39 ccpr_t ccpr; // 24.8 fixed point delay count
40 phase_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
47 phase_t const ccpPhase[] = {
48 literal(0x909),
49 literal(0x908),
50 literal(0x808),
51 literal(0x809)
52 }; // 00,01,11,10
53
current_on()54 void current_on(){/* code as needed */} // motor drive current
current_off()55 void 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
busy()61 bool busy(){
62 return ramp_idle != ramp_sts;
63 }
64
65 // set outputs to energize motor coils
update(ccpr_t ccpr,phase_ix_t phase_ix)66 void update(ccpr_t ccpr, phase_ix_t phase_ix){
67 // energize correct windings
68 const phase_t phase = ccpPhase[phase_ix];
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 >> literal(8));
73 CCPR1L = literal(0xff) & ccpr;
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.
isr_motor_step(void)81 void __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, phase_ix);
145 } // isr_motor_step()
146
147 // set up to drive motor to pos_new (absolute step#)
motor_run(position_t new_position)148 void 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, phase_ix);
180
181 CCP1IE = literal(1); // enable_interrupts(INT_CCP1);
182 T1CONbits.TMR1ON = literal(1); // restart timer1;
183 } // motor_run()
184
initialize()185 void 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