ceph/src/boost/libs/safe_numerics/example/motor3.c

   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 #include <assert.h>
  17
  18 // ramp state-machine states
  19 enum ramp_state {
  20     ramp_idle = 0,
  21     ramp_up = 1,
  22     ramp_const = 2,
  23     ramp_down = 3,
  24 };
  25
  26 // ***************************
  27 // 1. Define state variables using custom strong types
  28
  29 // initial setup
  30 enum ramp_state ramp_sts;
  31 position_t motor_position;
  32 position_t m;           // target position
  33 position_t m2;          // midpoint or point where acceleration changes
  34 direction_t d;          // direction of traval -1 or +1
  35
  36 // curent state along travel
  37 step_t i;               // step number
  38 c_t c;                  // 24.8 fixed point delay count increment
  39 ccpr_t ccpr;            // 24.8 fixed point delay count
  40 phase_ix_t phase_ix;    // motor phase index
  41
  42 // ***************************
  43 // 2. Surround all literal values with the "literal" keyword
  44
  45 // Config data to make CCP1&2 generate quadrature sequence on PHASE pins
  46 // Action on CCP match: 8=set+irq; 9=clear+irq
  47 phase_t const ccpPhase[] = {
  48     literal(0x909),
  49     literal(0x908),
  50     literal(0x808),
  51     literal(0x809)
  52 }; // 00,01,11,10
  53
  54 void current_on(){/* code as needed */}  // motor drive current
  55 void current_off(){/* code as needed */} // reduce to holding value
  56
  57 // ***************************
  58 // 3. Refactor code to make it easier to understand
  59 // and relate to the documentation
  60
  61 bool busy(){
  62     return ramp_idle != ramp_sts;
  63 }
  64
  65 // set outputs to energize motor coils
  66 void update(ccpr_t ccpr, phase_ix_t phase_ix){
  67     // energize correct windings
  68     const phase_t phase = ccpPhase[phase_ix];
  69     CCP1CON = phase & literal(0xff); // set CCP action on next match
  70     CCP2CON = phase >> literal(8);
  71     // timer value at next CCP match
  72     CCPR1H = literal(0xff) & (ccpr >> literal(8));
  73     CCPR1L = literal(0xff) & ccpr;
  74 }
  75
  76 // compiler-specific ISR declaration
  77 // ***************************
  78 // 4. Rewrite interrupt handler in a way which mirrors the orginal
  79 // description of the algorithm and minimizes usage of state variable,
  80 // accumulated values, etc.
  81 void __interrupt isr_motor_step(void) { // CCP1 match -> step pulse + IRQ
  82     // *** possible exception
  83     // motor_position += d;
  84     // use the following to avoid mixing exception policies which is an error
  85     if(d < 0)
  86         --motor_position;
  87     else
  88         ++motor_position;
  89     // *** possible exception
  90     ++i;
  91     // calculate next difference in time
  92     for(;;){
  93         switch (ramp_sts) {
  94             case ramp_up: // acceleration
  95                 if (i == m2) {
  96                     ramp_sts = ramp_down;
  97                     continue;
  98                 }
  99                 // equation 13
 100                 // *** possible negative overflow on update of c
 101                 c -= literal(2) * c / (literal(4) * i + literal(1));
 102                 if(c < C_MIN){
 103                     c = C_MIN;
 104                     ramp_sts = ramp_const;
 105                     // *** possible exception
 106                     m2 = m - i; // new inflection point
 107                     continue;
 108                 }
 109                 break;
 110             case ramp_const: // constant speed
 111                 if(i > m2) {
 112                     ramp_sts = ramp_down;
 113                     continue;
 114                 }
 115                 break;
 116             case ramp_down: // deceleration
 117                 if (i == m) {
 118                     ramp_sts = ramp_idle;
 119                     current_off(); // reduce motor current to holding value
 120                     CCP1IE = literal(0); // disable_interrupts(INT_CCP1);
 121                     return;
 122                 }
 123                 // equation 14
 124                 // *** possible positive overflow on update of c
 125                 // note: re-arrange expression to avoid negative result
 126                 // from difference of two unsigned values
 127                 c += literal(2) * c / (literal(4) * (m - i) - literal(1));
 128                 if(c > C0){
 129                     c = C0;
 130                 }
 131                 break;
 132             default:
 133                 // should never arrive here!
 134                 assert(false);
 135         } // switch (ramp_sts)
 136         break;
 137     }
 138     assert(c <= C0 && c >= C_MIN);
 139     // *** possible exception
 140     ccpr = literal(0xffffff) & (ccpr + c);
 141     phase_ix = (phase_ix + d) & literal(3);
 142     update(ccpr, phase_ix);
 143 } // isr_motor_step()
 144
 145 // set up to drive motor to pos_new (absolute step#)
 146 void motor_run(position_t new_position) {
 147     if(new_position > motor_position){
 148         d = literal(1);
 149         // *** possible exception
 150         m = new_position - motor_position;
 151     }
 152     else
 153     if(motor_position > new_position){
 154         d = literal(-1);
 155         // *** possible exception
 156         m = motor_position - new_position;
 157     }
 158     else{
 159         d = literal(0);
 160         m = literal(0);
 161         ramp_sts = ramp_idle; // start ramp state-machine
 162         return;
 163     }
 164
 165     i = literal(0);
 166     m2 = m / literal(2);
 167
 168     ramp_sts = ramp_up; // start ramp state-machine
 169
 170     T1CONbits.TMR1ON = literal(0); // stop timer1;
 171
 172     current_on(); // current in motor windings
 173
 174     c = C0;
 175     ccpr = (TMR1H << literal(8) | TMR1L) + C0 + literal(1000);
 176     phase_ix = d & literal(3);
 177     update(ccpr, phase_ix);
 178
 179     CCP1IE = literal(1); // enable_interrupts(INT_CCP1);
 180     T1CONbits.TMR1ON = literal(1); // restart timer1;
 181 } // motor_run()
 182
 183 void initialize() {
 184     di();                // disable_interrupts(GLOBAL);
 185     motor_position = literal(0);
 186     CCP1IE = literal(0); // disable_interrupts(INT_CCP1);
 187     CCP2IE = literal(0); // disable_interrupts(INT_CCP2);
 188     PORTC = literal(0);  // output_c(0);
 189     TRISC = literal(0);  // set_tris_c(0);
 190     T3CON = literal(0);
 191     T1CON = literal(0x35);
 192     INTCONbits.PEIE = literal(1);
 193     INTCONbits.RBIF = literal(0);
 194     ei();                // enable_interrupts(GLOBAL);
 195 } // initialize()