lline/motor.c

   1 /*
   2         Low level motor control via timer based pwm.
   3 */
   4 #include "motor.h"
   5 #include "math.h"
   6
   7 #include <stdbool.h>
   8 #include <avr/io.h>
   9 #include <avr/power.h>
  10 #include <util/delay.h>
  11
  12 // Register usage
  13 #define USE_OC1A 0
  14 #define USE_OC1B 1
  15 #define USE_OC1D 1
  16
  17 // Prescale selection
  18 #define MOTOR_T1_PRESCALE 1
  19
  20 struct pwm_param_s {
  21   uint8_t use_oc1a:1;
  22   uint8_t use_oc1b:1;
  23   uint8_t use_oc1d:1;
  24   uint8_t t1_prescale:5;
  25 } const pwm_param = { USE_OC1A, USE_OC1B, USE_OC1D, MOTOR_T1_PRESCALE };
  26
  27 static void motor_pwm10_init(struct pwm_param_s param);
  28 static inline void motor_pins(motor_t *motor, uint8_t dir1, uint8_t dir2);
  29 static inline void motor_pin(motor_t *motor, uint8_t pin, uint8_t dir);
  30 static inline void motor_pin_h(motor_t *motor, uint8_t pin);
  31 static inline void motor_pin_l(motor_t *motor, uint8_t pin);
  32
  33 void motor_init(void) {
  34         motor_pwm10_init(pwm_param);
  35 }
  36
  37 #define T1_PRESCALE_BITS(prescale)      \
  38         ((uint8_t)(  0x0F & (uint8_t)(( log(prescale) + log(2) )/log(2)) ))
  39
  40 static void motor_pwm10_init(const struct pwm_param_s param) {
  41         // Initialize a 10 bit pwm (as on the attinyX61 series)
  42         //   All done by the datasheet.
  43
  44         power_timer1_enable();
  45
  46         PLLCSR = (1<<PLLE);
  47         _delay_us(100);
  48         loop_until_bit_is_set(PLLCSR,PLOCK);
  49         PLLCSR |= (1<<PCKE);
  50
  51         /*
  52
  53         Compare Output Mode, Phase and Frequency Correct PWM Mode
  54         COM1x1..0  OCW1x Behaviour                         OC1x  !OC1x Pin
  55         00     Normal port operation.                        o     o
  56
  57         01     Cleared on Compare Match when up-counting.    x     x
  58                   Set on Compare Match when down-counting.
  59
  60         10     Cleared on Compare Match when up-counting.    x     o
  61                   Set on Compare Match when down-counting.
  62
  63         11     Set on Compare Match when up-counting.        x     o
  64                   Cleared on Compare Match when down-counting.
  65         (x=connected,o=disconnected)
  66         */
  67         /*
  68                                                                  Update of TOV1 Flag
  69         PWM1x WGM11..10 Timer/Counter Mode of Operation TOP   OCR1x at  Set on
  70           0     xx      Normal                          OCR1C Immediate TOP
  71           1     00      Fast PWM                        OCR1C TOP       TOP
  72           1     01      Phase and Frequency Correct PWM OCR1C BOTTOM    BOTTOM
  73           1     10      PWM6 / Single-slope             OCR1C TOP       TOP
  74           1     11      PWM6 / Dual-slope               OCR1C BOTTOM    BOTTOM
  75
  76         */
  77
  78         TCCR1A = ((param.use_oc1a)<<COM1A1) | (0<<COM1A0) | // Output pin modes (OCR1A)
  79                 ((param.use_oc1b)<<COM1B1) | (0<<COM1B0) | // ^^               (OCR1B)
  80                 (       0<<FOC1A ) | (0<<FOC1B ) | // Force output compare
  81                 ((param.use_oc1a)<<PWM1A ) |
  82                 ((param.use_oc1b)<<PWM1B ) ;
  83
  84         /*
  85         TCCR1B = (0<<PWM1X ) | // PWM inversion (switch !OCR1x and OCR1x)
  86                 (0<<PSR1  ) | // Prescaler reset, cleared by hw
  87                 (0<<DTPS11) | (0<<DTPS10); // Dead time prescaler bits
  88                 //CS1{3,2,1,0}: prescale bits
  89         */
  90
  91         TCCR1C = ((param.use_oc1a)<<COM1A1S)|(0<<COM1A0S) | // Shadow bits
  92                 ((param.use_oc1b))|(0<<COM1B0S) | // ^^
  93                 ((param.use_oc1d)<<COM1D1) | (0<<COM1D0) | // Output pin modes D
  94                 (0<<FOC1D ) | // Force output compare
  95                 ((param.use_oc1d)<<PWM1D ) ;
  96
  97         /*
  98         TCCR1D = (0<<FPIE1) | // Fault protection interrupt enable
  99                 (0<<FPEN1) | // FP enable
 100                 (0<<FPNC1) | // FP noise canceller
 101                 (0<<FPES1) | // FP edge select
 102                 (0<<FPAC1) | // FP analog comparitor enable
 103                 (0<<FPF1 ) | // FP interrupt flag
 104                 (0<<WGM11) | (1<<WGM10); // Waveform select
 105         */
 106         TCCR1D |= (1<<WGM10);
 107
 108         /*
 109         TCCR1E = (0<<7) | (0<<6) | // Reserved (on attiny861)
 110                 // Output compare override enable (PWM6). OC1OEx = PBx
 111                 (0<<OC1OE5) | (0<<OC1OE4) | (0<<OC1OE3) |
 112                 (0<<OC1OE2) | (0<<OC1OE1) | (0<<OC1OE0) ;
 113         */
 114
 115         // Shared with T0
 116         //TIMSK &= (uint8_t) ~((0<<OCIE1D) | (0<<OCIE1A) | (0<<OCIE1B) | (0<<TOIE1));
 117         //TIFR  |= (1<<OCF1D ) | (1<<OCF1A ) | (1<<OCF1B ) | (1<<TOV1 );
 118
 119         //TC1H  = 0;
 120         //TCNT1 = 0;
 121         //OCR1A = 0;
 122         //OCR1B = 0;
 123         //OCR1D = 0;
 124
 125         TC1H  = 0b11;
 126         OCR1C = 0xFF; // Top
 127
 128         TCCR1B |= T1_PRESCALE_BITS(param.t1_prescale);
 129 }
 130
 131 /*
 132  * 1 2 Motor lines
 133  * L H CCW
 134  * H L CW
 135  * H H Short break (??)
 136  * L L Stop
 137  */
 138
 139 #define LIMIT(var,limit) \
 140   if ( (var) > (limit) ) \
 141     var = limit; \
 142   else if ( (var) < -(limit) ) \
 143     var = -limit
 144
 145 typedef enum pin_state_e { l=false, h=true } pin_state_t;
 146 void motor_set(motor_t *motor, motor_speed_t speed) {
 147         /*
 148         motor_t curr_motor = motor_list[motor];
 149         curr_motor.set_speed(&curr_motor,abs(speed));
 150         curr_motor.set_dir(&curr_motor,sign(speed));
 151         */
 152         motor_uspeed_t abs_speed = (motor_uspeed_t) speed;
 153         if (abs_speed > MOTOR_SPEED_MAX) abs_speed=MOTOR_SPEED_MAX;
 154
 155         // atomic
 156         *(motor->reg_pwmh) = (abs_speed >> 8);
 157         *(motor->reg_pwm)  = (0x00FF & abs_speed);
 158
 159         if      (speed > 0) {
 160                 motor_pins(motor,h,l);
 161         }
 162         else if (speed < 0) {
 163                 motor_pins(motor,l,h);
 164         }
 165         else { //if (speed == 0)
 166                 motor_pins(motor,l,l);
 167         }
 168 }
 169
 170 motor_speed_t motor_get(motor_t *motor) {
 171         // atomic
 172         motor_speed_t sp = *(motor->reg_pwm);
 173         sp += *(motor->reg_pwmh) << 8;
 174
 175         return sp;
 176 }
 177
 178 static inline void motor_pins(motor_t *motor, pin_state_t dir1, pin_state_t dir2) {
 179   motor_pin(motor,1,dir1);
 180   motor_pin(motor,2,dir2);
 181 }
 182
 183 static inline void motor_pin(motor_t *motor, uint8_t pin, pin_state_t dir) {
 184   if (dir == h)
 185     motor_pin_h(motor,pin);
 186   else //if (dir==l)
 187     motor_pin_l(motor,pin);
 188 }
 189
 190 static inline void motor_pin_h(motor_t *motor, pin_state_t pin) {
 191   /*
 192   volatile uint8_t * const port = motor->pin[pin-1].port;
 193   const uint8_t mask  = motor->pin[pin-1].mask;
 194   *port |= mask;
 195   */
 196   *(motor->pin[pin-1].port) |= motor->pin[pin-1].mask;
 197 }
 198
 199 static inline void motor_pin_l(motor_t *motor, pin_state_t pin) {
 200   *(motor->pin[pin-1].port) &= (uint8_t) ~(motor->pin[pin-1].mask);
 201 }
 202