app/spejbl_motor/hbridge.c

   1 #include "hbridge.h"
   2 #include "pwm.h"
   3
   4 void hbridge_half_set(uint8_t side, uint32_t value) {
   5   uint32_t *s_down, *d_down, *d_up;
   6   uint32_t t;
   7
   8   if (side) {
   9     s_down = PWM_MR[6];  d_down = PWM_MR[4];  d_up = PWM_MR[3];
  10   }
  11   else {
  12     s_down = PWM_MR[5];  d_down = PWM_MR[2];  d_up = PWM_MR[1];
  13   }
  14
  15   if (value < 2*PWM_DEAD) {
  16     /* 0% */
  17     *s_down = PWM_PERIOD;
  18     *d_down = PWM_PERIOD+1;
  19     *d_up = 0;
  20   }
  21   else if (value > PWM_PERIOD-2*PWM_DEAD) {
  22     /* 100% */
  23     *s_down = *d_down = 0;
  24     *d_up = PWM_PERIOD;
  25   }
  26   else {
  27     *d_up = t = PWM_PERIOD-PWM_DEAD;
  28     //*d_down = t -= value;
  29     /****** !!!!!!!!! *******/
  30     *d_down = t -= value - 2*PWM_DEAD;
  31     *s_down = t - PWM_DEAD;
  32   }
  33
  34   PWMLER |= side ? 0x58 : 0x26;
  35 }
  36
  37 void hbridge_set(int32_t value) {
  38   if (value >= 0) {
  39     hbridge_half_set(0, value);
  40     hbridge_half_set(1, 0);
  41   }
  42   else {
  43     hbridge_half_set(1, -value);
  44     hbridge_half_set(0, 0);
  45   }
  46 }
  47
  48 void hbridge_off() {
  49   /* *s_down = 0 (L) */
  50   PWMMR5 = 0;
  51   PWMMR6 = 0;
  52   /* *d_down = PWM_PERIOD+1, *d_up = 0 (H) */
  53   PWMMR4 = PWM_PERIOD+1;
  54   PWMMR3 = 0;
  55   PWMMR2 = PWM_PERIOD+1;
  56   PWMMR1 = 0;
  57   PWMLER |= 0x58 | 0x26;
  58 }
  59
  60 void hbridge_init() {
  61   /* PWM2,4 double-edged, PWM5,6 single-edged */
  62   pwm_channel(2, 1);
  63   pwm_channel(4, 1);
  64   pwm_channel(5, 0);
  65   pwm_channel(6, 0);
  66   /* both sides to GND */
  67   //hbridge_half_set(0, 0);
  68   //hbridge_half_set(1, 0);
  69   /* disconnect the bridge */
  70   hbridge_off();
  71   /* go */
  72   pwm_init(1, PWM_PERIOD);
  73 }