X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fsensorboard%2Fgen_scan_tab.c;fp=projects%2Fmicrob2010%2Fsensorboard%2Fgen_scan_tab.c;h=0000000000000000000000000000000000000000;hp=7b94bfd6d95338b26e343fc2c17f757d88998015;hb=8d6a47e9e21a9a31f4bc12d32fb3d11091a4b305;hpb=821f753c0f88aff895d9feae59c442a6c446f96b diff --git a/projects/microb2010/sensorboard/gen_scan_tab.c b/projects/microb2010/sensorboard/gen_scan_tab.c deleted file mode 100644 index 7b94bfd..0000000 --- a/projects/microb2010/sensorboard/gen_scan_tab.c +++ /dev/null @@ -1,680 +0,0 @@ -#include -#include -#include -#include - -#include - - -#include "scanner.h" - -#define ADC_REF_AVCC 0 -#define MUX_ADC13 0 - - - -double TELEMETRE_A = TELEMETRE_A_INIT; -double TELEMETRE_B = TELEMETRE_B_INIT; - - -#define printf_P printf -#define PSTR(a) (a) -int32_t motor_angle = 0; -int32_t scan_dist = 0; - - -int32_t H_fin, L_fin; -double L, H; - -#define nop() - -#define ABS(val) ({ \ - __typeof(val) __val = (val); \ - if (__val < 0) \ - __val = - __val; \ - __val; \ - }) - -struct scan_params scan_params; - -int32_t encoders_spi_get_value_scanner_interpolation(void *a) -{ - return motor_angle; -} - - -int16_t adc_get_value(uint8_t a) -{ - return scan_dist; -} - - - -/* get motor angle in radian; return mirror angle in radian, cos a sin a */ -void ang2_a_mirror(double b, double * c_a, double* s_a, double* a) -{ - double x2, y2; - double A, DELTA, B, D; - - b+=scan_params.offset_b; - x2 = X + l1*cos(b); - y2 = Y + l1*sin(b); - - A = (l3*l3 + x2*x2 + y2*y2 - l2*l2)/(2*l3); - - DELTA = -(A*A - x2*x2 - y2*y2); - B = sqrt(DELTA); - - D = x2*x2 + y2*y2; - - *c_a = (x2*A + y2*B)/D; - *s_a = -(x2*B - y2*A)/D; - - *a = atan2(*s_a, *c_a); - - *a += scan_params.offset_a; - // *s_a = sin(*a); - // *c_a = cos(*a); - -} - -/* get telemeter dist , cos a, sin a, a and return H, L of scanned point */ -void ang2_H_L(double l_telemetre, double c_a, double s_a, double a, double *H, double *L) -{ - double d; - d = h_mirror*c_a/s_a; - *H = (l_telemetre - l_mirror - d)*sin(2*a); - *L = l_mirror + d + *H/tan(2*a); - - //*H+= 8*sin(a-scan_params.offset_a); -} - - - -//int32_t last_col_n; -//int32_t last_row_n; -//uint8_t last_sample; - -//uint8_t h_limit[] = {40, 53, 66, 78, 94, 111, 123}; -//uint8_t h_limit[] = {37, 48, 61, 72, 94, 111, 123}; - -/* last high is dummy, to deal higher columns */ -//uint8_t h_limit[] = {68, 79, 93, 107, 121, 138, 155, 170, 250}; - - -//uint8_t h_limit[] = {60, 72, 85, 98, 112, 129, 250}; - -//uint8_t h_limit[] = {80, 97, 118, 134, 145, 160, 250}; - - -//uint8_t h_limit[] = {79, 94, 108, 117, 129, 144, 250}; - -uint8_t h_limit[] = {83, 95, 108, 120, 135, 147, 164, 250}; -#define H_MARGIN (-6) - -#define cs_set_consign(a, b) - - - -void do_scan(void * dummy) -{ - - unsigned int i; - int16_t a; - int32_t row_n; - int32_t col_n; - - - int32_t tour_pos; - int32_t pos, last_pos; - int32_t pos_tmp ; - int32_t mot_pos; - double dist; - uint8_t min_sample; - - double b, c_a, s_a, /*H, L,*/ m_a; - // int32_t H_fin; - - - //printf("scan\n"); - if (scan_params.sample_i==0) - return; - - mot_pos = encoders_spi_get_value_scanner_interpolation((void *)SCANNER_ENC); -#if 0 - if (scan_params.sample_i==1){ - printf_P(PSTR("dump end enc %ld %d \r\n"), mot_pos, PIX_PER_SCAN); - //scanner.flags &= (~CS_ON); - - - - /* stop scan at cur pos + 10 round */ - mot_pos = encoders_spi_get_value_scanner_interpolation((void *)SCANNER_ENC); - mot_pos = SCANNER_STEP_TOUR * ((mot_pos/SCANNER_STEP_TOUR) + 1) ; - - printf_P(PSTR("set to %ld \r\n"), mot_pos); - - cs_set_consign(&sensorboard.scanner.cs, mot_pos); - //pwm_ng_set(SCANNER_MOT_PWM, 0); - - - } - - mot_pos-= scan_params.pos_start_scan; -#endif - a = adc_get_value( ADC_REF_AVCC | MUX_ADC13 ); - - - //dist = (a-TELEMETRE_B)/TELEMETRE_A; - //dist = TELEMETRE_A * a +TELEMETRE_B; - dist = telemetre_to_cm(a); - - //printf_P(PSTR("enc val = %ld\r\n"), encoders_microb_get_value((void *)SCANNER_ENC)); - - - //sample_tab[MAX_SAMPLE-sample_i] = a>0x1ff?0x1FF:a; - //sample_tab[MAX_SAMPLE-sample_i] |= PINF&2?0x200:0; - - - row_n = (mot_pos)/(SCANNER_STEP_TOUR/2); -#if 0 - /* separe scan forward/backword */ - if (row_n%2){ - row_n/=2; - } - else{ - row_n/=2; - row_n+=30; - } -#endif - - tour_pos = (mot_pos)%(SCANNER_STEP_TOUR); - - b = (2.*M_PI)*(double)tour_pos/(double)(SCANNER_STEP_TOUR); - - ang2_a_mirror(b, &c_a, &s_a, &m_a); - ang2_H_L(dist, c_a, s_a, m_a, &H, &L); - - - if (H >0){ - printf("zarb H\n"); - H = 0; - } - - if (dist> SCAN_MAX_DIST){ - H = 0; - L = 0; - } - - H = H;//m_a*180/M_PI; - L = L;//(L-100)*PIX_PER_SCAN; - - //printf_P(PSTR("polling : ADC0 = %i %f\r\n"),a, dist); - //printf_P(PSTR("%f %f %2.2f %f\r\n"), H, L, m_a*180./M_PI, dist); - - - //printf_P(PSTR("dist, b, a: %2.2f %2.2f %2.2f\r\n"), dist, b*180/M_PI, m_a*180/M_PI); - - H=(H+SCAN_H_MAX)*SCAN_H_COEF; - L-=SCAN_L_MIN; - - - /* XXX may never append */ - if (L<0) - L=0; - - - /* first filter => pixel modulo level */ -#define H_BASE 10 -#define H_MUL 14 - H_fin = H;//+SCAN_H_MAX; - //H_fin = ((H_fin-H_BASE)/H_MUL)*H_MUL + H_BASE; - - if (scan_params.filter){ - H_fin = 11; // default is level 11 - for (i=0;iPIX_PER_SCAN) - printf("BUG!!! RECALC MAX L\r\n"); - - //col_n = (PIX_PER_SCAN+col_n -5)%PIX_PER_SCAN; - - //pos = (row_n*SCANNER_STEP_TOUR + tour_pos)/STEP_PER_POS; - //pos= row_n*PIX_PER_SCAN+tour_pos; - //last_pos= last_row_n*PIX_PER_SCAN+last_tour_pos; - - - - pos= row_n*PIX_PER_SCAN+col_n; - last_pos= scan_params.last_row_n*PIX_PER_SCAN+scan_params.last_col_n; - - //printf_P(PSTR("%ld %ld %ld %ld\r\n"), row_n, col_n, pos, H_fin); - - //a-=0x100; - a-=200; - //a/=10; - - if (0<= pos && pos 0xff?0xFF:a; - //sample_tab[(int)L] = H ; - scan_params.sample_tab[pos] = H_fin; - nop(); - if ((scan_params.last_row_n == row_n) && ABS(last_pos-pos)>1){ - /* we have a hole, pad it with minimal hight */ - if (H_fin>scan_params.last_sample) - min_sample = scan_params.last_sample; - else - min_sample = H_fin; - - //printf("(%ld, %ld) (%ld %ld)\r\n", last_col_n, last_row_n, col_n, row_n); - - /* fill grow, avoid erasing curent pos */ - if (pos > last_pos){ - pos_tmp = last_pos; - last_pos = pos; - //printf("loop1 on (%ld, %ld) %ld\r\n", pos_tmp, last_pos, last_pos-pos_tmp); - - } - else{ - pos_tmp = pos+1; - //printf("loop1 on (%ld, %ld) %ld\r\n", pos_tmp, last_pos, last_pos-pos_tmp); - } - - - for (;pos_tmp< last_pos;pos_tmp++){ - if (0< pos_tmp && pos_tmp 0x1ff?0x1FF:a; - - //sample_ok_tab[MAX_SAMPLE-sample_i] = PORTF&2; - - /* - if (((pos =DIM_DIST) - j = DIM_DIST-1; - - if (i>=DIM_ANGLE) - i = DIM_ANGLE-1; - - - val.u16 = pgm_read_word_near(&array_h_l[j][i]); - - - //val.u16 = pgm_read_word_near(&array_h_l[(a-TELEMETRE_MIN)/DIST_STEP][mot_pos/ANGLE_STEP]); - //val.u16 = pgm_read_word_near(&array_h_l[a][tp]); - H = val.h_l.h; - L = val.h_l.l; - /* - val.u16 = pgm_read_word_near(&array_h_l[(a-TELEMETRE_MIN)/DIST_STEP][mot_pos/ANGLE_STEP]); - - H = val.h_l.h; - L = val.h_l.l; - */ - H_fin = H; - L_fin = L; - - - - col_n = (PIX_PER_SCAN*L)/(SCAN_L_MAX-SCAN_L_MIN); - if (col_n>PIX_PER_SCAN) - printf("BUG!!! RECALC MAX L\r\n"); - - //col_n = (PIX_PER_SCAN+col_n -5)%PIX_PER_SCAN; - - //pos = (row_n*SCANNER_STEP_TOUR + tour_pos)/STEP_PER_POS; - //pos= row_n*PIX_PER_SCAN+tour_pos; - //last_pos= last_row_n*PIX_PER_SCAN+last_tour_pos; - - row_n = (mot_pos)/(SCANNER_STEP_TOUR/2); - - - pos= row_n*PIX_PER_SCAN+col_n; - last_pos= scan_params.last_row_n*PIX_PER_SCAN+scan_params.last_col_n; - - //printf_P(PSTR("%ld %ld %ld %ld\r\n"), row_n, col_n, pos, H_fin); - - //a-=0x100; - a-=200; - //a/=10; - - if (0<= pos && pos 0xff?0xFF:a; - //sample_tab[(int)L] = H ; - scan_params.sample_tab[pos] = H_fin; - nop(); - if ((scan_params.last_row_n == row_n) && ABS(last_pos-pos)>1){ - /* we have a hole, pad it with minimal hight */ - if (H_fin>scan_params.last_sample) - min_sample = scan_params.last_sample; - else - min_sample = H_fin; - - //printf("(%ld, %ld) (%ld %ld)\r\n", last_col_n, last_row_n, col_n, row_n); - - /* fill grow, avoid erasing curent pos */ - if (pos > last_pos){ - pos_tmp = last_pos; - last_pos = pos; - //printf("loop1 on (%ld, %ld) %ld\r\n", pos_tmp, last_pos, last_pos-pos_tmp); - - } - else{ - pos_tmp = pos+1; - //printf("loop1 on (%ld, %ld) %ld\r\n", pos_tmp, last_pos, last_pos-pos_tmp); - } - - - for (;pos_tmp< last_pos;pos_tmp++){ - if (0< pos_tmp && pos_tmp 0x1ff?0x1FF:a; - - //sample_ok_tab[MAX_SAMPLE-sample_i] = PORTF&2; - - /* - if (((pos 1 && !strcmp(argv[1], "1")) - scan_params.filter = 1; - - - printf("max i max j %d %d \n", i, j); - dist_max = j; - angle_max = i; - - f_header = fopen("scan_h_l.h", "w"); - fprintf(f_header, "PROGMEM lookup_h_l array_h_l[%d][%d] = {\n", dist_max, angle_max); - for (j = 0, scan_dist = 0; j