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
17 enum ramp_state {
18     ramp_idle = 0,
19     ramp_up = 1,
20     ramp_max = 2,
21     ramp_down = 3,
22     ramp_last = 4,
23 };
24 
25 enum ramp_state ramp_sts=ramp_idle;
26 int16 motor_pos = 0;    // absolute step number
27 int16 pos_inc=0;        // motor_pos increment
28 uint16 phase=0;         // ccpPhase[phase_ix]
29 uint8  phase_ix=0;      // index to ccpPhase[]
30 uint8  phase_inc;       // phase_ix increment
31 uint8  run_flg;         // true while motor is running
32 uint16 ccpr;            // copy of CCPR1&2
33 uint16 c;               // integer delay count
34 uint16 step_no;          // progress of move
35 uint16 step_down;       // start of down-ramp
36 uint16 move;             // total steps to move
37 uint16 midpt;           // midpoint of move
38 uint32 c32;              // 24.8 fixed point delay count
39 int16 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
43 uint16 const ccpPhase[] = {0x909, 0x908, 0x808, 0x809}; // 00,01,11,10
44 
current_on()45 void current_on(){/* code as needed */}  // motor drive current
current_off()46 void current_off(){/* code as needed */} // reduce to holding value
47 
make16(uint8 l,uint8 r)48 uint16 make16(uint8 l, uint8 r) {
49     return (uint16) l << 8 + r;
50 }
51 
52 // compiler-specific ISR declaration
53 
isr_motor_step(void)54 void __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 
motor_run(int16 pos_new)109 void 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 
initialize()141 void 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