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// ramp state-machine states
17enum ramp_state {
18 ramp_idle = 0,
19 ramp_up = 1,
20 ramp_max = 2,
21 ramp_down = 3,
22 ramp_last = 4,
23};
24
25enum ramp_state ramp_sts=ramp_idle;
26int16 motor_pos = 0; // absolute step number
27int16 pos_inc=0; // motor_pos increment
28uint16 phase=0; // ccpPhase[phase_ix]
29uint8 phase_ix=0; // index to ccpPhase[]
30uint8 phase_inc; // phase_ix increment
31uint8 run_flg; // true while motor is running
32// ***************************
33// 1. keep track of total delay count
34uint32 ccpr; // 24.8 fixed point delay count
35uint32 c; // 24.8 fixed point delay count increment
36uint16 step_no; // progress of move
37uint16 step_down; // start of down-ramp
38uint16 move; // total steps to move
39uint16 midpt; // midpoint of move
40int16 denom; // 4.n+1 in ramp algo
41
42// Config data to make CCP1&2 generate quadrature sequence on PHASE pins
43// Action on CCP match: 8=set+irq; 9=clear+irq
44uint16 const ccpPhase[] = {0x909, 0x908, 0x808, 0x809}; // 00,01,11,10
45
46void current_on(){/* code as needed */} // motor drive current
47void current_off(){/* code as needed */} // reduce to holding value
48
49uint16 make16(uint8 l, uint8 r) {
50 return (uint16) l << 8 + r;
51}
52
53// compiler-specific ISR declaration
54
55void __interrupt isr_motor_step(void) { // CCP1 match -> step pulse + IRQ
56 ccpr += c; // next comparator value
57 switch (ramp_sts) {
58 case ramp_up: // accel
59 if (step_no == midpt) { // midpoint: decel
60 ramp_sts = ramp_down;
61 // ***************************
62 // 2. convert shift to multiplication
63 // 3. avoid negative result from subtraction of unsigned values
64 // denom = ((step_no - move) << 2) + 1;
65 if(step_no > move)
66 denom = ((step_no - move) * 4) + 1;
67 else
68 denom = ((move - step_no) * 4) - 1;
69 if (!(move & 1)) { // even move: repeat last delay before decel
70 denom += 4;
71 break;
72 }
73 }
74 // no break: share code for ramp algo
75 case ramp_down: // decel
76 if (step_no == move - 1) { // next irq is cleanup (no step)
77 ramp_sts = ramp_last;
78 break;
79 }
80 denom += 4;
81 // calculate increment/decrement in delay count
82 // ***************************
83 // 3. avoid negative result from subtraction of unsigned values
84 // c -= (c << 1) / denom; // ramp algorithm
85 if(denom > 0)
86 c -= (c << 1) / denom;
87 else
88 c += (c << 1) / -denom;
89
90 if (c <= C_MIN) { // go to constant speed
91 ramp_sts = ramp_max;
92 step_down = move - step_no;
93 c = C_MIN;
94 break;
95 }
96 break;
97 case ramp_max: // constant speed
98 if (step_no == step_down) { // start decel
99 ramp_sts = ramp_down;
100 // ***************************
101 // 2. convert shift to multiplication
102 // 3. avoid negative result from subtraction of unsigned values
103 // denom = ((step_no - move) << 2) + 1;
104 denom = 5 - ((move - step_no) * 4);
105 }
106 break;
107 default: // last step: cleanup
108 ramp_sts = ramp_idle;
109 current_off(); // reduce motor current to holding value
110 CCP1IE = 0; // disable_interrupts(INT_CCP1);
111 run_flg = false; // move complete
112 break;
113 } // switch (ramp_sts)
114 if (ramp_sts != ramp_idle) {
115 motor_pos += pos_inc;
116 ++step_no;
117 CCPR2H = CCPR1H = (ccpr >> 8); // timer value at next CCP match
118 CCPR2L = CCPR1L = (ccpr & 0xff);
119 if (ramp_sts != ramp_last) // else repeat last action: no step
120 phase_ix = (phase_ix + phase_inc) & 3;
121 phase = ccpPhase[phase_ix];
122 CCP1CON = phase & 0xff; // set CCP action on next match
123 CCP2CON = phase >> 8;
124 } // if (ramp_sts != ramp_idle)
125} // isr_motor_step()
126
127void motor_run(int16 pos_new) { // set up to drive motor to pos_new (absolute step#)
128 if (pos_new < motor_pos) { // get direction & #steps
129 move = motor_pos - pos_new;
130 pos_inc = -1;
131 phase_inc = 0xff;
132 }
133 else if (pos_new != motor_pos) {
134 move = pos_new - motor_pos;
135 pos_inc = 1;
136 phase_inc = 1;
137 } else return; // already there
138 midpt = (move - 1) >> 1;
139 c = C0;
140 step_no = 0; // step counter
141 denom = 1; // 4.n+1, n=0
142 ramp_sts = ramp_up; // start ramp state-machine
143 run_flg = true;
144 T1CONbits.TMR1ON = 0; // stop timer1;
145 ccpr = make16(l: TMR1H, r: TMR1L); // 16bit value of Timer1
146 ccpr += 1000; // 1st step + irq 1ms after timer1 restart
147 CCPR2H = CCPR1H = (ccpr >> 8);
148 CCPR2L = CCPR1L = (ccpr & 0xff);
149 phase_ix = (phase_ix + phase_inc) & 3;
150 phase = ccpPhase[phase_ix];
151 CCP1CON = phase & 0xff; // sets action on match
152 CCP2CON = phase >> 8;
153 current_on(); // current in motor windings
154 CCP1IE = 1; // enable_interrupts(INT_CCP1);
155 T1CONbits.TMR1ON = 1; // restart timer1;
156} // motor_run()
157
158void initialize() {
159 di(); // disable_interrupts(GLOBAL);
160 CCP1IE = 0; // disable_interrupts(INT_CCP1);
161 CCP2IE = 0; // disable_interrupts(INT_CCP2);
162 PORTC = 0; // output_c(0);
163 TRISC = 0; // set_tris_c(0);
164 T3CON = 0;
165 T1CON = 0x35;
166 INTCONbits.PEIE = 1;
167 INTCONbits.RBIF = 0;
168 ei(); // enable_interrupts(GLOBAL);
169} // initialize()
170

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