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

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