From 9a1c26e36849900a55c55c11f811e7a18ef1064e Mon Sep 17 00:00:00 2001 From: eddyem Date: Wed, 22 Apr 2015 12:26:54 +0300 Subject: [PATCH] Steppers worked & tested; inner ADC works fine; add median filtering by 9 points for TRD resistance values --- with_opencm3/hardware_ini.c | 31 +++++++++++++++++----- with_opencm3/hardware_ini.h | 6 +++-- with_opencm3/powerhw.c | 6 ++--- with_opencm3/stepper_motors.c | 49 ++++++++++++++++++++++++++--------- 4 files changed, 69 insertions(+), 23 deletions(-) diff --git a/with_opencm3/hardware_ini.c b/with_opencm3/hardware_ini.c index 007591a..3ad0443 100644 --- a/with_opencm3/hardware_ini.c +++ b/with_opencm3/hardware_ini.c @@ -45,7 +45,9 @@ * U10 -> 0 PA0 */ uint8_t adc_channel_array[16] = {9,8,15,14,7,6,5,4,1,0}; -volatile uint16_t ADC_value[ADC_CHANNELS_NUMBER]; // ADC DMA value +// ADC_value array have 9 records for each value for ability of median filtering +// values for channel num are ADC_value[num + ADC_CHANNELS_NUMBER*i] +volatile uint16_t ADC_value[ADC_CHANNELS_NUMBER*9]; // ADC DMA value /* * Configure SPI ports @@ -164,7 +166,7 @@ void adc_dma_on(){ dma_channel_reset(DMA1, DMA_CHANNEL1); DMA1_CPAR1 = (uint32_t) &(ADC_DR(ADC1)); DMA1_CMAR1 = (uint32_t) ADC_value; - DMA1_CNDTR1 = ADC_CHANNELS_NUMBER; + DMA1_CNDTR1 = ADC_CHANNELS_NUMBER * 9; // *9 for median filtering DMA1_CCR1 = DMA_CCR_MINC | DMA_CCR_PSIZE_16BIT | DMA_CCR_MSIZE_16BIT | DMA_CCR_CIRC | DMA_CCR_PL_HIGH | DMA_CCR_EN; adc_enable_dma(ADC1); @@ -253,20 +255,37 @@ int power_voltage(){ return (int)val; } +// This is some data for optimal median filtering of array[9] +uint16_t temp; +#define PIX_SORT(a,b) { if ((a)>(b)) PIX_SWAP((a),(b)); } +#define PIX_SWAP(a,b) { temp=(a);(a)=(b);(b)=temp; } +uint16_t p[9]; // buffer for median filtering +uint16_t opt_med9(){ + PIX_SORT(p[1], p[2]) ; PIX_SORT(p[4], p[5]) ; PIX_SORT(p[7], p[8]) ; + PIX_SORT(p[0], p[1]) ; PIX_SORT(p[3], p[4]) ; PIX_SORT(p[6], p[7]) ; + PIX_SORT(p[1], p[2]) ; PIX_SORT(p[4], p[5]) ; PIX_SORT(p[7], p[8]) ; + PIX_SORT(p[0], p[3]) ; PIX_SORT(p[5], p[8]) ; PIX_SORT(p[4], p[7]) ; + PIX_SORT(p[3], p[6]) ; PIX_SORT(p[1], p[4]) ; PIX_SORT(p[2], p[5]) ; + PIX_SORT(p[4], p[7]) ; PIX_SORT(p[4], p[2]) ; PIX_SORT(p[6], p[4]) ; + PIX_SORT(p[4], p[2]) ; return(p[4]) ; +} +#undef PIX_SORT +#undef PIX_SWAP /** - * Resistance of TRD + * Resistance of TRD with median filtering * @param num - number of sensor * @return R*100 - * we measure */ int TRD_value(uint8_t num){ - uint32_t v = ADC_value[num]; + int i, addr = num; + for(i = 0; i < 9; i++, addr+=ADC_CHANNELS_NUMBER) // first we should prepare array for optmed + p[i] = ADC_value[addr]; + uint32_t v = opt_med9(); uint32_t r = v * ADC_multipliers[num]; r /= (uint32_t)(4096 - v) * ADC_divisors[num]; return (int) r; } - uint16_t tim2_buff[TIM2_DMABUFF_SIZE]; uint16_t tim2_inbuff[TIM2_DMABUFF_SIZE]; int tum2buff_ctr = 0; diff --git a/with_opencm3/hardware_ini.h b/with_opencm3/hardware_ini.h index ee242b4..25784cc 100644 --- a/with_opencm3/hardware_ini.h +++ b/with_opencm3/hardware_ini.h @@ -97,8 +97,10 @@ void ADC_calibrate_and_start(); #define MOTOR_TIM1_PIN (GPIO6) #define MOTOR_TIM2_PORT (GPIOD) #define MOTOR_TIM2_PIN (GPIO15) -// don't even try to move motor if motors' voltage less than 9.5V -#define MOTORS_VOLTAGE_THRES (950) +// don't even try to move motor if motors' voltage less than 9.0V +#define MOTORS_VOLTAGE_THRES (900) +// stop all motors if voltage less than 7.5V +#define MOTORS_VOLTAGE_ALERT (750) /* * One Wire interface diff --git a/with_opencm3/powerhw.c b/with_opencm3/powerhw.c index 4f300d3..91c59e5 100644 --- a/with_opencm3/powerhw.c +++ b/with_opencm3/powerhw.c @@ -241,6 +241,8 @@ shutter_state shutter_init(){ // gpio_set(SHUTTER_EXT_PORT, SHUTTER_CAM_PIN | SHUTTER_MAN_PIN | SHUTTER_FBSW_PIN); // turn on pull up //DBG("shutter fb ready\n"); shutter_off(); + camera_pin_old_state = (gpio_get(SHUTTER_EXT_PORT, SHUTTER_CAM_PIN)) ? 1 : 0; + manual_pin_old_state = (gpio_get(SHUTTER_EXT_PORT, SHUTTER_MAN_PIN)) ? 1 : 0; //shutter_timer_fn = NULL; shutter_wait_block(SHUTTER_OP_DELAY, shutter_test); return SHUTTER_INITIALIZED; // we return this state in spite of the shutter isn't really initialized yet @@ -270,9 +272,7 @@ void process_shutter(){ man_pin_state = (gpio_get(SHUTTER_EXT_PORT, SHUTTER_MAN_PIN)) ? 1 : 0; // to avoid opening shutter if user forget to set manual switch to "closed" position // all operations with manual switch processed only in changing state of the switch - if(manual_pin_old_state == -1){ // refresh manual pin state - manual_pin_old_state = man_pin_state; - }else if(manual_pin_old_state != man_pin_state){ // user changed switch state -> open/close + if(manual_pin_old_state != man_pin_state){ // user changed switch state -> open/close manual_pin_old_state = man_pin_state; //changed_manually = 1; if(man_pin_state){ // close diff --git a/with_opencm3/stepper_motors.c b/with_opencm3/stepper_motors.c index 7a46731..7af1f12 100644 --- a/with_opencm3/stepper_motors.c +++ b/with_opencm3/stepper_motors.c @@ -189,27 +189,47 @@ uint8_t check_ep(uint8_t num){ return 0; } - /** - * Move motor Motor_number to User_value steps - * return 0 if motor is still moving + * test 12V voltage + * if it is less than MOTORS_VOLTAGE_THRES, return 1 & show err message + * else return 0 */ -uint8_t move_motor(uint8_t num, int32_t steps){ - uint8_t curpos, negative_dir = 0; - if(steps == 0) return 0; - // check whether motor is moving - if(Motor_active[num]){ - ERR("moving\n"); - return 0; - } +uint8_t undervoltage_test(int thres){ int voltage = power_voltage(); - if(voltage < MOTORS_VOLTAGE_THRES){ + if(voltage < thres){ ERR("undervoltage!\n"); if(mode == LINE_MODE){ P("[ " STR_MOTORS_VOLTAGE " ", lastsendfun); print_int(voltage, lastsendfun); P(" ]\n", lastsendfun); } + return 1; + } + return 0; +} + +/** + * Move motor Motor_number to User_value steps + * return 0 if motor is still moving + */ +uint8_t move_motor(uint8_t num, int32_t steps){ + uint8_t curpos, negative_dir = 0, N_active_in_group = 0; + if(steps == 0) return 0; + // check whether motor is moving +/* if(Motor_active[num]){ + ERR("moving\n"); + return 0; + }*/ + // don't move motors if there's no power enough + if(undervoltage_test(MOTORS_VOLTAGE_THRES)) return 0; + if(num < 4){ + for(curpos = 0; curpos < 4; curpos++) + if(Motor_active[curpos]) N_active_in_group++; + }else{ + if(Motor_active[3] || Motor_active[4]) N_active_in_group = 1; + } + if(N_active_in_group){ // we can't move: there's any active motor in group + ERR("moving\n"); return 0; } #ifdef EBUG @@ -330,6 +350,11 @@ void process_stepper_motors(){ for(i = startno[j]; i < stopno[j]; i++) if(Motor_active[i]) is_active = 1; if(!is_active) continue; // don't generate clock pulses when there's no moving motors + if(undervoltage_test(MOTORS_VOLTAGE_ALERT)){ // UNDERVOLTAGE! Stop all active motors + for(i = 0; i < 5; i++) + if(Motor_active[i]) stop_motor(i); + return; + } gpio_toggle(ports[j], pins[j]); // change clock state if(!gpio_get(ports[j], pins[j])){ // negative pulse - omit this half-step continue;