170 lines
6.0 KiB
C
170 lines
6.0 KiB
C
/*
|
|
* 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)
|
|
*/
|
|
|
|
// ramp state-machine states
|
|
enum ramp_state {
|
|
ramp_idle = 0,
|
|
ramp_up = 1,
|
|
ramp_max = 2,
|
|
ramp_down = 3,
|
|
ramp_last = 4,
|
|
};
|
|
|
|
enum ramp_state ramp_sts=ramp_idle;
|
|
int16 motor_pos = 0; // absolute step number
|
|
int16 pos_inc=0; // motor_pos increment
|
|
uint16 phase=0; // ccpPhase[phase_ix]
|
|
uint8 phase_ix=0; // index to ccpPhase[]
|
|
uint8 phase_inc; // phase_ix increment
|
|
uint8 run_flg; // true while motor is running
|
|
// ***************************
|
|
// 1. keep track of total delay count
|
|
uint32 ccpr; // 24.8 fixed point delay count
|
|
uint32 c; // 24.8 fixed point delay count increment
|
|
uint16 step_no; // progress of move
|
|
uint16 step_down; // start of down-ramp
|
|
uint16 move; // total steps to move
|
|
uint16 midpt; // midpoint of move
|
|
int16 denom; // 4.n+1 in ramp algo
|
|
|
|
// Config data to make CCP1&2 generate quadrature sequence on PHASE pins
|
|
// Action on CCP match: 8=set+irq; 9=clear+irq
|
|
uint16 const ccpPhase[] = {0x909, 0x908, 0x808, 0x809}; // 00,01,11,10
|
|
|
|
void current_on(){/* code as needed */} // motor drive current
|
|
void current_off(){/* code as needed */} // reduce to holding value
|
|
|
|
uint16 make16(uint8 l, uint8 r) {
|
|
return (uint16) l << 8 + r;
|
|
}
|
|
|
|
// compiler-specific ISR declaration
|
|
|
|
void __interrupt isr_motor_step(void) { // CCP1 match -> step pulse + IRQ
|
|
ccpr += c; // next comparator value
|
|
switch (ramp_sts) {
|
|
case ramp_up: // accel
|
|
if (step_no == midpt) { // midpoint: decel
|
|
ramp_sts = ramp_down;
|
|
// ***************************
|
|
// 2. convert shift to multiplication
|
|
// 3. avoid negative result from subtraction of unsigned values
|
|
// denom = ((step_no - move) << 2) + 1;
|
|
if(step_no > move)
|
|
denom = ((step_no - move) * 4) + 1;
|
|
else
|
|
denom = ((move - step_no) * 4) - 1;
|
|
if (!(move & 1)) { // even move: repeat last delay before decel
|
|
denom += 4;
|
|
break;
|
|
}
|
|
}
|
|
// no break: share code for ramp algo
|
|
case ramp_down: // decel
|
|
if (step_no == move - 1) { // next irq is cleanup (no step)
|
|
ramp_sts = ramp_last;
|
|
break;
|
|
}
|
|
denom += 4;
|
|
// calculate increment/decrement in delay count
|
|
// ***************************
|
|
// 3. avoid negative result from subtraction of unsigned values
|
|
// c -= (c << 1) / denom; // ramp algorithm
|
|
if(denom > 0)
|
|
c -= (c << 1) / denom;
|
|
else
|
|
c += (c << 1) / -denom;
|
|
|
|
if (c <= C_MIN) { // go to constant speed
|
|
ramp_sts = ramp_max;
|
|
step_down = move - step_no;
|
|
c = C_MIN;
|
|
break;
|
|
}
|
|
break;
|
|
case ramp_max: // constant speed
|
|
if (step_no == step_down) { // start decel
|
|
ramp_sts = ramp_down;
|
|
// ***************************
|
|
// 2. convert shift to multiplication
|
|
// 3. avoid negative result from subtraction of unsigned values
|
|
// denom = ((step_no - move) << 2) + 1;
|
|
denom = 5 - ((move - step_no) * 4);
|
|
}
|
|
break;
|
|
default: // last step: cleanup
|
|
ramp_sts = ramp_idle;
|
|
current_off(); // reduce motor current to holding value
|
|
CCP1IE = 0; // disable_interrupts(INT_CCP1);
|
|
run_flg = false; // move complete
|
|
break;
|
|
} // switch (ramp_sts)
|
|
if (ramp_sts != ramp_idle) {
|
|
motor_pos += pos_inc;
|
|
++step_no;
|
|
CCPR2H = CCPR1H = (ccpr >> 8); // timer value at next CCP match
|
|
CCPR2L = CCPR1L = (ccpr & 0xff);
|
|
if (ramp_sts != ramp_last) // else repeat last action: no step
|
|
phase_ix = (phase_ix + phase_inc) & 3;
|
|
phase = ccpPhase[phase_ix];
|
|
CCP1CON = phase & 0xff; // set CCP action on next match
|
|
CCP2CON = phase >> 8;
|
|
} // if (ramp_sts != ramp_idle)
|
|
} // isr_motor_step()
|
|
|
|
void motor_run(int16 pos_new) { // set up to drive motor to pos_new (absolute step#)
|
|
if (pos_new < motor_pos) { // get direction & #steps
|
|
move = motor_pos - pos_new;
|
|
pos_inc = -1;
|
|
phase_inc = 0xff;
|
|
}
|
|
else if (pos_new != motor_pos) {
|
|
move = pos_new - motor_pos;
|
|
pos_inc = 1;
|
|
phase_inc = 1;
|
|
} else return; // already there
|
|
midpt = (move - 1) >> 1;
|
|
c = C0;
|
|
step_no = 0; // step counter
|
|
denom = 1; // 4.n+1, n=0
|
|
ramp_sts = ramp_up; // start ramp state-machine
|
|
run_flg = true;
|
|
T1CONbits.TMR1ON = 0; // stop timer1;
|
|
ccpr = make16(TMR1H, TMR1L); // 16bit value of Timer1
|
|
ccpr += 1000; // 1st step + irq 1ms after timer1 restart
|
|
CCPR2H = CCPR1H = (ccpr >> 8);
|
|
CCPR2L = CCPR1L = (ccpr & 0xff);
|
|
phase_ix = (phase_ix + phase_inc) & 3;
|
|
phase = ccpPhase[phase_ix];
|
|
CCP1CON = phase & 0xff; // sets action on match
|
|
CCP2CON = phase >> 8;
|
|
current_on(); // current in motor windings
|
|
CCP1IE = 1; // enable_interrupts(INT_CCP1);
|
|
T1CONbits.TMR1ON = 1; // restart timer1;
|
|
} // motor_run()
|
|
|
|
void initialize() {
|
|
di(); // disable_interrupts(GLOBAL);
|
|
CCP1IE = 0; // disable_interrupts(INT_CCP1);
|
|
CCP2IE = 0; // disable_interrupts(INT_CCP2);
|
|
PORTC = 0; // output_c(0);
|
|
TRISC = 0; // set_tris_c(0);
|
|
T3CON = 0;
|
|
T1CON = 0x35;
|
|
INTCONbits.PEIE = 1;
|
|
INTCONbits.RBIF = 0;
|
|
ei(); // enable_interrupts(GLOBAL);
|
|
} // initialize()
|