From 9e173c14bfa8cb633de9793f4385b091796c816f Mon Sep 17 00:00:00 2001 From: Martin Rakovec Date: Thu, 14 May 2009 23:22:33 +0200 Subject: [PATCH] New software for Spejbl motor control now on lpcanvca. --- app/can_HelloWorld/main.c | 28 ++- app/spejbl_hodiny/Makefile | 14 ++ app/spejbl_hodiny/Makefile.omk | 7 + app/spejbl_hodiny/hodiny.c | 67 ++++++ app/spejbl_motor/Makefile | 14 ++ app/spejbl_motor/Makefile.omk | 6 + app/spejbl_motor/led.c | 35 +++ app/spejbl_motor/led.h | 20 ++ app/spejbl_motor/motorx.c | 509 +++++++++++++++++++++++++++++++++++++++++ app/spejbl_motor/pwm.c | 66 ++++++ app/spejbl_motor/pwm.h | 11 + 11 files changed, 768 insertions(+), 9 deletions(-) create mode 100644 app/spejbl_hodiny/Makefile create mode 100644 app/spejbl_hodiny/Makefile.omk create mode 100644 app/spejbl_hodiny/hodiny.c create mode 100644 app/spejbl_motor/Makefile create mode 100644 app/spejbl_motor/Makefile.omk create mode 100644 app/spejbl_motor/led.c create mode 100644 app/spejbl_motor/led.h create mode 100644 app/spejbl_motor/motorx.c create mode 100644 app/spejbl_motor/pwm.c create mode 100644 app/spejbl_motor/pwm.h diff --git a/app/can_HelloWorld/main.c b/app/can_HelloWorld/main.c index 9ce87f8..c623c6b 100644 --- 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 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 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 index 0000000..8e954dd --- /dev/null +++ b/app/spejbl_hodiny/hodiny.c @@ -0,0 +1,67 @@ +/* LPC21xx definitions */ +#include +/* CAN definitions*/ +#include +#include +#include + +#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 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 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 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 index 0000000..167a4a4 --- /dev/null +++ b/app/spejbl_motor/led.h @@ -0,0 +1,20 @@ +#include +#include + +#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 index 0000000..0820319 --- /dev/null +++ b/app/spejbl_motor/motorx.c @@ -0,0 +1,509 @@ +#include +#include +#include +#include +#include +/* CAN definitions*/ +#include +#include +#include +// +#include +#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< 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<>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< 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 index 0000000..d6879d4 --- /dev/null +++ b/app/spejbl_motor/pwm.c @@ -0,0 +1,66 @@ +#include +#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 index 0000000..fe69077 --- /dev/null +++ b/app/spejbl_motor/pwm.h @@ -0,0 +1,11 @@ +#include +#include + +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); -- 2.11.4.GIT