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 }