hostsim test
[aversive.git] / projects / microb2009 / sensorboard / gen_scan_tab.c
1 #include <stdio.h>
2 #include <stdlib.h>
3 #include <math.h>
4 #include <inttypes.h>
5
6 #include <stdint.h>
7
8
9 #include "scanner.h"
10
11 #define ADC_REF_AVCC 0
12 #define  MUX_ADC13 0
13
14
15
16 double  TELEMETRE_A =  TELEMETRE_A_INIT;
17 double  TELEMETRE_B = TELEMETRE_B_INIT;
18
19
20 #define printf_P printf
21 #define PSTR(a) (a)
22 int32_t motor_angle = 0;
23 int32_t scan_dist = 0;
24
25
26 int32_t H_fin, L_fin;
27 double L, H;
28
29 #define nop()
30
31 #define ABS(val) ({                                     \
32                         __typeof(val) __val = (val);    \
33                         if (__val < 0)                  \
34                                 __val = - __val;        \
35                         __val;                          \
36                 })
37
38 struct scan_params scan_params;
39
40 int32_t encoders_spi_get_value_scanner_interpolation(void *a)
41 {
42         return motor_angle;
43 }
44
45
46 int16_t adc_get_value(uint8_t a)
47 {
48         return scan_dist;
49 }
50
51
52
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)
55 {
56         double x2, y2;
57         double A, DELTA, B, D;
58
59         b+=scan_params.offset_b;
60         x2 = X + l1*cos(b);
61         y2 = Y + l1*sin(b);
62
63         A = (l3*l3 + x2*x2 + y2*y2 - l2*l2)/(2*l3);
64
65         DELTA = -(A*A - x2*x2 - y2*y2);
66         B = sqrt(DELTA);
67
68         D = x2*x2 + y2*y2;
69
70         *c_a = (x2*A + y2*B)/D;
71         *s_a = -(x2*B - y2*A)/D;
72
73         *a = atan2(*s_a, *c_a);
74
75         *a += scan_params.offset_a;
76         // *s_a = sin(*a);
77         // *c_a = cos(*a);
78
79 }
80
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)
83 {
84         double d;
85         d = h_mirror*c_a/s_a;
86         *H = (l_telemetre - l_mirror - d)*sin(2*a);
87         *L = l_mirror + d + *H/tan(2*a);
88
89         //*H+= 8*sin(a-scan_params.offset_a);
90 }
91
92
93
94 //int32_t last_col_n;
95 //int32_t last_row_n;
96 //uint8_t last_sample;
97
98 //uint8_t h_limit[] = {40, 53, 66, 78, 94, 111, 123};
99 //uint8_t h_limit[] = {37, 48, 61, 72, 94, 111, 123};
100
101 /* last high is dummy, to deal higher columns */
102 //uint8_t h_limit[] = {68, 79, 93, 107, 121, 138, 155, 170, 250};
103
104
105 //uint8_t h_limit[] = {60, 72, 85, 98, 112, 129, 250};
106
107 //uint8_t h_limit[] = {80, 97, 118, 134, 145, 160, 250};
108
109
110 //uint8_t h_limit[] = {79, 94, 108, 117, 129, 144, 250};
111
112 uint8_t h_limit[] = {83, 95, 108, 120, 135, 147, 164, 250};
113 #define H_MARGIN (-6)
114
115 #define cs_set_consign(a, b) 
116
117
118
119 void do_scan(void * dummy) 
120 {
121
122         unsigned int i;
123         int16_t a;
124         int32_t row_n;
125         int32_t col_n;
126
127
128         int32_t tour_pos;
129         int32_t pos, last_pos;
130         int32_t pos_tmp ;
131         int32_t mot_pos;
132         double dist;
133         uint8_t min_sample;
134
135         double b, c_a, s_a, /*H, L,*/ m_a;
136         //      int32_t H_fin;
137
138
139         //printf("scan\n");
140         if (scan_params.sample_i==0)
141                 return;
142
143         mot_pos = encoders_spi_get_value_scanner_interpolation((void *)SCANNER_ENC);
144 #if 0
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);
148                 
149
150
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) ;
154
155                 printf_P(PSTR("set to %ld \r\n"), mot_pos);
156
157                 cs_set_consign(&sensorboard.scanner.cs, mot_pos);
158                 //pwm_ng_set(SCANNER_MOT_PWM, 0);
159
160                 
161         }
162
163         mot_pos-= scan_params.pos_start_scan;
164 #endif
165         a = adc_get_value( ADC_REF_AVCC | MUX_ADC13 );
166         
167
168         //dist = (a-TELEMETRE_B)/TELEMETRE_A;
169         //dist = TELEMETRE_A * a +TELEMETRE_B;
170         dist = telemetre_to_cm(a);
171
172         //printf_P(PSTR("enc val = %ld\r\n"),   encoders_microb_get_value((void *)SCANNER_ENC));
173
174
175         //sample_tab[MAX_SAMPLE-sample_i] = a>0x1ff?0x1FF:a;
176         //sample_tab[MAX_SAMPLE-sample_i] |= PINF&2?0x200:0;
177
178
179         row_n = (mot_pos)/(SCANNER_STEP_TOUR/2);
180 #if 0
181         /* separe scan forward/backword */
182         if (row_n%2){
183                 row_n/=2;
184         }
185         else{
186                 row_n/=2;
187                 row_n+=30;
188         }
189 #endif 
190
191         tour_pos = (mot_pos)%(SCANNER_STEP_TOUR);
192
193         b = (2.*M_PI)*(double)tour_pos/(double)(SCANNER_STEP_TOUR);
194
195         ang2_a_mirror(b, &c_a, &s_a, &m_a);
196         ang2_H_L(dist, c_a, s_a, m_a, &H, &L);
197
198
199         if (H >0){
200                 printf("zarb H\n");
201                 H = 0;
202         }
203
204         if (dist> SCAN_MAX_DIST){
205                 H = 0;
206                 L = 0;
207         }
208
209         H = H;//m_a*180/M_PI;
210         L = L;//(L-100)*PIX_PER_SCAN;
211
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);
214
215
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);
217
218         H=(H+SCAN_H_MAX)*SCAN_H_COEF;
219         L-=SCAN_L_MIN;
220
221
222         /* XXX may never append */
223         if (L<0)
224                 L=0;
225
226
227         /* first filter => pixel modulo level */
228 #define H_BASE  10
229 #define H_MUL  14
230         H_fin = H;//+SCAN_H_MAX;
231         //H_fin = ((H_fin-H_BASE)/H_MUL)*H_MUL + H_BASE;
232
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){
237                                 H_fin = i;
238                                 break;
239                         }
240                 }
241         }
242
243         L_fin = L;
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);
246
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);
249
250         //printf_P(PSTR("a, tpos: %d %d \r\n"), a, tour_pos);
251
252
253         //printf_P(PSTR("%f %f\r\n"), H, L);
254
255
256         /*
257         if (row_n%2){
258                 //tour_pos = ((SCANNER_STEP_TOUR/2)-tour_pos);
259                 tour_pos = (tour_pos*PIX_PER_SCAN)/(SCANNER_STEP_TOUR);
260         }
261         else{
262                 tour_pos = ((SCANNER_STEP_TOUR-tour_pos)*PIX_PER_SCAN)/(SCANNER_STEP_TOUR);
263         }
264         */
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");
268
269         //col_n = (PIX_PER_SCAN+col_n -5)%PIX_PER_SCAN;
270
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;
274
275
276         
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;
279         
280         //printf_P(PSTR("%ld %ld %ld %ld\r\n"), row_n, col_n, pos, H_fin);
281
282         //a-=0x100;
283         a-=200;
284         //a/=10;
285
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;
290         nop();
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;
295                 else
296                         min_sample = H_fin;
297
298                 //printf("(%ld, %ld) (%ld %ld)\r\n", last_col_n, last_row_n, col_n, row_n);
299
300                 /* fill grow, avoid erasing curent pos */
301                 if (pos > last_pos){
302                         pos_tmp = last_pos;
303                         last_pos = pos;
304                         //printf("loop1 on (%ld, %ld) %ld\r\n", pos_tmp, last_pos, last_pos-pos_tmp);
305                 
306                 }
307                 else{
308                         pos_tmp = pos+1;
309                         //printf("loop1 on (%ld, %ld) %ld\r\n", pos_tmp, last_pos, last_pos-pos_tmp);
310                 }
311
312                 
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;
316                         nop();
317                         
318                 }
319                 
320
321         }
322
323         scan_params.last_row_n = row_n;
324         scan_params.last_col_n = col_n;
325         scan_params.last_sample = H_fin;
326
327         
328         //printf("pos : %ld\r\n", pos);
329         //sample_tab[sample_i] =  a>0x1ff?0x1FF:a;
330
331         //sample_ok_tab[MAX_SAMPLE-sample_i] = PORTF&2;
332
333         /*
334         if (((pos <MAX_SAMPLE)) && (tour_pos<=(SCANNER_STEP_TOUR/2)))
335                 sample_tab[pos] = 0xffff;
336         */
337         scan_params.sample_i--;
338 }
339
340
341 #define my_offset_a (0*M_PI/180)
342 #define my_offset_b (34*M_PI/180)
343
344 //#define my_offset_a (40*M_PI/180)
345 //#define my_offset_b (-5*M_PI/180)
346
347
348
349 /* 
350  *2 cause we don't known exactly limits 
351  at computation time 
352 */
353 lookup_h_l array_h_l[DIM_DIST*2][DIM_ANGLE*2];
354
355
356
357 #define pgm_read_word_near(a) (*((uint16_t*)(a)))
358
359
360 void do_scan_quick(void * dummy) 
361 {
362
363         int i, j;
364         int16_t a;
365         int32_t row_n;
366         int32_t col_n;
367
368
369         int32_t tour_pos;
370         int32_t pos, last_pos;
371         int32_t pos_tmp ;
372         int32_t mot_pos;
373         uint8_t min_sample;
374         //double H, L;
375         //int32_t H_fin, L_fin;
376
377
378         union{
379                 uint16_t u16;
380                 lookup_h_l h_l;
381         }val;
382         if (scan_params.sample_i==0)
383                 return;
384
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);
389                 
390
391
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) ;
395
396                 printf_P(PSTR("set to %ld \r\n"), mot_pos);
397
398                 cs_set_consign(&sensorboard.scanner.cs, mot_pos);
399                 //pwm_ng_set(SCANNER_MOT_PWM, 0);
400
401                 
402         }
403
404         mot_pos-= scan_params.pos_start_scan;
405
406         a = adc_get_value( ADC_REF_AVCC | MUX_ADC13 );
407
408
409         tour_pos = (mot_pos)%(SCANNER_STEP_TOUR);
410
411
412         if (scan_params.offset_a != 0)
413                 printf_P(PSTR("%ld %d \r\n"), tour_pos, a);
414
415         /* lookup in precomputed array */
416
417
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;
420         
421
422         if (j<0)
423                 j=0;
424         if (j>=DIM_DIST)
425                 j = DIM_DIST-1;
426         
427         if (i>=DIM_ANGLE)
428                 i = DIM_ANGLE-1;
429
430         
431         val.u16 = pgm_read_word_near(&array_h_l[j][i]);
432
433
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]);
436         H = val.h_l.h;
437         L = val.h_l.l;
438         /*
439         val.u16 = pgm_read_word_near(&array_h_l[(a-TELEMETRE_MIN)/DIST_STEP][mot_pos/ANGLE_STEP]);
440
441         H = val.h_l.h;
442         L = val.h_l.l;
443         */
444         H_fin = H;
445         L_fin = L;
446
447
448
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");
452
453         //col_n = (PIX_PER_SCAN+col_n -5)%PIX_PER_SCAN;
454
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;
458
459         row_n = (mot_pos)/(SCANNER_STEP_TOUR/2);
460
461         
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;
464         
465         //printf_P(PSTR("%ld %ld %ld %ld\r\n"), row_n, col_n, pos, H_fin);
466
467         //a-=0x100;
468         a-=200;
469         //a/=10;
470
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;
475         nop();
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;
480                 else
481                         min_sample = H_fin;
482
483                 //printf("(%ld, %ld) (%ld %ld)\r\n", last_col_n, last_row_n, col_n, row_n);
484
485                 /* fill grow, avoid erasing curent pos */
486                 if (pos > last_pos){
487                         pos_tmp = last_pos;
488                         last_pos = pos;
489                         //printf("loop1 on (%ld, %ld) %ld\r\n", pos_tmp, last_pos, last_pos-pos_tmp);
490                 
491                 }
492                 else{
493                         pos_tmp = pos+1;
494                         //printf("loop1 on (%ld, %ld) %ld\r\n", pos_tmp, last_pos, last_pos-pos_tmp);
495                 }
496
497                 
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;
501                         nop();
502                         
503                 }
504                 
505
506         }
507
508         scan_params.last_row_n = row_n;
509         scan_params.last_col_n = col_n;
510         scan_params.last_sample = H_fin;
511
512         
513         //printf("pos : %ld\r\n", pos);
514         //sample_tab[sample_i] =  a>0x1ff?0x1FF:a;
515
516         //sample_ok_tab[MAX_SAMPLE-sample_i] = PORTF&2;
517
518         /*
519         if (((pos <MAX_SAMPLE)) && (tour_pos<=(SCANNER_STEP_TOUR/2)))
520                 sample_tab[pos] = 0xffff;
521         */
522         scan_params.sample_i--;
523 }
524
525
526 int main(int argc, char** argv)
527 {
528         union{
529                 uint16_t u16;
530                 lookup_h_l h_l;
531         }val;
532
533         int i, j;
534         FILE* f, *fin, *f_header;
535
536         unsigned char line[1000];
537         unsigned int a, b;
538         unsigned int dist_max, angle_max;
539
540         scan_params.filter = 1; 
541         
542
543         
544         f = fopen("oo", "w");
545
546
547         TELEMETRE_B = 23.50;
548
549         scan_params.offset_a = my_offset_a;
550         scan_params.offset_b = my_offset_b;
551
552
553
554         
555
556         /*precompute H & L array */
557         for (j = 0, scan_dist = TELEMETRE_MIN; scan_dist< TELEMETRE_MAX; j++, scan_dist+=DIST_STEP){
558                 
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;
562                         //printf("%d\n", i);
563                         do_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);
566                         /*
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;
572                         */
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;
577
578                 }
579         }       
580
581
582         scan_params.filter = 0;
583         if (argc>1 && !strcmp(argv[1], "1"))
584                 scan_params.filter = 1; 
585
586
587         printf("max i max j %d %d \n", i, j);
588         dist_max = j;
589         angle_max = i;
590
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");
595  
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);
598                         
599                 }
600                 fprintf(f_header, "},\n");
601         }       
602         fprintf(f_header, "};\n", DIM_DIST, DIM_ANGLE);
603         
604         fclose(f_header);
605         
606
607
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); 
611
612         motor_angle = 9000;
613         scan_dist = 388;
614         
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;
619                 
620                 do_scan(0); 
621
622
623                 //printf("m s %d %d\n", motor_angle, scan_dist);
624                 //printf("R %d %d\n", H_fin, L_fin);
625                 
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); 
628
629                 if (val.h_l.h != H_fin || val.h_l.l != L_fin)
630                         printf("BUG BUG\n");
631         }
632         }
633
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;
638                 
639                 //printf("[%s]\n", line);
640                 sscanf(line, "%d %d\n", &a, &b);
641                 //printf("%d %d\n", a, b);
642                 motor_angle = a;
643                 scan_dist = b;
644                 do_scan_quick(0); 
645                 /*
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;
648                 
649                 val.u16 = pgm_read_word_near(&array_h_l[j][i]);
650                 H_fin = val.h_l.h;
651                 L_fin = val.h_l.l;
652                 */
653                 
654                 printf("Q %d %d\n", H_fin, L_fin);
655
656                 do_scan(0); 
657
658                 printf("R %d %d\n", H_fin, L_fin);
659
660                 fprintf(f, "mangle, dist, h, l: %d %d, (%d, %d)\n", motor_angle, scan_dist, H_fin, L_fin);
661
662         }
663         fclose(fin);
664
665         fclose(f);
666
667         return;
668         /*
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){
671                         do_scan(0); 
672                         printf("%d %d, (%d, %2.2f)\n", motor_angle, scan_dist, H_fin, L);
673         
674                 }
675
676         }
677         */
678
679
680 }