projects / ceph.git / blame
| Commit | Line | Data |
|---|---|---|
| 92f5a8d4 TL |
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 | ||
| 45 | void current_on(){/* code as needed */} // motor drive current | |
| 46 | void current_off(){/* code as needed */} // reduce to holding value | |
| 47 | ||
| 48 | uint16 make16(uint8 l, uint8 r) { | |
| 20effc67 | 49 | return (uint16) (l << 8) + r; |
| 92f5a8d4 TL |
50 | } |
| 51 | ||
| 52 | // compiler-specific ISR declaration | |
| 53 | ||
| 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 | ||
| 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 | ||
| 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() |