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

   1 // Demo program for stepper motor control with linear ramps
   2 // Hardware: PIC18F252, L6219
   3 #include "18F252.h"
   4
   5 // PIC18F252 SFRs
   6 #byte TRISC   = 0xf94
   7 #byte T3CON   = 0xfb1
   8 #byte CCP2CON = 0xfba
   9 #byte CCPR2L  = 0xfbb
  10 #byte CCPR2H  = 0xfbc
  11 #byte CCP1CON = 0xfbd
  12 #byte CCPR1L  = 0xfbe
  13 #byte CCPR1H  = 0xfbf
  14 #byte T1CON   = 0xfcd
  15 #byte TMR1L   = 0xfce
  16 #byte TMR1H   = 0xfcf
  17 #bit  TMR1ON  = T1CON.0
  18
  19 // 1st step=50ms; max speed=120rpm (based on 1MHz timer, 1.8deg steps)
  20 #define C0    50000
  21 #define C_MIN  2500
  22
  23 // ramp state-machine states
  24 #define ramp_idle 0
  25 #define ramp_up   1
  26 #define ramp_max  2
  27 #define ramp_down 3
  28 #define ramp_last 4
  29
  30 // Types: int8,int16,int32=8,16,32bit integers, unsigned by default
  31 int8  ramp_sts=ramp_idle;
  32 signed int16 motor_pos = 0; // absolute step number
  33 signed int16 pos_inc=0;     // motor_pos increment
  34 int16 phase=0;    // ccpPhase[phase_ix]
  35 int8  phase_ix=0; // index to ccpPhase[]
  36 int8  phase_inc;  // phase_ix increment
  37 int8  run_flg;    // true while motor is running
  38 int16 ccpr;       // copy of CCPR1&2
  39 int16 c;          // integer delay count
  40 int16 step_no;    // progress of move
  41 int16 step_down;  // start of down-ramp
  42 int16 move;       // total steps to move
  43 int16 midpt;      // midpoint of move
  44 int32 c32;        // 24.8 fixed point delay count
  45 signed int16 denom; // 4.n+1 in ramp algo
  46
  47 // Config data to make CCP1&2 generate quadrature sequence on PHASE pins
  48 // Action on CCP match: 8=set+irq; 9=clear+irq
  49 int16 const ccpPhase[] = {0x909, 0x908, 0x808, 0x809}; // 00,01,11,10
  50
  51 void current_on(){/* code as needed */}  // motor drive current
  52 void current_off(){/* code as needed */} // reduce to holding value
  53
  54 // compiler-specific ISR declaration
  55 #INT_CCP1
  56 void isr_motor_step()
  57 { // CCP1 match -> step pulse + IRQ
  58   ccpr += c; // next comparator value: add step delay count
  59   switch (ramp_sts)
  60   {
  61   case ramp_up:   // accel
  62     if (step_no==midpt)
  63     { // midpoint: decel
  64       ramp_sts = ramp_down;
  65       denom = ((step_no - move)<<2)+1;
  66       if (!(move & 1))
  67       { // even move: repeat last delay before decel
  68         denom +=4;
  69         break;
  70       }
  71     }
  72     // no break: share code for ramp algo
  73   case ramp_down: // decel
  74     if (step_no == move-1)
  75     { // next irq is cleanup (no step)
  76       ramp_sts = ramp_last;
  77       break;
  78     }
  79     denom+=4;
  80     c32 -= (c32<<1)/denom; // ramp algorithm
  81     // beware confict with foreground code if long div not reentrant
  82     c = (c32+128)>>8; // round 24.8format->int16
  83     if (c <= C_MIN)
  84     { // go to constant speed
  85       ramp_sts = ramp_max;
  86       step_down = move - step_no;
  87       c = C_MIN;
  88       break;
  89     }
  90     break;
  91   case ramp_max: // constant speed
  92     if (step_no == step_down)
  93     { // start decel
  94       ramp_sts = ramp_down;
  95       denom = ((step_no - move)<<2)+5;
  96     }
  97     break;
  98   default: // last step: cleanup
  99     ramp_sts = ramp_idle;
 100     current_off(); // reduce motor current to holding value
 101     disable_interrupts(INT_CCP1);
 102     run_flg = FALSE; // move complete
 103     break;
 104   } // switch (ramp_sts)
 105   if (ramp_sts!=ramp_idle)
 106   {
 107     motor_pos += pos_inc;
 108     ++step_no;
 109     CCPR2H = CCPR1H = (ccpr >> 8); // timer value at next CCP match
 110     CCPR2L = CCPR1L = (ccpr & 0xff);
 111     if (ramp_sts!=ramp_last) // else repeat last action: no step
 112             phase_ix = (phase_ix + phase_inc) & 3;
 113     phase = ccpPhase[phase_ix];
 114     CCP1CON = phase & 0xff; // set CCP action on next match
 115     CCP2CON = phase >> 8;
 116   } // if (ramp_sts != ramp_idle)
 117 } // isr_motor_step()
 118
 119 void motor_run(short pos_new)
 120 { // set up to drive motor to pos_new (absolute step#)
 121   if (pos_new < motor_pos) // get direction & #steps
 122   {
 123     move = motor_pos-pos_new;
 124     pos_inc   = -1;
 125     phase_inc = 0xff;
 126   }
 127   else if (pos_new != motor_pos)
 128   {
 129     move = pos_new-motor_pos;
 130     pos_inc   = 1;
 131     phase_inc = 1;
 132   }
 133   else return; // already there
 134   midpt = (move-1)>>1;
 135   c   = C0;
 136   c32 = c<<8; // keep c in 24.8 fixed-point format for ramp calcs
 137   step_no  = 0; // step counter
 138   denom    = 1; // 4.n+1, n=0
 139   ramp_sts = ramp_up; // start ramp state-machine
 140   run_flg  = TRUE;
 141   TMR1ON   = 0; // stop timer1;
 142   ccpr = make16(TMR1H,TMR1L);  // 16bit value of Timer1
 143   ccpr += 1000; // 1st step + irq 1ms after timer1 restart
 144   CCPR2H = CCPR1H = (ccpr >> 8);
 145   CCPR2L = CCPR1L = (ccpr & 0xff);
 146   phase_ix = (phase_ix + phase_inc) & 3;
 147   phase = ccpPhase[phase_ix];
 148   CCP1CON = phase & 0xff; // sets action on match
 149   CCP2CON = phase >> 8;
 150   current_on(); // current in motor windings
 151   enable_interrupts(INT_CCP1);
 152   TMR1ON=1; // restart timer1;
 153 } // motor_run()
 154
 155
 156 void initialize()
 157 {
 158   disable_interrupts(GLOBAL);
 159   disable_interrupts(INT_CCP1);
 160   disable_interrupts(INT_CCP2);
 161   output_c(0);
 162   set_tris_c(0);
 163   T3CON = 0;
 164   T1CON = 0x35;
 165   enable_interrupts(GLOBAL);
 166 } // initialize()
 167
 168 void main()
 169 {
 170         initialize();
 171         while (1)
 172         { // repeat 5 revs forward & back
 173         motor_run(1000);
 174         while (run_flg);
 175         motor_run(0);
 176         while (run_flg);
 177         }
 178 } // main()
 179 // end of file motor.c