From 1faa5f5cb1a8e716b02550730f505aceaaeb609c Mon Sep 17 00:00:00 2001 From: Martin Rakovec Date: Thu, 2 Jul 2009 15:51:08 +0200 Subject: [PATCH] New version of spejbl motor software --- app/spejbl_motor/Makefile.omk | 5 +- app/spejbl_motor/constants.h | 45 +++++++ app/spejbl_motor/hbridge.c | 73 ++++++++++++ app/spejbl_motor/hbridge.h | 16 +++ app/spejbl_motor/led.h | 3 + app/spejbl_motor/motorx.c | 268 ++++++------------------------------------ 6 files changed, 175 insertions(+), 235 deletions(-) create mode 100644 app/spejbl_motor/constants.h create mode 100644 app/spejbl_motor/hbridge.c create mode 100644 app/spejbl_motor/hbridge.h diff --git a/app/spejbl_motor/Makefile.omk b/app/spejbl_motor/Makefile.omk index b61563a..d39a81b 100644 --- a/app/spejbl_motor/Makefile.omk +++ b/app/spejbl_motor/Makefile.omk @@ -1,6 +1,3 @@ -# -*- makefile -*- - bin_PROGRAMS = spejblmotor -spejblmotor_SOURCES = motorx.c pwm.c led.c +spejblmotor_SOURCES = motorx.c pwm.c led.c hbridge.c spejblmotor_LIBS = lpcanvca lpcan -#link_VARIANTS = mpram \ No newline at end of file diff --git a/app/spejbl_motor/constants.h b/app/spejbl_motor/constants.h new file mode 100644 index 0000000..2d15e32 --- /dev/null +++ b/app/spejbl_motor/constants.h @@ -0,0 +1,45 @@ +/****************************************/ +/* Constants definition for */ +/* robot Spejbl - motor */ +/****************************************/ + +/* CAN message IDs */ +#define CAN_CLOCK_ID 0x401 +#define CAN_CFGMSG_ID 0x4ab +#define CAN_UNDEF_ID 0xf800 + +#define CANLOAD_ID 0x481 +//(*((volatile unsigned long *) 0x40000120)) +#define MOTOR_ID CANLOAD_ID + +/* CAN timing */ +#define CAN_OVERSAMP 80 + +#define ADC_OVERSAMP 6 +#define ADC_MA_LEN (2*ADC_OVERSAMP) + +/* position limits */ +#define POS_MIN 0x060 +#define POS_MAX 0x3a0 + +/* input channels */ +#define ADCIN_POS 0 +#define ADCIN_CURRENT 1 + +/* reserved value for power-off */ +#define CTRL_OFF 0xffff +#define CTRL_MAX SHRT_MAX + +//#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))) + +/*controler constants*/ +#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) + +/* PWM and sampling timing */ +#define PWM_PERIOD 3000 +#define PWM_DEAD 60 diff --git a/app/spejbl_motor/hbridge.c b/app/spejbl_motor/hbridge.c new file mode 100644 index 0000000..4de52cf --- /dev/null +++ b/app/spejbl_motor/hbridge.c @@ -0,0 +1,73 @@ +#include "hbridge.h" +#include "pwm.h" + +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); +} diff --git a/app/spejbl_motor/hbridge.h b/app/spejbl_motor/hbridge.h new file mode 100644 index 0000000..816d66f --- /dev/null +++ b/app/spejbl_motor/hbridge.h @@ -0,0 +1,16 @@ +/****************************************/ +/* H-bridge control function */ +/****************************************/ + +#include +#include +#include "constants.h" + +/* H-bridge set */ +void hbridge_half_set(uint8_t side, uint32_t value); +/* H-bridge set */ +void hbridge_set(int32_t value); +/* Switch off H-bridge */ +void hbridge_off(); +/* Init H-bridge */ +void hbridge_init(); diff --git a/app/spejbl_motor/led.h b/app/spejbl_motor/led.h index 167a4a4..824e9a9 100644 --- a/app/spejbl_motor/led.h +++ b/app/spejbl_motor/led.h @@ -13,8 +13,11 @@ #define LED_GRN 2 #define LED_BLU 3 +/* LED init */ void led_init(); +/* Set LED state */ void led_set(uint8_t led, uint8_t state); +/* Switch LED state */ void led_toggle(uint8_t led); /* .oOo. */ diff --git a/app/spejbl_motor/motorx.c b/app/spejbl_motor/motorx.c index ef601bc..0362405 100644 --- a/app/spejbl_motor/motorx.c +++ b/app/spejbl_motor/motorx.c @@ -3,77 +3,61 @@ #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 +#include "hbridge.h" +#include "constants.h" #define adc_setup(src,clkdiv,on,start,edge) \ (((src)&0x7) | ((((clkdiv)-1)&0xff)<<8) | ((!(!(on)))<<21) | \ (((start)&0x7)<<24) | ((!(!(edge)))<<27)) +/********* Variables **********/ + /* 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*/ +/* CAN handle */ vca_handle_t can; -/* own message for current position signalling */ +/* CAN Bus Timing register value */ +uint32_t btr = 0; +/* Own message for current position signalling */ canmsg_t tx_msg = {.flags = 0, .length = 8}; +/* Recieved message */ canmsg_t rx_msg; -/* command message ID and index */ +/* Command message ID and index */ uint16_t cmd_msg_id = CAN_UNDEF_ID, cmd_msg_ndx = 0; -/* command value, current position */ +/* Command value, current position */ volatile int16_t rx_cmd_value = CTRL_OFF; volatile uint8_t tx_request = 0; +/***/ +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 */ +/***/ + +uint8_t DBG_clk_seq = 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 */ @@ -96,89 +80,7 @@ inline void adc_init() { ADCR = adc_setup(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) - +/* Saturation */ inline int32_t add_sat(int32_t big, int32_t small, int32_t min, int32_t max) { if (big >= max - small) return max; @@ -187,6 +89,7 @@ inline int32_t add_sat(int32_t big, int32_t small, int32_t min, int32_t max) { return big + small; } +/* PI controler */ int32_t pi_ctrl(int16_t w, int16_t y) { static int32_t s = 0; int32_t p, e, u, q; @@ -200,73 +103,7 @@ int32_t pi_ctrl(int16_t w, int16_t y) { 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 */ - +/* MA filter */ int32_t ctrl_ma_filter(int32_t *mem, uint8_t *idx, int32_t val) { int32_t diff = val - mem[*idx]; mem[*idx] = val; @@ -275,6 +112,7 @@ int32_t ctrl_ma_filter(int32_t *mem, uint8_t *idx, int32_t val) { return diff; } +/**** Control ****/ 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], @@ -282,28 +120,7 @@ void control(int32_t ad_i, int16_t ad_x) { 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); @@ -312,17 +129,12 @@ void control(int32_t ad_i, int16_t ad_x) { 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]; @@ -346,14 +158,6 @@ void adc_irq() { 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 */ @@ -382,12 +186,9 @@ void adc_irq() { /******/ -uint8_t DBG_clk_seq = 0; - void can_rx(){ - - led_set(LED_YEL, 1); switch (rx_msg.id) { + /*message from CAN clock*/ case CAN_CLOCK_ID: if ((uint16_t)rx_cmd_value != CTRL_OFF) { control_w = rx_cmd_value; @@ -396,15 +197,22 @@ void can_rx(){ } tx_request = 1; break; + /*config message from control computer*/ case CAN_CFGMSG_ID: + /*is this message for this motor?*/ if ((rx_msg.data[0] | ((rx_msg.data[1])<<8)) == MOTOR_ID) { + /*ID of control message for this motor*/ cmd_msg_id = rx_msg.data[2] | ((rx_msg.data[3])<<8); + /*part of control message*/ cmd_msg_ndx = rx_msg.data[4] | ((rx_msg.data[5])<<8); } break; default: + /*control message for this motor*/ if (rx_msg.id == cmd_msg_id) { + /*current value*/ rx_cmd_value = rx_msg.data[cmd_msg_ndx] | ((rx_msg.data[cmd_msg_ndx+1])<<8); + /*switch off motor*/ if ((uint16_t)rx_cmd_value == CTRL_OFF) { control_on = 0; hbridge_off(); @@ -412,10 +220,8 @@ void can_rx(){ } break; } - - led_set(LED_YEL, 0); } -uint32_t btr = 0; + void can_tx() { /* x: 16b, y: 24b, u: 24b */ @@ -435,7 +241,7 @@ void can_tx() { tx_msg.data[5] = *((uint8_t*)&cmd_msg_ndx); tx_msg.data[6] = *((uint8_t*)&rx_cmd_value+1); tx_msg.data[7] = *((uint8_t*)&rx_cmd_value);*/ - /* do NOT wait */ + /* send message and do NOT wait*/ vca_send_msg_seq(can,&tx_msg,1); tx_request = 0; } -- 2.11.4.GIT