EQ2EMu/EQ2/source/depends/boost_1_72_0/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()