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