New software for Spejbl motor control now on lpcanvca.

diff --git a/app/can_HelloWorld/main.c b/app/can_HelloWorld/main.c
index 9ce87f8..c623c6b 100644 (file)
--- a/app/can_HelloWorld/main.c
+++ b/app/can_HelloWorld/main.c
@@ -3,7 +3,7 @@
 /*                                                     */
 /* This software recieves messages via CAN bus. If     */
 /* message with right data (0x1) is recieve, adc value  */
-/* is send also via can.                                       */
+/* is send also via can.                               */
 /* This software is an example how to use lpcan.h and  */
 /* lpcan_vca.h.                                                */
 /*                                                     */
@@ -38,17 +38,14 @@ void adc_init(unsigned clk_div) {
  */
 int main (void)  {
 
-       MEMMAP = 0x1;   //flash
-       //MEMMAP = 0x2; //ram
-
+       VPBDIV = 1;
+       
        VICIntEnClr = 0xFFFFFFFF;
        VICIntSelect = 0x00000000;      
 
        /* CAN bus setup */
        uint32_t btr;
-       /*20MHz je empiricky urcene cislo pro desku spejblarm(xtal 10MHz)*/
-       /*s jinou konstantou zatim komunikace po can nebezi*/
-       lpcan_btr(&btr, 1000000 /*Bd*/, 20000000, 0/*SJW*/, 70/*%sampl.pt.*/, 0/*SAM*/);
+       lpcan_btr(&btr, 1000000 /*Bd*/, 10000000, 0/*SJW*/, 70/*%sampl.pt.*/, 0/*SAM*/);
        lpc_vca_open_handle(&can, 0/*device*/, 0/*flags*/, btr, 10, 11, 12);    
 
        /*init adc*/
@@ -59,12 +56,25 @@ int main (void)  {
                if(vca_rec_msg_seq(can,&msg,1)<0) continue;
                switch (msg.data[0]){
                /* if message with id = 1 send adc value*/ 
+               case 0x0:
+                       ADCR = ADC_CR_ADC0_m | ((14&0xff)<<8)/*ADC clock*/ | ADC_CR_PDN_ON_m; 
+                       ADCR |= (1<<24); /* star conversion now */
+                       /* wait for end of conversion */
+                       while((ADDR & (1<<31)) == 0);
+                       /*set can message*/     
+                       msg.id = 0x0;
+                       msg.length = 1;
+                       /* Read value from AIN0*/
+                       msg.data[0] = ((ADDR>>6)&0x3ff);
+                       vca_send_msg_seq(can,&msg,1);
+                       break;
                case 0x1:
+                       ADCR = ADC_CR_ADC1_m | ((14&0xff)<<8)/*ADC clock*/ | ADC_CR_PDN_ON_m; 
                        ADCR |= (1<<24); /* star conversion now */
                        /* wait for end of conversion */
                        while((ADDR & (1<<31)) == 0);
                        /*set can message*/     
-                       msg.id = 0x3;
+                       msg.id = 0x1;
                        msg.length = 1;
                        /* Read value from AIN0*/
                        msg.data[0] = ((ADDR>>6)&0x3ff);
@@ -72,7 +82,7 @@ int main (void)  {
                        break;
                default: 
                        /*send ee*/
-                       msg.id = 0x2;
+                       msg.id = 0xe;
                        msg.length = 1;
                        msg.data[0] = 0xee;
                        vca_send_msg_seq(can,&msg,1);

diff --git a/app/spejbl_hodiny/Makefile b/app/spejbl_hodiny/Makefile
new file mode 100644 (file)
index 0000000..08cf5ff
--- /dev/null
+++ b/app/spejbl_hodiny/Makefile
@@ -0,0 +1,14 @@
+# Generic directory or leaf node makefile for OCERA make framework
+
+ifndef MAKERULES_DIR
+MAKERULES_DIR := $(shell ( old_pwd="" ;  while [ ! -e Makefile.rules ] ; do if [ "$$old_pwd" = `pwd`  ] ; then exit 1 ; else old_pwd=`pwd` ; cd -L .. 2>/dev/null ; fi ; done ; pwd ) )
+endif
+
+ifeq ($(MAKERULES_DIR),)
+all : default
+.DEFAULT::
+       @echo -e "\nThe Makefile.rules has not been found in this or partent directory\n"
+else   
+include $(MAKERULES_DIR)/Makefile.rules
+endif
+

diff --git a/app/spejbl_hodiny/Makefile.omk b/app/spejbl_hodiny/Makefile.omk
new file mode 100644 (file)
index 0000000..9a15c0e
--- /dev/null
+++ b/app/spejbl_hodiny/Makefile.omk
@@ -0,0 +1,7 @@
+bin_PROGRAMS = shodvca
+
+shodvca_SOURCES =  hodiny.c
+
+shodvca_LIBS = lpcanvca lpcan
+
+

diff --git a/app/spejbl_hodiny/hodiny.c b/app/spejbl_hodiny/hodiny.c
new file mode 100644 (file)
index 0000000..8e954dd
--- /dev/null
+++ b/app/spejbl_hodiny/hodiny.c
@@ -0,0 +1,67 @@
+/* LPC21xx definitions */
+#include <lpc21xx.h>
+/* CAN definitions*/
+#include <can/canmsg.h>
+#include <can/lpcan.h>
+#include <can/lpcan_vca.h>
+
+#define CAN_ID 0x401
+/* can handle*/
+vca_handle_t can;
+uint32_t btr;
+/*own can message*/
+canmsg_t msg = {.id = CAN_ID, .flags = 0, .length = 1};
+
+uint8_t seq_num = 0;
+
+void timer_irq() __attribute__((interrupt));
+
+void timer_init(uint32_t prescale, uint32_t period) {
+  T0PR = prescale - 1;
+  T0MR1 = period - 1;
+  T0MCR = 0x3<<3; /* interrupt and counter reset on match1 */
+  T0EMR = 0x1<<6 | 0x2; /* set MAT0.1 low on match and high now */
+  T0CCR = 0x0; /* no capture */
+  T0TCR |= 0x1; /* go! */
+}
+
+void timer_init_irq(unsigned irq_vect) {
+  /* set interrupt vector */
+  ((uint32_t*)&VICVectAddr0)[irq_vect] = (uint32_t)timer_irq;
+  ((uint32_t*)&VICVectCntl0)[irq_vect] = 0x20 | 4;
+  /* enable timer int */
+  VICIntEnable = 0x00000010;
+}
+
+void timer_irq() {
+  msg.data[0] = seq_num++;
+  vca_send_msg_seq(can,&msg,1);
+  /* clear int flag */
+  T0IR = 0xffffffff;
+  /* int acknowledge */
+  VICVectAddr = 0;
+}
+
+/**
+ * Main function 
+ */
+int main (void)  {
+
+       //MEMMAP = 0x1; //flash
+       //MEMMAP = 0x2; //ram
+       VPBDIV = 1;
+       VICIntEnClr = 0xFFFFFFFF;
+       VICIntSelect = 0x00000000;      
+
+       /* CAN bus setup */
+       lpcan_btr(&btr, 1000000 /*Bd*/, 10000000, 0/*SJW*/, 70/*%sampl.pt.*/, 0/*SAM*/);
+       lpc_vca_open_handle(&can, 0/*device*/, 1/*flags*/,btr, 10, 11, 12);     
+
+       timer_init(1000, 4000 /*250Hz*/);
+       timer_init_irq(13);
+
+       for(;;);
+}
+
+
+

diff --git a/app/spejbl_motor/Makefile b/app/spejbl_motor/Makefile
new file mode 100644 (file)
index 0000000..f595272
--- /dev/null
+++ b/app/spejbl_motor/Makefile
@@ -0,0 +1,14 @@
+# Generic directory or leaf node makefile for OCERA make framework
+
+ifndef MAKERULES_DIR
+MAKERULES_DIR := $(shell ( old_pwd="" ;  while [ ! -e Makefile.rules ] ; do if [ "$$old_pwd" == `pwd`  ] ; then exit 1 ; else old_pwd=`pwd` ; cd -L .. 2>/dev/null ; fi ; done ; pwd ) )
+endif
+
+ifeq ($(MAKERULES_DIR),)
+all : default
+.DEFAULT::
+       @echo -e "\nThe Makefile.rules has not been found in this or partent directory\n"
+else   
+include $(MAKERULES_DIR)/Makefile.rules
+endif
+

diff --git a/app/spejbl_motor/Makefile.omk b/app/spejbl_motor/Makefile.omk
new file mode 100644 (file)
index 0000000..b61563a
--- /dev/null
+++ b/app/spejbl_motor/Makefile.omk
@@ -0,0 +1,6 @@
+# -*- makefile -*-
+
+bin_PROGRAMS = spejblmotor
+spejblmotor_SOURCES = motorx.c pwm.c led.c
+spejblmotor_LIBS = lpcanvca lpcan
+#link_VARIANTS = mpram
\ No newline at end of file

diff --git a/app/spejbl_motor/led.c b/app/spejbl_motor/led.c
new file mode 100644 (file)
index 0000000..fb6c228
--- /dev/null
+++ b/app/spejbl_motor/led.c
@@ -0,0 +1,35 @@
+#include "led.h"
+
+struct {
+  volatile uint32_t *ioset, *ioclr, bit;
+} led_ctl[] = {
+  {(uint32_t*)&IOSET0, (uint32_t*)&IOCLR0, 1<<22},
+  {(uint32_t*)&IOSET0, (uint32_t*)&IOCLR0, 1<<23},
+  {(uint32_t*)&IOSET0, (uint32_t*)&IOCLR0, 1<<24},
+  {(uint32_t*)&IOSET1, (uint32_t*)&IOCLR1, 1<<19}
+};
+
+void led_init() {
+  PINSEL1 &= ~0x0003f000;
+  PINSEL2 &= ~0x00000008;
+  IODIR0 |= 0x01c00000;
+  IODIR1 |= 0x00040000;
+  IOCLR0 = 0x01c00000;
+  IOCLR1 = 0x00040000;
+}
+
+void led_set(uint8_t led, uint8_t state) {
+  if (state)
+    *led_ctl[led].ioset = led_ctl[led].bit;
+  else
+    *led_ctl[led].ioclr = led_ctl[led].bit;
+}
+
+void led_toggle(uint8_t led) {
+  if ((*led_ctl[led].ioset) & led_ctl[led].bit)
+    *led_ctl[led].ioclr = led_ctl[led].bit;
+  else
+    *led_ctl[led].ioset = led_ctl[led].bit;
+}
+
+/* Tot vse. */

diff --git a/app/spejbl_motor/led.h b/app/spejbl_motor/led.h
new file mode 100644 (file)
index 0000000..167a4a4
--- /dev/null
+++ b/app/spejbl_motor/led.h
@@ -0,0 +1,20 @@
+#include <types.h>
+#include <lpc21xx.h>
+
+#define LED_PINSEL     PINSEL1
+#define LED_PINSEL_VAL 0xffff3fff;
+#define LED_IODIR      IODIR0
+#define LED_IOSET      IOSET0
+#define LED_IOCLR      IOCLR0
+#define LED_IOBIT      0x00800000
+
+#define LED_RED 0
+#define LED_YEL 1
+#define LED_GRN 2
+#define LED_BLU 3
+
+void led_init();
+void led_set(uint8_t led, uint8_t state);
+void led_toggle(uint8_t led);
+
+/* .oOo. */

diff --git a/app/spejbl_motor/motorx.c b/app/spejbl_motor/motorx.c
new file mode 100644 (file)
index 0000000..0820319
--- /dev/null
+++ b/app/spejbl_motor/motorx.c
@@ -0,0 +1,509 @@
+#include <stdlib.h>
+#include <types.h>
+#include <limits.h>
+#include <lpc21xx.h>
+#include <cpu_def.h>
+/* CAN definitions*/
+#include <can/canmsg.h>
+#include <can/lpcan.h>
+#include <can/lpcan_vca.h>
+//
+#include <string.h>
+#include "led.h"
+#include "pwm.h"
+
+/* CAN message IDs */
+#define CAN_CLOCK_ID  0x401
+//#define CAN_CLOCK_ID  0x481
+#define CAN_CFGMSG_ID 0x4ab 
+#define CAN_UNDEF_ID  0xf800
+
+#define CANLOAD_ID 0x481 
+//(*((volatile unsigned long *) 0x40000120))
+#define MOTOR_ID CANLOAD_ID
+
+/* PWM and sampling timing */
+#define PWM_PERIOD   3000
+#define PWM_DEAD     60
+//#define PWM_DEAD     0
+#define ADC_OVERSAMP 6
+#define ADC_MA_LEN   (2*ADC_OVERSAMP)
+
+/* CAN timing */
+#define CAN_OVERSAMP 80
+
+/* position limits */
+#define POS_MIN 0x060
+#define POS_MAX 0x3a0
+
+/* input channels */
+#define ADCIN_POS 0
+#define ADCIN_CURRENT 1
+
+#define adc_setup(src,clkdiv,on,start,edge) \
+  (((src)&0x7) | ((((clkdiv)-1)&0xff)<<8) | ((!(!(on)))<<21) | \
+  (((start)&0x7)<<24) | ((!(!(edge)))<<27))
+
+/* A/D ISR <=> controller function variables */
+volatile uint8_t timer_count = 0;
+volatile uint32_t adc_i = 0;
+volatile uint32_t adc_x = 0;
+volatile int8_t current_negative = 0;
+
+/* reserved value for power-off */
+#define CTRL_OFF 0xffff
+#define CTRL_MAX SHRT_MAX
+
+/* can handle*/
+vca_handle_t can;
+/* own message for current position signalling */
+canmsg_t tx_msg = {.flags = 0, .length = 8};
+canmsg_t rx_msg;
+
+/* command message ID and index */
+uint16_t cmd_msg_id = 0x440, cmd_msg_ndx = 0; //puvodne cmd_msg_id = CAN_UNDEF_ID;
+
+/* command value, current position */
+volatile int16_t rx_cmd_value = CTRL_OFF;
+volatile uint8_t tx_request = 0;
+
+/* * * * */
+
+void adc_irq() __attribute__((interrupt));
+
+void timer_init() {
+  /* zapni MAT0.1 na noze AIN0 */
+
+  T0PR = 0; //prescale - 1
+  T0MR1 = PWM_PERIOD/ADC_OVERSAMP-1; //period
+  T0MCR = 0x2<<3; /* counter reset on match1 */
+  T0EMR = 0x1<<6 | 0x2; /* MAT0.1 set low on match */
+  T0CCR = 0x0; /* no capture */
+  T0TCR |= 0x1; /* go! */
+
+  sync_pwm_timer((uint32_t*)&T0TC);
+}
+
+void set_irq_handler(uint8_t source, uint8_t irq_vect, void (*handler)()) {
+  /* set interrupt vector */
+  ((uint32_t*)&VICVectAddr0)[irq_vect] = (uint32_t)handler;
+  ((uint32_t*)&VICVectCntl0)[irq_vect] = 0x20 | source;
+  /* enable interrupt */
+  VICIntEnable = 1<<source;
+}
+
+inline void adc_init() {
+  ADCR = adc_setup(1<<ADCIN_CURRENT, 14, 1, 0x4 /*MAT0.1*/, 1 /*FALL! edge*/);
+}
+
+/************/
+
+void hbridge_half_set(uint8_t side, uint32_t value) {
+  uint32_t *s_down, *d_down, *d_up;
+  uint32_t t;
+
+  if (side) {
+    s_down = PWM_MR[6];  d_down = PWM_MR[4];  d_up = PWM_MR[3];
+  }
+  else {
+    s_down = PWM_MR[5];  d_down = PWM_MR[2];  d_up = PWM_MR[1];
+  }
+
+  if (value < 2*PWM_DEAD) {
+    /* 0% */
+    *s_down = PWM_PERIOD;
+    *d_down = PWM_PERIOD+1;
+    *d_up = 0;
+  }
+  else if (value > PWM_PERIOD-2*PWM_DEAD) {
+    /* 100% */
+    *s_down = *d_down = 0;
+    *d_up = PWM_PERIOD;
+  }
+  else {
+    *d_up = t = PWM_PERIOD-PWM_DEAD;
+    //*d_down = t -= value;
+    /****** !!!!!!!!! *******/
+    *d_down = t -= value - 2*PWM_DEAD;
+    *s_down = t - PWM_DEAD;
+  }
+
+  PWMLER |= side ? 0x58 : 0x26;
+}
+
+void hbridge_set(int32_t value) {
+  if (value >= 0) {
+    hbridge_half_set(0, value);
+    hbridge_half_set(1, 0);
+  }
+  else {
+    hbridge_half_set(1, -value);
+    hbridge_half_set(0, 0);
+  }
+}
+
+void hbridge_off() {
+  /* *s_down = 0 (L) */
+  PWMMR5 = 0;
+  PWMMR6 = 0;
+  /* *d_down = PWM_PERIOD+1, *d_up = 0 (H) */
+  PWMMR4 = PWM_PERIOD+1;
+  PWMMR3 = 0;
+  PWMMR2 = PWM_PERIOD+1;
+  PWMMR1 = 0;
+  PWMLER |= 0x58 | 0x26;
+}
+
+void hbridge_init() {
+  /* PWM2,4 double-edged, PWM5,6 single-edged */
+  pwm_channel(2, 1);
+  pwm_channel(4, 1);
+  pwm_channel(5, 0);
+  pwm_channel(6, 0);
+  /* both sides to GND */
+  //hbridge_half_set(0, 0);
+  //hbridge_half_set(1, 0);
+  /* disconnect the bridge */
+  hbridge_off();
+  /* go */
+  pwm_init(1, PWM_PERIOD);
+}
+
+/*****/
+
+//#define CTRL_INT_DIV  (1<<19)
+#define CTRL_INT_DIV  (1<<15)
+#define CTRL_INT_MAX  (CTRL_INT_DIV*PWM_PERIOD)
+#define CTRL_AMP_MUL  (20.0/(12*(1<<11)))
+#define CTRL_PI_Q     ((4.0/7.0)*CTRL_INT_MAX*CTRL_AMP_MUL)
+#define CTRL_PI_K     (0.6*CTRL_PI_Q)
+#define CTRL_PI_Kn    (0.6*0.674*CTRL_PI_Q)
+
+inline int32_t add_sat(int32_t big, int32_t small, int32_t min, int32_t max) {
+  if (big >= max - small)
+    return max;
+  if (big <= min - small)
+    return min;
+  return big + small;
+}
+
+int32_t pi_ctrl(int16_t w, int16_t y) {
+  static int32_t s = 0;
+  int32_t p, e, u, q;
+
+  e = w;  e -= y;
+  p = e;  p *= (int32_t)CTRL_PI_K;
+  u = add_sat(p, s, -CTRL_INT_MAX, CTRL_INT_MAX);
+  q = e;  q *= (int32_t)(CTRL_PI_K-CTRL_PI_Kn);
+  s = add_sat(q, s, -CTRL_INT_MAX-p, CTRL_INT_MAX-p);
+
+  return u/CTRL_INT_DIV;
+}
+
+
+int32_t pi_ctrl_(int16_t w, int16_t y) {
+  static int32_t z = 0, intg = 0;
+  int32_t p, e;
+
+  e = w;  e -= y;
+
+  p = e;  p *= (int32_t)CTRL_PI_K;
+  p -= z;
+  z = e*(int32_t)CTRL_PI_Kn;
+
+  if (intg > 0) {
+    if (p >= CTRL_INT_MAX - intg) {
+      intg = CTRL_INT_MAX;
+      led_set(LED_YEL, 1);
+    }
+    else {
+      intg += p;
+      led_set(LED_YEL, 0);
+      led_set(LED_BLU, 0);
+    }
+  }
+  else {
+    if (p <= -CTRL_INT_MAX - intg) {
+      intg = -CTRL_INT_MAX;
+      led_set(LED_BLU, 1);
+    }
+    else {
+      intg += p;
+      led_set(LED_YEL, 0);
+      led_set(LED_BLU, 0);
+    }
+  }
+
+  return intg/CTRL_INT_DIV;
+}
+
+#if 0
+/*** kmitani -- zobrazeni prechodaku ***/
+void control(int32_t ad_i, int16_t ad_x) {
+  int32_t u;
+
+#define REF_PERIOD 60
+  static int counter = 0;
+  float w;
+
+  if (counter++ == 2*REF_PERIOD)
+    counter = 0;
+  w = (counter >= REF_PERIOD) ? (1.0/CTRL_AMP_MUL) : 0.0; //(-1.0/CTRL_AMP_MUL);
+
+  u = pi_ctrl(w, ad_i);
+
+  hbridge_set(u);
+  current_negative = (u < 0);
+
+  ((int16_t*)tx_msg.data)[counter%4] = ad_i;
+  if (counter%4 == 3)
+    can_tx_msg(&tx_msg);
+}
+#endif
+
+volatile int16_t control_w = 0; /* reference current (*12) */
+volatile int32_t control_y; /* measured current (*12*80) */
+volatile int32_t control_x; /* measured position (*80) */
+volatile int32_t control_u; /* applied PWM (*80) */
+volatile int8_t control_on = 0; /* switches H-bridge on/off */
+
+int32_t ctrl_ma_filter(int32_t *mem, uint8_t *idx, int32_t val) {
+  int32_t diff = val - mem[*idx];
+  mem[*idx] = val;
+  if (++*idx == CAN_OVERSAMP)
+    *idx = 0;
+  return diff;
+}
+
+void control(int32_t ad_i, int16_t ad_x) {
+  static int32_t cy = 0, cx = 0, cu = 0;
+  static int ma_y[CAN_OVERSAMP],
+    ma_x[CAN_OVERSAMP], ma_u[CAN_OVERSAMP];
+  static uint8_t idx_y = 0, idx_x = 0, idx_u = 0;
+  int32_t u;
+
+#if 0
+  /* boj s blbym merenim proudu */
+#define CTRL_I_MIN 80
+  static int8_t sub_pwm = 0;
+  int32_t cw, im;
+  if (control_w > 0) {
+    cw = (control_w + 1)/2;
+    im = 2*CTRL_I_MIN;
+  }
+  else {
+    cw = (-control_w - 1)/2;
+    im = -2*CTRL_I_MIN;
+  }
+  if (cw < CTRL_I_MIN)
+    u = pi_ctrl((cw > sub_pwm) ? im : 0, ad_i);
+  else
+    u = pi_ctrl(control_w, ad_i);
+  sub_pwm = (sub_pwm + 1) % CTRL_I_MIN;
+  /* job */
+#else
+  u = pi_ctrl(control_w, ad_i);
+#endif
+
+  if (control_on)
+    hbridge_set(u);
+  current_negative = (u < 0);
+
+  cy += ctrl_ma_filter(ma_y, &idx_y, ad_i);
+  cx += ctrl_ma_filter(ma_x, &idx_x, ad_x);
+  cu += ctrl_ma_filter(ma_u, &idx_u, u);
+  /***** pozustak mylne honby za synchronizaci... ****/
+  //control_y = 80*ad_i; //cy;
+  //control_x = 80*ad_x; //cx;
+  //control_u = 80*u; //cu;
+  control_y = cy;
+  control_x = cx;
+  control_u = cu;
+}
+
+/*****/
+
+void adc_irq() {
+  static uint8_t ma_idx = 0;
+  static int32_t ma_val[ADC_MA_LEN];
+  static int8_t negative = 0;
+  int32_t ad;
+  int32_t ma_diff;
+  int8_t last;
+
+  /* read & clear irq flag */
+  ad = ADDR;
+
+  /* reset MAT0.1 */
+  T0EMR = 0x1<<6 | 0x2; /* must be here due to ADC.5 erratum */
+
+  if ((last = (timer_count == ADC_OVERSAMP-1))) {
+    /* last sample before PWM period end */
+    /* twice -- ADC.2 erratum workaround */
+    ADCR = adc_setup(1<<ADCIN_POS, 14, 1, 0x0 /* now! */, 1);
+    ADCR = adc_setup(1<<ADCIN_POS, 14, 1, 0x1 /* now! */, 1);
+  }
+
+  ad = (ad>>6)&0x3ff;
+
+  /***! odpor na 3.3V !***/
+  //static int32_t ad_zero = 5.0/(12*CTRL_AMP_MUL);
+  //if (!control_on) ad_zero = ad;
+  //ad -= ad_zero;
+
+  /**** boj s kmitanim ****/
+  //ad += 24;
+  /****/
+  if (negative)
+    ad = -ad;
+  /* shift value through MA filter */
+  ma_diff = ad - ma_val[ma_idx];
+  ma_val[ma_idx] = ad;
+  if (++ma_idx == ADC_MA_LEN)
+    ma_idx = 0;
+  /* MA filter output (should be atomic): */
+  adc_i += ma_diff;
+
+  if (last) {
+    while (((ad = ADDR)&0x80000000) == 0);
+    adc_x = (ad>>6)&0x3ff;
+    /* twice -- ADC.2 erratum workaround */
+    ADCR = adc_setup(1<<ADCIN_CURRENT, 14, 1, 0x0 /*MAT0.1*/, 1);
+    ADCR = adc_setup(1<<ADCIN_CURRENT, 14, 1, 0x4 /*MAT0.1*/, 1);
+    timer_count = 0;
+    negative = current_negative;
+  }
+  else
+    ++timer_count;
+
+  /* int acknowledge */
+  VICVectAddr = 0;
+}
+
+/******/
+
+uint8_t DBG_clk_seq = 0;
+
+void can_rx(){
+
+  led_set(LED_YEL, 1);
+  switch (rx_msg.id) {
+  case CAN_CLOCK_ID:
+    if ((uint16_t)rx_cmd_value != CTRL_OFF) {
+      control_w = rx_cmd_value;
+      control_on = 1;
+      DBG_clk_seq = rx_msg.data[0];
+    }
+    tx_request = 1;
+    break;
+  case CAN_CFGMSG_ID:
+    if (((uint16_t*)rx_msg.data)[0] == MOTOR_ID) {
+      cmd_msg_id = ((uint16_t*)rx_msg.data)[1];
+      cmd_msg_ndx = ((uint16_t*)rx_msg.data)[2];
+    }
+    break;
+  default:
+    if (rx_msg.id == cmd_msg_id) {
+      rx_cmd_value = ((int16_t*)rx_msg.data)[cmd_msg_ndx]; //zde si nejsem jisty tito pretypovanim
+      if ((uint16_t)rx_cmd_value == CTRL_OFF) {
+       control_on = 0;
+       hbridge_off();
+      }
+    }
+    break;
+  }
+  
+  led_set(LED_YEL, 0);
+}
+uint32_t btr = 0;
+void can_tx() {
+
+  /* x: 16b, y: 24b, u: 24b */
+  uint16_t control_2 = (uint16_t)(control_x/2);
+  tx_msg.data[0] = *((uint8_t*)&control_2);
+  tx_msg.data[1] = *((uint8_t*)&control_2 + 1);
+  tx_msg.data[2] = *((uint8_t*)&control_y);
+//  tx_msg.data[2] = *((uint8_t*)&rx_cmd_value+1);
+  tx_msg.data[3] = *((uint8_t*)&control_y + 1);
+//  tx_msg.data[3] = *((uint8_t*)&rx_cmd_value);
+  tx_msg.data[4] = *((uint8_t*)&control_y + 2);
+  tx_msg.data[5] = *((uint8_t*)&control_u);
+  tx_msg.data[6] = *((uint8_t*)&control_u + 1);
+  tx_msg.data[7] = *((uint8_t*)&control_u + 2);
+ 
+  /* do NOT wait */
+  vca_send_msg_seq(can,&tx_msg,1);
+  tx_request = 0;
+}
+
+/*****************/
+
+void sys_pll_init(uint32_t f_cclk, uint32_t f_osc) {
+  uint32_t m, p, p_max;
+
+  led_set(LED_YEL, 1);
+
+  /* turn memory acceleration off */
+  MAMCR = 0x0;
+
+  /* compute PLL divider and internal osc multiplier */
+  m = f_cclk/f_osc;
+  p_max = 160000000/f_cclk;
+  for (p = 3; ((1<<p) > p_max) && (p > 0); p = p>>1);
+  /* switch PLL on */
+  PLLCFG = (p<<5) | ((m-1)&0x1f);
+  PLLCON = 0x1;
+  PLLFEED = 0xaa;  PLLFEED = 0x55;
+  /* wait for lock */
+  while (!(PLLSTAT & (1<<10)));
+  /* connect to clock */
+  PLLCON = 0x3;
+  PLLFEED = 0xaa;  PLLFEED = 0x55;
+
+  /* turn memory acceleration on */
+  MAMTIM = f_cclk/20000000;
+  MAMCR = 0x2;
+
+  led_set(LED_YEL, 0);
+}
+
+int main() {
+  led_init();
+  led_set(LED_GRN, 1);
+  /* switch the CAN off due to clock change */
+  //can_off(0);
+  /* boost clock to 60MHz */
+  sys_pll_init(60000000, 10000000);
+  /* peripheral clock = CPU clock (60MHz) */
+  VPBDIV = 1;
+
+  /* init Vector Interrupt Controller */
+  VICIntEnClr = 0xFFFFFFFF;
+  VICIntSelect = 0x00000000;
+
+  /* CAN bus setup */
+  
+  /*inicialice CANu*/
+  lpcan_btr(&btr, 1000000 /*Bd*/, 60000000, 0/*SJW*/, 70/*%sampl.pt.*/, 0/*SAM*/);
+  lpc_vca_open_handle(&can, 0/*device*/, 1/*flags*/, btr, 10, 11, 12); 
+
+  tx_msg.id = MOTOR_ID;
+
+  hbridge_init();
+//  hbridge_set((PWM_PERIOD/ADC_OVERSAMP-2*PWM_DEAD));
+
+  set_irq_handler(18 /*ADC*/, 13, adc_irq);
+  adc_init();
+  timer_init();
+
+
+  for (;;) {
+    if(vca_rec_msg_seq(can,&rx_msg,1)<1){
+     if (timer_count == 1)
+       control(adc_i, adc_x);
+     if (tx_request)
+       can_tx();
+    }
+    else can_rx();
+  }
+}

diff --git a/app/spejbl_motor/pwm.c b/app/spejbl_motor/pwm.c
new file mode 100644 (file)
index 0000000..d6879d4
--- /dev/null
+++ b/app/spejbl_motor/pwm.c
@@ -0,0 +1,66 @@
+#include <cpu_def.h>
+#include "pwm.h"
+
+int PWM_PINSEL[] = {
+  /*nothing*/ 1, /*PWM1*/ 1, 15, 3, 17, /*PWM5*/ 11, /*PWM6*/ 19
+};
+
+uint32_t *PWM_MR[] = {
+  (uint32_t*)&(PWMMR0),
+  (uint32_t*)&(PWMMR1),
+  (uint32_t*)&(PWMMR2),
+  (uint32_t*)&(PWMMR3),
+  (uint32_t*)&(PWMMR4),
+  (uint32_t*)&(PWMMR5),
+  (uint32_t*)&(PWMMR6)
+};
+
+void pwm_channel(int n, int double_edge) {
+  uint32_t bit;
+
+  PWMPCR |= (0x100 | (double_edge && n)) << n;
+  if (n == 5) {
+    PINSEL1 |= 0x00000400;
+    PINSEL1 &= 0xfffff7ff;
+  }
+  else {
+    bit = 1 << PWM_PINSEL[n];
+    PINSEL0 |= bit;
+    bit = ~(bit >> 1);
+    PINSEL0 &= bit;
+  }
+}
+
+void pwm_set(int n, uint32_t when) {
+  *PWM_MR[n] = when;
+  PWMLER |= 1 << n;
+}
+
+void pwm_set_double(int n, uint32_t from, uint32_t to) {
+  *PWM_MR[n-1] = from;
+  *PWM_MR[n] = to;
+  PWMLER |= 0x3 << (n-1);
+}
+
+void pwm_init(uint32_t prescale, uint32_t period) {
+  PWMPR = prescale - 1;
+  PWMMR0 = period;
+  PWMLER |= 0x1;
+  PWMMCR |= 0x00000002;
+  PWMTCR &= ~0x2;
+  PWMTCR |= 0x9;
+}
+
+void sync_pwm_timer(uint32_t *tc_addr) {
+  cli();
+  asm volatile
+    (
+     "mov  r2, %0       \n\t"
+     "mov  r3, %1       \n\t"
+     "ldr  r1, [r2]     \n\t"
+     "add  r1, r1, #12  \n\t"
+     "str  r1, [r3]     \n\t"
+     : /* no output */ : "r" (tc_addr), "r" (&PWMTC)
+     );
+  sti();
+}

diff --git a/app/spejbl_motor/pwm.h b/app/spejbl_motor/pwm.h
new file mode 100644 (file)
index 0000000..fe69077
--- /dev/null
+++ b/app/spejbl_motor/pwm.h
@@ -0,0 +1,11 @@
+#include <lpc21xx.h>
+#include <types.h>
+
+extern int PWM_PINSEL[];
+extern uint32_t *PWM_MR[];
+
+void pwm_channel(int n, int double_edge);
+void pwm_set(int n, uint32_t when);
+void pwm_set_double(int n, uint32_t from, uint32_t to);
+void pwm_init(uint32_t prescale, uint32_t period);
+void sync_pwm_timer(uint32_t *tc_addr);