libs/safe_numerics/example/motor2.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()