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 #include <string.h>
   7 /* CAN definitions*/
   8 #include <can/canmsg.h>
   9 #include <can/lpcan.h>
  10 #include <can/lpcan_vca.h>
  11 /*****************/
  12 #include "led.h"
  13 #include "pwm.h"
  14 #include "hbridge.h"
  15 #include "constants.h"
  16
  17 #define adc_setup(src,clkdiv,on,start,edge) \
  18   (((src)&0x7) | ((((clkdiv)-1)&0xff)<<8) | ((!(!(on)))<<21) | \
  19   (((start)&0x7)<<24) | ((!(!(edge)))<<27))
  20
  21 /********* Variables **********/
  22
  23 /* A/D ISR <=> controller function variables */
  24 volatile uint8_t timer_count = 0;
  25 volatile uint32_t adc_i = 0;
  26 volatile uint32_t adc_x = 0;
  27 volatile int8_t current_negative = 0;
  28
  29 /* CAN handle */
  30 vca_handle_t can;
  31 /* CAN Bus Timing register value */
  32 uint32_t btr = 0;
  33 /* Own message for current position signalling */
  34 canmsg_t tx_msg = {.flags = 0, .length = 8};
  35 /* Recieved message */
  36 canmsg_t rx_msg;
  37
  38 /* Command message ID and index */
  39 uint16_t cmd_msg_id = CAN_UNDEF_ID, cmd_msg_ndx = 0;
  40
  41 /* Command value, current position */
  42 volatile int16_t rx_cmd_value = CTRL_OFF;
  43 volatile uint8_t tx_request = 0;
  44
  45 /***/
  46 volatile int16_t control_w = 0;         /* reference current (*12)      */
  47 volatile int32_t control_y;             /* measured current (*12*80)    */
  48 volatile int32_t control_x;             /* measured position (*80)      */
  49 volatile int32_t control_u;             /* applied PWM (*80)            */
  50 volatile int8_t control_on = 0;         /* switches H-bridge on/off     */
  51 /***/
  52
  53 uint8_t DBG_clk_seq = 0;
  54
  55 /* * * * */
  56
  57 void adc_irq() __attribute__((interrupt));
  58
  59 void timer_init() {
  60   /* zapni MAT0.1 na noze AIN0 */
  61   T0PR = 0; //prescale - 1
  62   T0MR1 = PWM_PERIOD/ADC_OVERSAMP-1; //period
  63   T0MCR = 0x2<<3; /* counter reset on match1 */
  64   T0EMR = 0x1<<6 | 0x2; /* MAT0.1 set low on match */
  65   T0CCR = 0x0; /* no capture */
  66   T0TCR |= 0x1; /* go! */
  67
  68   sync_pwm_timer((uint32_t*)&T0TC);
  69 }
  70
  71 void set_irq_handler(uint8_t source, uint8_t irq_vect, void (*handler)()) {
  72   /* set interrupt vector */
  73   ((uint32_t*)&VICVectAddr0)[irq_vect] = (uint32_t)handler;
  74   ((uint32_t*)&VICVectCntl0)[irq_vect] = 0x20 | source;
  75   /* enable interrupt */
  76   VICIntEnable = 1<<source;
  77 }
  78
  79 inline void adc_init() {
  80   ADCR = adc_setup(1<<ADCIN_CURRENT, 14, 1, 0x4 /*MAT0.1*/, 1 /*FALL! edge*/);
  81 }
  82
  83 /* Saturation */
  84 inline int32_t add_sat(int32_t big, int32_t small, int32_t min, int32_t max) {
  85   if (big >= max - small)
  86     return max;
  87   if (big <= min - small)
  88     return min;
  89   return big + small;
  90 }
  91
  92 /* PI controler */
  93 int32_t pi_ctrl(int16_t w, int16_t y) {
  94   static int32_t s = 0;
  95   int32_t p, e, u, q;
  96
  97   e = w;  e -= y;
  98   p = e;  p *= (int32_t)CTRL_PI_K;
  99   u = add_sat(p, s, -CTRL_INT_MAX, CTRL_INT_MAX);
 100   q = e;  q *= (int32_t)(CTRL_PI_K-CTRL_PI_Kn);
 101   s = add_sat(q, s, -CTRL_INT_MAX-p, CTRL_INT_MAX-p);
 102
 103   return u/CTRL_INT_DIV;
 104 }
 105
 106 /* MA filter */
 107 int32_t ctrl_ma_filter(int32_t *mem, uint8_t *idx, int32_t val) {
 108   int32_t diff = val - mem[*idx];
 109   mem[*idx] = val;
 110   if (++*idx == CAN_OVERSAMP)
 111     *idx = 0;
 112   return diff;
 113 }
 114
 115 /**** Control ****/
 116 void control(int32_t ad_i, int16_t ad_x) {
 117   static int32_t cy = 0, cx = 0, cu = 0;
 118   static int ma_y[CAN_OVERSAMP],
 119     ma_x[CAN_OVERSAMP], ma_u[CAN_OVERSAMP];
 120   static uint8_t idx_y = 0, idx_x = 0, idx_u = 0;
 121   int32_t u;
 122
 123   u = pi_ctrl(control_w, ad_i);
 124
 125   if (control_on)
 126     hbridge_set(u);
 127   current_negative = (u < 0);
 128
 129   cy += ctrl_ma_filter(ma_y, &idx_y, ad_i);
 130   cx += ctrl_ma_filter(ma_x, &idx_x, ad_x);
 131   cu += ctrl_ma_filter(ma_u, &idx_u, u);
 132   control_y = cy;
 133   control_x = cx;
 134   control_u = cu;
 135 }
 136
 137 /*****/
 138 void adc_irq() {
 139   static uint8_t ma_idx = 0;
 140   static int32_t ma_val[ADC_MA_LEN];
 141   static int8_t negative = 0;
 142   int32_t ad;
 143   int32_t ma_diff;
 144   int8_t last;
 145
 146   /* read & clear irq flag */
 147   ad = ADDR;
 148
 149   /* reset MAT0.1 */
 150   T0EMR = 0x1<<6 | 0x2; /* must be here due to ADC.5 erratum */
 151
 152   if ((last = (timer_count == ADC_OVERSAMP-1))) {
 153     /* last sample before PWM period end */
 154     /* twice -- ADC.2 erratum workaround */
 155     ADCR = adc_setup(1<<ADCIN_POS, 14, 1, 0x0 /* now! */, 1);
 156     ADCR = adc_setup(1<<ADCIN_POS, 14, 1, 0x1 /* now! */, 1);
 157   }
 158
 159   ad = (ad>>6)&0x3ff;
 160
 161   if (negative)
 162     ad = -ad;
 163   /* shift value through MA filter */
 164   ma_diff = ad - ma_val[ma_idx];
 165   ma_val[ma_idx] = ad;
 166   if (++ma_idx == ADC_MA_LEN)
 167     ma_idx = 0;
 168   /* MA filter output (should be atomic): */
 169   adc_i += ma_diff;
 170
 171   if (last) {
 172     while (((ad = ADDR)&0x80000000) == 0);
 173     adc_x = (ad>>6)&0x3ff;
 174     /* twice -- ADC.2 erratum workaround */
 175     ADCR = adc_setup(1<<ADCIN_CURRENT, 14, 1, 0x0 /*MAT0.1*/, 1);
 176     ADCR = adc_setup(1<<ADCIN_CURRENT, 14, 1, 0x4 /*MAT0.1*/, 1);
 177     timer_count = 0;
 178     negative = current_negative;
 179   }
 180   else
 181     ++timer_count;
 182
 183   /* int acknowledge */
 184   VICVectAddr = 0;
 185 }
 186
 187 /******/
 188
 189 void can_rx(){
 190   switch (rx_msg.id) {
 191   /*message from CAN clock*/
 192   case CAN_CLOCK_ID:
 193     if ((uint16_t)rx_cmd_value != CTRL_OFF) {
 194       control_w = rx_cmd_value;
 195       control_on = 1;
 196       DBG_clk_seq = rx_msg.data[0];
 197     }
 198     tx_request = 1;
 199     break;
 200   /*config message from control computer*/
 201   case CAN_CFGMSG_ID:
 202     /*is this message for this motor?*/
 203     if ((rx_msg.data[0] | ((rx_msg.data[1])<<8)) == MOTOR_ID) {
 204       /*ID of control message for this motor*/
 205       cmd_msg_id = rx_msg.data[2] | ((rx_msg.data[3])<<8);
 206       /*part of control message*/
 207       cmd_msg_ndx = rx_msg.data[4] | ((rx_msg.data[5])<<8);
 208     }
 209     break;
 210   default:
 211     /*control message for this motor*/
 212     if (rx_msg.id == cmd_msg_id) {
 213       /*current value*/
 214       rx_cmd_value = rx_msg.data[cmd_msg_ndx] | ((rx_msg.data[cmd_msg_ndx+1])<<8);
 215       /*switch off motor*/
 216       if ((uint16_t)rx_cmd_value == CTRL_OFF) {
 217         control_on = 0;
 218         hbridge_off();
 219       }
 220     }
 221     break;
 222   }
 223 }
 224
 225 void can_tx() {
 226
 227   /* x: 16b, y: 24b, u: 24b */
 228   uint16_t control_2 = (uint16_t)(control_x/2);
 229   tx_msg.data[0] = *((uint8_t*)&control_2);
 230   tx_msg.data[1] = *((uint8_t*)&control_2 + 1);
 231   tx_msg.data[2] = *((uint8_t*)&control_y);
 232   tx_msg.data[3] = *((uint8_t*)&control_y + 1);
 233   tx_msg.data[4] = *((uint8_t*)&control_y + 2);
 234   tx_msg.data[5] = *((uint8_t*)&control_u);
 235   tx_msg.data[6] = *((uint8_t*)&control_u + 1);
 236   tx_msg.data[7] = *((uint8_t*)&control_u + 2);
 237  /*debug message*/
 238  /* tx_msg.data[2] = *((uint8_t*)&cmd_msg_id+1);
 239   tx_msg.data[3] = *((uint8_t*)&cmd_msg_id);
 240   tx_msg.data[4] = *((uint8_t*)&cmd_msg_ndx+1);
 241   tx_msg.data[5] = *((uint8_t*)&cmd_msg_ndx);
 242   tx_msg.data[6] = *((uint8_t*)&rx_cmd_value+1);
 243   tx_msg.data[7] = *((uint8_t*)&rx_cmd_value);*/
 244   /* send message and do NOT wait*/
 245   vca_send_msg_seq(can,&tx_msg,1);
 246   tx_request = 0;
 247 }
 248
 249 /*****************/
 250
 251 void sys_pll_init(uint32_t f_cclk, uint32_t f_osc) {
 252   uint32_t m, p, p_max;
 253
 254   led_set(LED_YEL, 1);
 255
 256   /* turn memory acceleration off */
 257   MAMCR = 0x0;
 258
 259   /* compute PLL divider and internal osc multiplier */
 260   m = f_cclk/f_osc;
 261   p_max = 160000000/f_cclk;
 262   for (p = 3; ((1<<p) > p_max) && (p > 0); p = p>>1);
 263   /* switch PLL on */
 264   PLLCFG = (p<<5) | ((m-1)&0x1f);
 265   PLLCON = 0x1;
 266   PLLFEED = 0xaa;  PLLFEED = 0x55;
 267   /* wait for lock */
 268   while (!(PLLSTAT & (1<<10)));
 269   /* connect to clock */
 270   PLLCON = 0x3;
 271   PLLFEED = 0xaa;  PLLFEED = 0x55;
 272
 273   /* turn memory acceleration on */
 274   MAMTIM = f_cclk/20000000;
 275   MAMCR = 0x2;
 276
 277   led_set(LED_YEL, 0);
 278 }
 279
 280 int main() {
 281   led_init();
 282   led_set(LED_GRN, 1);
 283   /* switch the CAN off due to clock change */
 284   //can_off(0);
 285   /* boost clock to 60MHz */
 286   sys_pll_init(60000000, 10000000);
 287   /* peripheral clock = CPU clock (60MHz) */
 288   VPBDIV = 1; /*!!! Pozor defaultne VPBDIV = 2 !!!*/
 289
 290   /* init Vector Interrupt Controller */
 291   VICIntEnClr = 0xFFFFFFFF;
 292   VICIntSelect = 0x00000000;
 293
 294   /* CAN bus setup */
 295
 296   /*inicialice CANu*/
 297   /*Nyni plne funkcni na SpejblARM desce s 10MHz xtalem*/
 298   /*Zaroven funguje na euroboti desce s 10MHz xtalem*/
 299   /*Zde 60MHz kvuli pll*/
 300   lpcan_btr(&btr, 1000000 /*Bd*/, 60000000, 0/*SJW*/, 70/*%sampl.pt.*/, 0/*SAM*/);
 301   lpc_vca_open_handle(&can, 0/*device*/, 1/*flags*/, btr, 10, 11, 12);
 302
 303   tx_msg.id = MOTOR_ID;
 304
 305   hbridge_init();
 306 //  hbridge_set((PWM_PERIOD/ADC_OVERSAMP-2*PWM_DEAD));
 307
 308   set_irq_handler(18 /*ADC*/, 13, adc_irq);
 309   adc_init();
 310   timer_init();
 311
 312
 313   for (;;) {
 314     if(vca_rec_msg_seq(can,&rx_msg,1)<1){
 315      if (timer_count == 1)
 316        control(adc_i, adc_x);
 317      if (tx_request)
 318        can_tx();
 319     }
 320     else can_rx();
 321   }
 322 }