app/spejbl_motor/motorx.c

   1 #include <stdlib.h>
   2 #include <types.h>
   3 #include <limits.h>
   4 #include <lpc21xx.h>
   5 #include <cpu_def.h>
   6 /* CAN definitions*/
   7 #include <can/canmsg.h>
   8 #include <can/lpcan.h>
   9 #include <can/lpcan_vca.h>
  10 //
  11 #include <string.h>
  12 #include "led.h"
  13 #include "pwm.h"
  14
  15 /* CAN message IDs */
  16 #define CAN_CLOCK_ID  0x401
  17 //#define CAN_CLOCK_ID  0x481
  18 #define CAN_CFGMSG_ID 0x4ab
  19 #define CAN_UNDEF_ID  0xf800
  20
  21 #define CANLOAD_ID 0x481
  22 //(*((volatile unsigned long *) 0x40000120))
  23 #define MOTOR_ID CANLOAD_ID
  24
  25 /* PWM and sampling timing */
  26 #define PWM_PERIOD   3000
  27 #define PWM_DEAD     60
  28 //#define PWM_DEAD     0
  29 #define ADC_OVERSAMP 6
  30 #define ADC_MA_LEN   (2*ADC_OVERSAMP)
  31
  32 /* CAN timing */
  33 #define CAN_OVERSAMP 80
  34
  35 /* position limits */
  36 #define POS_MIN 0x060
  37 #define POS_MAX 0x3a0
  38
  39 /* input channels */
  40 #define ADCIN_POS 0
  41 #define ADCIN_CURRENT 1
  42
  43 #define adc_setup(src,clkdiv,on,start,edge) \
  44   (((src)&0x7) | ((((clkdiv)-1)&0xff)<<8) | ((!(!(on)))<<21) | \
  45   (((start)&0x7)<<24) | ((!(!(edge)))<<27))
  46
  47 /* A/D ISR <=> controller function variables */
  48 volatile uint8_t timer_count = 0;
  49 volatile uint32_t adc_i = 0;
  50 volatile uint32_t adc_x = 0;
  51 volatile int8_t current_negative = 0;
  52
  53 /* reserved value for power-off */
  54 #define CTRL_OFF 0xffff
  55 #define CTRL_MAX SHRT_MAX
  56
  57 /* can handle*/
  58 vca_handle_t can;
  59 /* own message for current position signalling */
  60 canmsg_t tx_msg = {.flags = 0, .length = 8};
  61 canmsg_t rx_msg;
  62
  63 /* command message ID and index */
  64 uint16_t cmd_msg_id = 0x440, cmd_msg_ndx = 0; //puvodne cmd_msg_id = CAN_UNDEF_ID;
  65
  66 /* command value, current position */
  67 volatile int16_t rx_cmd_value = CTRL_OFF;
  68 volatile uint8_t tx_request = 0;
  69
  70 /* * * * */
  71
  72 void adc_irq() __attribute__((interrupt));
  73
  74 void timer_init() {
  75   /* zapni MAT0.1 na noze AIN0 */
  76
  77   T0PR = 0; //prescale - 1
  78   T0MR1 = PWM_PERIOD/ADC_OVERSAMP-1; //period
  79   T0MCR = 0x2<<3; /* counter reset on match1 */
  80   T0EMR = 0x1<<6 | 0x2; /* MAT0.1 set low on match */
  81   T0CCR = 0x0; /* no capture */
  82   T0TCR |= 0x1; /* go! */
  83
  84   sync_pwm_timer((uint32_t*)&T0TC);
  85 }
  86
  87 void set_irq_handler(uint8_t source, uint8_t irq_vect, void (*handler)()) {
  88   /* set interrupt vector */
  89   ((uint32_t*)&VICVectAddr0)[irq_vect] = (uint32_t)handler;
  90   ((uint32_t*)&VICVectCntl0)[irq_vect] = 0x20 | source;
  91   /* enable interrupt */
  92   VICIntEnable = 1<<source;
  93 }
  94
  95 inline void adc_init() {
  96   ADCR = adc_setup(1<<ADCIN_CURRENT, 14, 1, 0x4 /*MAT0.1*/, 1 /*FALL! edge*/);
  97 }
  98
  99 /************/
 100
 101 void hbridge_half_set(uint8_t side, uint32_t value) {
 102   uint32_t *s_down, *d_down, *d_up;
 103   uint32_t t;
 104
 105   if (side) {
 106     s_down = PWM_MR[6];  d_down = PWM_MR[4];  d_up = PWM_MR[3];
 107   }
 108   else {
 109     s_down = PWM_MR[5];  d_down = PWM_MR[2];  d_up = PWM_MR[1];
 110   }
 111
 112   if (value < 2*PWM_DEAD) {
 113     /* 0% */
 114     *s_down = PWM_PERIOD;
 115     *d_down = PWM_PERIOD+1;
 116     *d_up = 0;
 117   }
 118   else if (value > PWM_PERIOD-2*PWM_DEAD) {
 119     /* 100% */
 120     *s_down = *d_down = 0;
 121     *d_up = PWM_PERIOD;
 122   }
 123   else {
 124     *d_up = t = PWM_PERIOD-PWM_DEAD;
 125     //*d_down = t -= value;
 126     /****** !!!!!!!!! *******/
 127     *d_down = t -= value - 2*PWM_DEAD;
 128     *s_down = t - PWM_DEAD;
 129   }
 130
 131   PWMLER |= side ? 0x58 : 0x26;
 132 }
 133
 134 void hbridge_set(int32_t value) {
 135   if (value >= 0) {
 136     hbridge_half_set(0, value);
 137     hbridge_half_set(1, 0);
 138   }
 139   else {
 140     hbridge_half_set(1, -value);
 141     hbridge_half_set(0, 0);
 142   }
 143 }
 144
 145 void hbridge_off() {
 146   /* *s_down = 0 (L) */
 147   PWMMR5 = 0;
 148   PWMMR6 = 0;
 149   /* *d_down = PWM_PERIOD+1, *d_up = 0 (H) */
 150   PWMMR4 = PWM_PERIOD+1;
 151   PWMMR3 = 0;
 152   PWMMR2 = PWM_PERIOD+1;
 153   PWMMR1 = 0;
 154   PWMLER |= 0x58 | 0x26;
 155 }
 156
 157 void hbridge_init() {
 158   /* PWM2,4 double-edged, PWM5,6 single-edged */
 159   pwm_channel(2, 1);
 160   pwm_channel(4, 1);
 161   pwm_channel(5, 0);
 162   pwm_channel(6, 0);
 163   /* both sides to GND */
 164   //hbridge_half_set(0, 0);
 165   //hbridge_half_set(1, 0);
 166   /* disconnect the bridge */
 167   hbridge_off();
 168   /* go */
 169   pwm_init(1, PWM_PERIOD);
 170 }
 171
 172 /*****/
 173
 174 //#define CTRL_INT_DIV  (1<<19)
 175 #define CTRL_INT_DIV  (1<<15)
 176 #define CTRL_INT_MAX  (CTRL_INT_DIV*PWM_PERIOD)
 177 #define CTRL_AMP_MUL  (20.0/(12*(1<<11)))
 178 #define CTRL_PI_Q     ((4.0/7.0)*CTRL_INT_MAX*CTRL_AMP_MUL)
 179 #define CTRL_PI_K     (0.6*CTRL_PI_Q)
 180 #define CTRL_PI_Kn    (0.6*0.674*CTRL_PI_Q)
 181
 182 inline int32_t add_sat(int32_t big, int32_t small, int32_t min, int32_t max) {
 183   if (big >= max - small)
 184     return max;
 185   if (big <= min - small)
 186     return min;
 187   return big + small;
 188 }
 189
 190 int32_t pi_ctrl(int16_t w, int16_t y) {
 191   static int32_t s = 0;
 192   int32_t p, e, u, q;
 193
 194   e = w;  e -= y;
 195   p = e;  p *= (int32_t)CTRL_PI_K;
 196   u = add_sat(p, s, -CTRL_INT_MAX, CTRL_INT_MAX);
 197   q = e;  q *= (int32_t)(CTRL_PI_K-CTRL_PI_Kn);
 198   s = add_sat(q, s, -CTRL_INT_MAX-p, CTRL_INT_MAX-p);
 199
 200   return u/CTRL_INT_DIV;
 201 }
 202
 203
 204 int32_t pi_ctrl_(int16_t w, int16_t y) {
 205   static int32_t z = 0, intg = 0;
 206   int32_t p, e;
 207
 208   e = w;  e -= y;
 209
 210   p = e;  p *= (int32_t)CTRL_PI_K;
 211   p -= z;
 212   z = e*(int32_t)CTRL_PI_Kn;
 213
 214   if (intg > 0) {
 215     if (p >= CTRL_INT_MAX - intg) {
 216       intg = CTRL_INT_MAX;
 217       led_set(LED_YEL, 1);
 218     }
 219     else {
 220       intg += p;
 221       led_set(LED_YEL, 0);
 222       led_set(LED_BLU, 0);
 223     }
 224   }
 225   else {
 226     if (p <= -CTRL_INT_MAX - intg) {
 227       intg = -CTRL_INT_MAX;
 228       led_set(LED_BLU, 1);
 229     }
 230     else {
 231       intg += p;
 232       led_set(LED_YEL, 0);
 233       led_set(LED_BLU, 0);
 234     }
 235   }
 236
 237   return intg/CTRL_INT_DIV;
 238 }
 239
 240 #if 0
 241 /*** kmitani -- zobrazeni prechodaku ***/
 242 void control(int32_t ad_i, int16_t ad_x) {
 243   int32_t u;
 244
 245 #define REF_PERIOD 60
 246   static int counter = 0;
 247   float w;
 248
 249   if (counter++ == 2*REF_PERIOD)
 250     counter = 0;
 251   w = (counter >= REF_PERIOD) ? (1.0/CTRL_AMP_MUL) : 0.0; //(-1.0/CTRL_AMP_MUL);
 252
 253   u = pi_ctrl(w, ad_i);
 254
 255   hbridge_set(u);
 256   current_negative = (u < 0);
 257
 258   ((int16_t*)tx_msg.data)[counter%4] = ad_i;
 259   if (counter%4 == 3)
 260     can_tx_msg(&tx_msg);
 261 }
 262 #endif
 263
 264 volatile int16_t control_w = 0; /* reference current (*12) */
 265 volatile int32_t control_y; /* measured current (*12*80) */
 266 volatile int32_t control_x; /* measured position (*80) */
 267 volatile int32_t control_u; /* applied PWM (*80) */
 268 volatile int8_t control_on = 0; /* switches H-bridge on/off */
 269
 270 int32_t ctrl_ma_filter(int32_t *mem, uint8_t *idx, int32_t val) {
 271   int32_t diff = val - mem[*idx];
 272   mem[*idx] = val;
 273   if (++*idx == CAN_OVERSAMP)
 274     *idx = 0;
 275   return diff;
 276 }
 277
 278 void control(int32_t ad_i, int16_t ad_x) {
 279   static int32_t cy = 0, cx = 0, cu = 0;
 280   static int ma_y[CAN_OVERSAMP],
 281     ma_x[CAN_OVERSAMP], ma_u[CAN_OVERSAMP];
 282   static uint8_t idx_y = 0, idx_x = 0, idx_u = 0;
 283   int32_t u;
 284
 285 #if 0
 286   /* boj s blbym merenim proudu */
 287 #define CTRL_I_MIN 80
 288   static int8_t sub_pwm = 0;
 289   int32_t cw, im;
 290   if (control_w > 0) {
 291     cw = (control_w + 1)/2;
 292     im = 2*CTRL_I_MIN;
 293   }
 294   else {
 295     cw = (-control_w - 1)/2;
 296     im = -2*CTRL_I_MIN;
 297   }
 298   if (cw < CTRL_I_MIN)
 299     u = pi_ctrl((cw > sub_pwm) ? im : 0, ad_i);
 300   else
 301     u = pi_ctrl(control_w, ad_i);
 302   sub_pwm = (sub_pwm + 1) % CTRL_I_MIN;
 303   /* job */
 304 #else
 305   u = pi_ctrl(control_w, ad_i);
 306 #endif
 307
 308   if (control_on)
 309     hbridge_set(u);
 310   current_negative = (u < 0);
 311
 312   cy += ctrl_ma_filter(ma_y, &idx_y, ad_i);
 313   cx += ctrl_ma_filter(ma_x, &idx_x, ad_x);
 314   cu += ctrl_ma_filter(ma_u, &idx_u, u);
 315   /***** pozustak mylne honby za synchronizaci... ****/
 316   //control_y = 80*ad_i; //cy;
 317   //control_x = 80*ad_x; //cx;
 318   //control_u = 80*u; //cu;
 319   control_y = cy;
 320   control_x = cx;
 321   control_u = cu;
 322 }
 323
 324 /*****/
 325
 326 void adc_irq() {
 327   static uint8_t ma_idx = 0;
 328   static int32_t ma_val[ADC_MA_LEN];
 329   static int8_t negative = 0;
 330   int32_t ad;
 331   int32_t ma_diff;
 332   int8_t last;
 333
 334   /* read & clear irq flag */
 335   ad = ADDR;
 336
 337   /* reset MAT0.1 */
 338   T0EMR = 0x1<<6 | 0x2; /* must be here due to ADC.5 erratum */
 339
 340   if ((last = (timer_count == ADC_OVERSAMP-1))) {
 341     /* last sample before PWM period end */
 342     /* twice -- ADC.2 erratum workaround */
 343     ADCR = adc_setup(1<<ADCIN_POS, 14, 1, 0x0 /* now! */, 1);
 344     ADCR = adc_setup(1<<ADCIN_POS, 14, 1, 0x1 /* now! */, 1);
 345   }
 346
 347   ad = (ad>>6)&0x3ff;
 348
 349   /***! odpor na 3.3V !***/
 350   //static int32_t ad_zero = 5.0/(12*CTRL_AMP_MUL);
 351   //if (!control_on) ad_zero = ad;
 352   //ad -= ad_zero;
 353
 354   /**** boj s kmitanim ****/
 355   //ad += 24;
 356   /****/
 357   if (negative)
 358     ad = -ad;
 359   /* shift value through MA filter */
 360   ma_diff = ad - ma_val[ma_idx];
 361   ma_val[ma_idx] = ad;
 362   if (++ma_idx == ADC_MA_LEN)
 363     ma_idx = 0;
 364   /* MA filter output (should be atomic): */
 365   adc_i += ma_diff;
 366
 367   if (last) {
 368     while (((ad = ADDR)&0x80000000) == 0);
 369     adc_x = (ad>>6)&0x3ff;
 370     /* twice -- ADC.2 erratum workaround */
 371     ADCR = adc_setup(1<<ADCIN_CURRENT, 14, 1, 0x0 /*MAT0.1*/, 1);
 372     ADCR = adc_setup(1<<ADCIN_CURRENT, 14, 1, 0x4 /*MAT0.1*/, 1);
 373     timer_count = 0;
 374     negative = current_negative;
 375   }
 376   else
 377     ++timer_count;
 378
 379   /* int acknowledge */
 380   VICVectAddr = 0;
 381 }
 382
 383 /******/
 384
 385 uint8_t DBG_clk_seq = 0;
 386
 387 void can_rx(){
 388
 389   led_set(LED_YEL, 1);
 390   switch (rx_msg.id) {
 391   case CAN_CLOCK_ID:
 392     if ((uint16_t)rx_cmd_value != CTRL_OFF) {
 393       control_w = rx_cmd_value;
 394       control_on = 1;
 395       DBG_clk_seq = rx_msg.data[0];
 396     }
 397     tx_request = 1;
 398     break;
 399   case CAN_CFGMSG_ID:
 400     if (((uint16_t*)rx_msg.data)[0] == MOTOR_ID) {
 401       cmd_msg_id = ((uint16_t*)rx_msg.data)[1];
 402       cmd_msg_ndx = ((uint16_t*)rx_msg.data)[2];
 403     }
 404     break;
 405   default:
 406     if (rx_msg.id == cmd_msg_id) {
 407       rx_cmd_value = ((int16_t*)rx_msg.data)[cmd_msg_ndx]; //zde si nejsem jisty tito pretypovanim
 408       if ((uint16_t)rx_cmd_value == CTRL_OFF) {
 409         control_on = 0;
 410         hbridge_off();
 411       }
 412     }
 413     break;
 414   }
 415
 416   led_set(LED_YEL, 0);
 417 }
 418 uint32_t btr = 0;
 419 void can_tx() {
 420
 421   /* x: 16b, y: 24b, u: 24b */
 422   uint16_t control_2 = (uint16_t)(control_x/2);
 423   tx_msg.data[0] = *((uint8_t*)&control_2);
 424   tx_msg.data[1] = *((uint8_t*)&control_2 + 1);
 425   tx_msg.data[2] = *((uint8_t*)&control_y);
 426 //  tx_msg.data[2] = *((uint8_t*)&rx_cmd_value+1);
 427   tx_msg.data[3] = *((uint8_t*)&control_y + 1);
 428 //  tx_msg.data[3] = *((uint8_t*)&rx_cmd_value);
 429   tx_msg.data[4] = *((uint8_t*)&control_y + 2);
 430   tx_msg.data[5] = *((uint8_t*)&control_u);
 431   tx_msg.data[6] = *((uint8_t*)&control_u + 1);
 432   tx_msg.data[7] = *((uint8_t*)&control_u + 2);
 433
 434   /* do NOT wait */
 435   vca_send_msg_seq(can,&tx_msg,1);
 436   tx_request = 0;
 437 }
 438
 439 /*****************/
 440
 441 void sys_pll_init(uint32_t f_cclk, uint32_t f_osc) {
 442   uint32_t m, p, p_max;
 443
 444   led_set(LED_YEL, 1);
 445
 446   /* turn memory acceleration off */
 447   MAMCR = 0x0;
 448
 449   /* compute PLL divider and internal osc multiplier */
 450   m = f_cclk/f_osc;
 451   p_max = 160000000/f_cclk;
 452   for (p = 3; ((1<<p) > p_max) && (p > 0); p = p>>1);
 453   /* switch PLL on */
 454   PLLCFG = (p<<5) | ((m-1)&0x1f);
 455   PLLCON = 0x1;
 456   PLLFEED = 0xaa;  PLLFEED = 0x55;
 457   /* wait for lock */
 458   while (!(PLLSTAT & (1<<10)));
 459   /* connect to clock */
 460   PLLCON = 0x3;
 461   PLLFEED = 0xaa;  PLLFEED = 0x55;
 462
 463   /* turn memory acceleration on */
 464   MAMTIM = f_cclk/20000000;
 465   MAMCR = 0x2;
 466
 467   led_set(LED_YEL, 0);
 468 }
 469
 470 int main() {
 471   led_init();
 472   led_set(LED_GRN, 1);
 473   /* switch the CAN off due to clock change */
 474   //can_off(0);
 475   /* boost clock to 60MHz */
 476   sys_pll_init(60000000, 10000000);
 477   /* peripheral clock = CPU clock (60MHz) */
 478   VPBDIV = 1;
 479
 480   /* init Vector Interrupt Controller */
 481   VICIntEnClr = 0xFFFFFFFF;
 482   VICIntSelect = 0x00000000;
 483
 484   /* CAN bus setup */
 485
 486   /*inicialice CANu*/
 487   lpcan_btr(&btr, 1000000 /*Bd*/, 60000000, 0/*SJW*/, 70/*%sampl.pt.*/, 0/*SAM*/);
 488   lpc_vca_open_handle(&can, 0/*device*/, 1/*flags*/, btr, 10, 11, 12);
 489
 490   tx_msg.id = MOTOR_ID;
 491
 492   hbridge_init();
 493 //  hbridge_set((PWM_PERIOD/ADC_OVERSAMP-2*PWM_DEAD));
 494
 495   set_irq_handler(18 /*ADC*/, 13, adc_irq);
 496   adc_init();
 497   timer_init();
 498
 499
 500   for (;;) {
 501     if(vca_rec_msg_seq(can,&rx_msg,1)<1){
 502      if (timer_count == 1)
 503        control(adc_i, adc_x);
 504      if (tx_request)
 505        can_tx();
 506     }
 507     else can_rx();
 508   }
 509 }