11 #define ADC_REF_AVCC 0
16 double TELEMETRE_A = TELEMETRE_A_INIT;
17 double TELEMETRE_B = TELEMETRE_B_INIT;
20 #define printf_P printf
22 int32_t motor_angle = 0;
23 int32_t scan_dist = 0;
32 __typeof(val) __val = (val); \
38 struct scan_params scan_params;
40 int32_t encoders_spi_get_value_scanner_interpolation(void *a)
46 int16_t adc_get_value(uint8_t a)
53 /* get motor angle in radian; return mirror angle in radian, cos a sin a */
54 void ang2_a_mirror(double b, double * c_a, double* s_a, double* a)
57 double A, DELTA, B, D;
59 b+=scan_params.offset_b;
63 A = (l3*l3 + x2*x2 + y2*y2 - l2*l2)/(2*l3);
65 DELTA = -(A*A - x2*x2 - y2*y2);
70 *c_a = (x2*A + y2*B)/D;
71 *s_a = -(x2*B - y2*A)/D;
73 *a = atan2(*s_a, *c_a);
75 *a += scan_params.offset_a;
81 /* get telemeter dist , cos a, sin a, a and return H, L of scanned point */
82 void ang2_H_L(double l_telemetre, double c_a, double s_a, double a, double *H, double *L)
86 *H = (l_telemetre - l_mirror - d)*sin(2*a);
87 *L = l_mirror + d + *H/tan(2*a);
89 //*H+= 8*sin(a-scan_params.offset_a);
96 //uint8_t last_sample;
98 //uint8_t h_limit[] = {40, 53, 66, 78, 94, 111, 123};
99 //uint8_t h_limit[] = {37, 48, 61, 72, 94, 111, 123};
101 /* last high is dummy, to deal higher columns */
102 //uint8_t h_limit[] = {68, 79, 93, 107, 121, 138, 155, 170, 250};
105 //uint8_t h_limit[] = {60, 72, 85, 98, 112, 129, 250};
107 //uint8_t h_limit[] = {80, 97, 118, 134, 145, 160, 250};
110 //uint8_t h_limit[] = {79, 94, 108, 117, 129, 144, 250};
112 uint8_t h_limit[] = {83, 95, 108, 120, 135, 147, 164, 250};
113 #define H_MARGIN (-6)
115 #define cs_set_consign(a, b)
119 void do_scan(void * dummy)
129 int32_t pos, last_pos;
135 double b, c_a, s_a, /*H, L,*/ m_a;
140 if (scan_params.sample_i==0)
143 mot_pos = encoders_spi_get_value_scanner_interpolation((void *)SCANNER_ENC);
145 if (scan_params.sample_i==1){
146 printf_P(PSTR("dump end enc %ld %d \r\n"), mot_pos, PIX_PER_SCAN);
147 //scanner.flags &= (~CS_ON);
151 /* stop scan at cur pos + 10 round */
152 mot_pos = encoders_spi_get_value_scanner_interpolation((void *)SCANNER_ENC);
153 mot_pos = SCANNER_STEP_TOUR * ((mot_pos/SCANNER_STEP_TOUR) + 1) ;
155 printf_P(PSTR("set to %ld \r\n"), mot_pos);
157 cs_set_consign(&sensorboard.scanner.cs, mot_pos);
158 //pwm_ng_set(SCANNER_MOT_PWM, 0);
163 mot_pos-= scan_params.pos_start_scan;
165 a = adc_get_value( ADC_REF_AVCC | MUX_ADC13 );
168 //dist = (a-TELEMETRE_B)/TELEMETRE_A;
169 //dist = TELEMETRE_A * a +TELEMETRE_B;
170 dist = telemetre_to_cm(a);
172 //printf_P(PSTR("enc val = %ld\r\n"), encoders_microb_get_value((void *)SCANNER_ENC));
175 //sample_tab[MAX_SAMPLE-sample_i] = a>0x1ff?0x1FF:a;
176 //sample_tab[MAX_SAMPLE-sample_i] |= PINF&2?0x200:0;
179 row_n = (mot_pos)/(SCANNER_STEP_TOUR/2);
181 /* separe scan forward/backword */
191 tour_pos = (mot_pos)%(SCANNER_STEP_TOUR);
193 b = (2.*M_PI)*(double)tour_pos/(double)(SCANNER_STEP_TOUR);
195 ang2_a_mirror(b, &c_a, &s_a, &m_a);
196 ang2_H_L(dist, c_a, s_a, m_a, &H, &L);
204 if (dist> SCAN_MAX_DIST){
209 H = H;//m_a*180/M_PI;
210 L = L;//(L-100)*PIX_PER_SCAN;
212 //printf_P(PSTR("polling : ADC0 = %i %f\r\n"),a, dist);
213 //printf_P(PSTR("%f %f %2.2f %f\r\n"), H, L, m_a*180./M_PI, dist);
216 //printf_P(PSTR("dist, b, a: %2.2f %2.2f %2.2f\r\n"), dist, b*180/M_PI, m_a*180/M_PI);
218 H=(H+SCAN_H_MAX)*SCAN_H_COEF;
222 /* XXX may never append */
227 /* first filter => pixel modulo level */
230 H_fin = H;//+SCAN_H_MAX;
231 //H_fin = ((H_fin-H_BASE)/H_MUL)*H_MUL + H_BASE;
233 if (scan_params.filter){
234 H_fin = 11; // default is level 11
235 for (i=0;i<sizeof(h_limit)/sizeof(uint8_t);i++){
236 if (H < h_limit[i]+H_MARGIN){
244 //printf_P(PSTR("%f %f\r\n"), dist, m_a*180/M_PI);
245 //printf_P(PSTR("%f %f\r\n"), m_a*180/M_PI, b*180/M_PI);
247 //printf_P(PSTR("%d %f\r\n"), a, b*180/M_PI);
248 //printf_P(PSTR("%f %f %f\r\n"), H, m_a*180/M_PI, offset_b);
250 //printf_P(PSTR("a, tpos: %d %d \r\n"), a, tour_pos);
253 //printf_P(PSTR("%f %f\r\n"), H, L);
258 //tour_pos = ((SCANNER_STEP_TOUR/2)-tour_pos);
259 tour_pos = (tour_pos*PIX_PER_SCAN)/(SCANNER_STEP_TOUR);
262 tour_pos = ((SCANNER_STEP_TOUR-tour_pos)*PIX_PER_SCAN)/(SCANNER_STEP_TOUR);
265 col_n = (PIX_PER_SCAN*L)/(SCAN_L_MAX-SCAN_L_MIN);
266 if (col_n>PIX_PER_SCAN)
267 printf("BUG!!! RECALC MAX L\r\n");
269 //col_n = (PIX_PER_SCAN+col_n -5)%PIX_PER_SCAN;
271 //pos = (row_n*SCANNER_STEP_TOUR + tour_pos)/STEP_PER_POS;
272 //pos= row_n*PIX_PER_SCAN+tour_pos;
273 //last_pos= last_row_n*PIX_PER_SCAN+last_tour_pos;
277 pos= row_n*PIX_PER_SCAN+col_n;
278 last_pos= scan_params.last_row_n*PIX_PER_SCAN+scan_params.last_col_n;
280 //printf_P(PSTR("%ld %ld %ld %ld\r\n"), row_n, col_n, pos, H_fin);
286 if (0<= pos && pos <MAX_SAMPLE)// && row_n%2)
287 //sample_tab[pos] = a>0xff?0xFF:a;
288 //sample_tab[(int)L] = H ;
289 scan_params.sample_tab[pos] = H_fin;
291 if ((scan_params.last_row_n == row_n) && ABS(last_pos-pos)>1){
292 /* we have a hole, pad it with minimal hight */
293 if (H_fin>scan_params.last_sample)
294 min_sample = scan_params.last_sample;
298 //printf("(%ld, %ld) (%ld %ld)\r\n", last_col_n, last_row_n, col_n, row_n);
300 /* fill grow, avoid erasing curent pos */
304 //printf("loop1 on (%ld, %ld) %ld\r\n", pos_tmp, last_pos, last_pos-pos_tmp);
309 //printf("loop1 on (%ld, %ld) %ld\r\n", pos_tmp, last_pos, last_pos-pos_tmp);
313 for (;pos_tmp< last_pos;pos_tmp++){
314 if (0< pos_tmp && pos_tmp <MAX_SAMPLE)// && row_n%2)
315 //sample_tab[pos_tmp] = min_sample;
323 scan_params.last_row_n = row_n;
324 scan_params.last_col_n = col_n;
325 scan_params.last_sample = H_fin;
328 //printf("pos : %ld\r\n", pos);
329 //sample_tab[sample_i] = a>0x1ff?0x1FF:a;
331 //sample_ok_tab[MAX_SAMPLE-sample_i] = PORTF&2;
334 if (((pos <MAX_SAMPLE)) && (tour_pos<=(SCANNER_STEP_TOUR/2)))
335 sample_tab[pos] = 0xffff;
337 scan_params.sample_i--;
341 #define my_offset_a (0*M_PI/180)
342 #define my_offset_b (34*M_PI/180)
344 //#define my_offset_a (40*M_PI/180)
345 //#define my_offset_b (-5*M_PI/180)
350 *2 cause we don't known exactly limits
353 lookup_h_l array_h_l[DIM_DIST*2][DIM_ANGLE*2];
357 #define pgm_read_word_near(a) (*((uint16_t*)(a)))
360 void do_scan_quick(void * dummy)
370 int32_t pos, last_pos;
375 //int32_t H_fin, L_fin;
382 if (scan_params.sample_i==0)
385 mot_pos = encoders_spi_get_value_scanner_interpolation((void *)SCANNER_ENC);
386 if (scan_params.sample_i==1){
387 printf_P(PSTR("dump end enc %ld %d \r\n"), mot_pos, PIX_PER_SCAN);
388 //scanner.flags &= (~CS_ON);
392 /* stop scan at cur pos + 10 round */
393 mot_pos = encoders_spi_get_value_scanner_interpolation((void *)SCANNER_ENC);
394 mot_pos = SCANNER_STEP_TOUR * ((mot_pos/SCANNER_STEP_TOUR) + 1) ;
396 printf_P(PSTR("set to %ld \r\n"), mot_pos);
398 cs_set_consign(&sensorboard.scanner.cs, mot_pos);
399 //pwm_ng_set(SCANNER_MOT_PWM, 0);
404 mot_pos-= scan_params.pos_start_scan;
406 a = adc_get_value( ADC_REF_AVCC | MUX_ADC13 );
409 tour_pos = (mot_pos)%(SCANNER_STEP_TOUR);
412 if (scan_params.offset_a != 0)
413 printf_P(PSTR("%ld %d \r\n"), tour_pos, a);
415 /* lookup in precomputed array */
418 j = (DIM_DIST * (telemetre_to_cm(a)-TELEMETRE_MIN_CM))/(TELEMETRE_MAX_CM - TELEMETRE_MIN_CM);
419 i = (DIM_ANGLE * tour_pos)/STEP_PER_ROUND;
431 val.u16 = pgm_read_word_near(&array_h_l[j][i]);
434 //val.u16 = pgm_read_word_near(&array_h_l[(a-TELEMETRE_MIN)/DIST_STEP][mot_pos/ANGLE_STEP]);
435 //val.u16 = pgm_read_word_near(&array_h_l[a][tp]);
439 val.u16 = pgm_read_word_near(&array_h_l[(a-TELEMETRE_MIN)/DIST_STEP][mot_pos/ANGLE_STEP]);
449 col_n = (PIX_PER_SCAN*L)/(SCAN_L_MAX-SCAN_L_MIN);
450 if (col_n>PIX_PER_SCAN)
451 printf("BUG!!! RECALC MAX L\r\n");
453 //col_n = (PIX_PER_SCAN+col_n -5)%PIX_PER_SCAN;
455 //pos = (row_n*SCANNER_STEP_TOUR + tour_pos)/STEP_PER_POS;
456 //pos= row_n*PIX_PER_SCAN+tour_pos;
457 //last_pos= last_row_n*PIX_PER_SCAN+last_tour_pos;
459 row_n = (mot_pos)/(SCANNER_STEP_TOUR/2);
462 pos= row_n*PIX_PER_SCAN+col_n;
463 last_pos= scan_params.last_row_n*PIX_PER_SCAN+scan_params.last_col_n;
465 //printf_P(PSTR("%ld %ld %ld %ld\r\n"), row_n, col_n, pos, H_fin);
471 if (0<= pos && pos <MAX_SAMPLE)// && row_n%2)
472 //sample_tab[pos] = a>0xff?0xFF:a;
473 //sample_tab[(int)L] = H ;
474 scan_params.sample_tab[pos] = H_fin;
476 if ((scan_params.last_row_n == row_n) && ABS(last_pos-pos)>1){
477 /* we have a hole, pad it with minimal hight */
478 if (H_fin>scan_params.last_sample)
479 min_sample = scan_params.last_sample;
483 //printf("(%ld, %ld) (%ld %ld)\r\n", last_col_n, last_row_n, col_n, row_n);
485 /* fill grow, avoid erasing curent pos */
489 //printf("loop1 on (%ld, %ld) %ld\r\n", pos_tmp, last_pos, last_pos-pos_tmp);
494 //printf("loop1 on (%ld, %ld) %ld\r\n", pos_tmp, last_pos, last_pos-pos_tmp);
498 for (;pos_tmp< last_pos;pos_tmp++){
499 if (0< pos_tmp && pos_tmp <MAX_SAMPLE)// && row_n%2)
500 //sample_tab[pos_tmp] = min_sample;
508 scan_params.last_row_n = row_n;
509 scan_params.last_col_n = col_n;
510 scan_params.last_sample = H_fin;
513 //printf("pos : %ld\r\n", pos);
514 //sample_tab[sample_i] = a>0x1ff?0x1FF:a;
516 //sample_ok_tab[MAX_SAMPLE-sample_i] = PORTF&2;
519 if (((pos <MAX_SAMPLE)) && (tour_pos<=(SCANNER_STEP_TOUR/2)))
520 sample_tab[pos] = 0xffff;
522 scan_params.sample_i--;
526 int main(int argc, char** argv)
534 FILE* f, *fin, *f_header;
536 unsigned char line[1000];
538 unsigned int dist_max, angle_max;
540 scan_params.filter = 1;
544 f = fopen("oo", "w");
549 scan_params.offset_a = my_offset_a;
550 scan_params.offset_b = my_offset_b;
556 /*precompute H & L array */
557 for (j = 0, scan_dist = TELEMETRE_MIN; scan_dist< TELEMETRE_MAX; j++, scan_dist+=DIST_STEP){
559 for (i = 0, motor_angle = 0; motor_angle < STEP_PER_ROUND; i++, motor_angle+=ANGLE_STEP){
560 scan_params.sample_i = 100;
561 scan_params.pos_start_scan = 0;
564 //printf("mangle, dist, h, l: %d %d, (%d, %2.2f)\n", motor_angle, scan_dist, H_fin, L);
565 //fprintf(f, "mangle, dist, h, l: %d %d, (%d, %2.2f)\n", motor_angle, scan_dist, H_fin, L);
567 j = (scan_dist - TELEMETRE_MIN)/DIST_STEP;
568 i = motor_angle/ANGLE_STEP;
569 //printf("%d %d (%d %d) (%d %d %d = %d)\n", i, j, motor_angle, ANGLE_STEP, scan_dist, TELEMETRE_MIN, DIST_STEP, (scan_dist - TELEMETRE_MIN)/DIST_STEP);
570 array_h_l[j][i].h = H_fin;
571 array_h_l[j][i].l = L_fin;
573 j = (DIM_DIST * (telemetre_to_cm(scan_dist)-TELEMETRE_MIN_CM))/(TELEMETRE_MAX_CM - TELEMETRE_MIN_CM);
574 i = (DIM_ANGLE * motor_angle)/STEP_PER_ROUND;
575 array_h_l[j][i].h = H_fin;
576 array_h_l[j][i].l = L_fin;
582 scan_params.filter = 0;
583 if (argc>1 && !strcmp(argv[1], "1"))
584 scan_params.filter = 1;
587 printf("max i max j %d %d \n", i, j);
591 f_header = fopen("scan_h_l.h", "w");
592 fprintf(f_header, "PROGMEM lookup_h_l array_h_l[%d][%d] = {\n", dist_max, angle_max);
593 for (j = 0, scan_dist = 0; j<dist_max; j++, scan_dist+=DIST_STEP){
594 fprintf(f_header, "{\n");
596 for (i = 0, motor_angle = 0; i < angle_max; i++, motor_angle+=ANGLE_STEP){
597 fprintf(f_header, "{ .h = %d, .l = %d }, ", array_h_l[j][i].h, array_h_l[j][i].l);
600 fprintf(f_header, "},\n");
602 fprintf(f_header, "};\n", DIM_DIST, DIM_ANGLE);
608 printf("IIIIIIIIIIII %d %d\n", array_h_l[38][152].h, array_h_l[38][152].l);
609 val.u16 = pgm_read_word_near(&array_h_l[38][152]);
610 printf("IIIIIIIIIIII %d %d\n", val.h_l.h, val.h_l.l);
615 for (motor_angle = 0; motor_angle<14000; motor_angle+=100){
616 for (scan_dist = TELEMETRE_MIN; scan_dist<300; scan_dist+=10){
617 scan_params.sample_i = 100;
618 scan_params.pos_start_scan = 0;
623 //printf("m s %d %d\n", motor_angle, scan_dist);
624 //printf("R %d %d\n", H_fin, L_fin);
626 val.u16 = pgm_read_word_near(&array_h_l[(scan_dist-TELEMETRE_MIN)/DIST_STEP][motor_angle/ANGLE_STEP]);
627 //printf("Q %d %d\n\n", val.h_l.h, val.h_l.l);
629 if (val.h_l.h != H_fin || val.h_l.l != L_fin)
634 fin = fopen("out", "r");
635 while(fgets(line, sizeof(line), fin)){
636 scan_params.sample_i = 100;
637 scan_params.pos_start_scan = 0;
639 //printf("[%s]\n", line);
640 sscanf(line, "%d %d\n", &a, &b);
641 //printf("%d %d\n", a, b);
646 j = (DIM_DIST * (telemetre_to_cm(scan_dist)-TELEMETRE_MIN_CM))/(TELEMETRE_MAX_CM - TELEMETRE_MIN_CM);
647 i = (DIM_ANGLE * motor_angle)/STEP_PER_ROUND;
649 val.u16 = pgm_read_word_near(&array_h_l[j][i]);
654 printf("Q %d %d\n", H_fin, L_fin);
658 printf("R %d %d\n", H_fin, L_fin);
660 fprintf(f, "mangle, dist, h, l: %d %d, (%d, %d)\n", motor_angle, scan_dist, H_fin, L_fin);
669 for (j = 0, scan_dist = 0; j<DIM_DIST; j++, scan_dist+=DIST_STEP){
670 for (i = 0, motor_angle = 0; i < DIM_ANGLE; i++, motor_angle+=ANGLE_STEP){
672 printf("%d %d, (%d, %2.2f)\n", motor_angle, scan_dist, H_fin, L);