7 #include <aversive/wait.h>
8 #include <aversive/error.h>
9 #include <aversive/pgmspace.h>
14 #include <control_system_manager.h>
20 #include <blocking_detection_manager.h>
22 #include <encoders_spi.h>
39 #include <vect_base.h>
40 #include "img_processing.h"
43 #include "../common/i2c_commands.h"
45 #define SCANNER_DEBUG(args...) DEBUG(E_USER_SCANNER, args)
46 #define SCANNER_NOTICE(args...) NOTICE(E_USER_SCANNER, args)
47 #define SCANNER_ERROR(args...) ERROR(E_USER_SCANNER, args)
50 #define MODULO_TIMER (1023L)
52 #define COEFF_TIMER (2)
53 #define COEFF_MULT (1000L)
54 #define COEFF_MULT2 (1000L)
56 double TELEMETRE_A = TELEMETRE_A_INIT;
57 double TELEMETRE_B = TELEMETRE_B_INIT;
62 struct scan_params scan_params;
64 static volatile int32_t scan_delta_pos;
65 static volatile int32_t scan_tick_cur = 0;
66 static volatile int32_t scan_tick_prev = 0;
67 /*static volatile int32_t count = 0;
68 static volatile int32_t count_diff_rising = 0;
69 static volatile int32_t count_diff_falling = 0;
71 static int32_t scanner_coeff = 0;
73 //static volatile int8_t valid_scanner = 0;
75 static volatile int32_t scan_pos_cur = 0;
77 static int32_t pos_ref = 0;
80 int32_t encoders_spi_get_value_scanner(void *number)
84 ret = encoders_spi_get_value(number);
89 void encoders_spi_set_value_scanner(void * number, int32_t v)
91 encoders_spi_set_value(number, v/4);
94 int32_t encoders_spi_update_scanner(void * number)
100 ret = encoders_spi_get_value_scanner(number);
101 scan_delta_pos = ret - scan_pos_cur;
103 scan_tick_prev = scan_tick_cur;
104 scan_tick_cur = TCNT3;
106 scanner_coeff = (scan_delta_pos * COEFF_MULT) /
107 ((scan_tick_cur - scan_tick_prev + MODULO_TIMER + 1) & MODULO_TIMER);
115 int32_t encoders_spi_get_value_scanner_interpolation(void * number)
122 pos += (scanner_coeff * ((TCNT3 - scan_tick_cur + MODULO_TIMER + 1)& MODULO_TIMER ))/
131 void scanner_reset_pos(void)
133 pwm_ng_set(SCANNER_PWM, 0);
134 encoders_spi_set_value_scanner(SCANNER_ENCODER, 0);
137 void scanner_init(void)
140 scan_params.working = 0;
141 scan_params.must_stop = 0;
144 pos_ref = encoders_spi_get_value_scanner(SCANNER_ENCODER);
146 //memset(&scanner, 0, sizeof(struct scanner));
150 /*for(i=0;i<SCANNER_MAX_SAMPLE;i++)
151 scanner_sample_size[i] = 0;*/
158 #define SCANNER_OFFSET_CALIBRE 1500
160 #define CALIBRE_LASER_SAMPLE 100
167 void scanner_calibre_laser(void)
170 int32_t laser_value = 0;
173 pwm_ng_set(&gen.servo3, SCANNER_POS_OUT);
175 /* set mirror to have vertical laser */
176 cs_set_consign(&sensorboard.scanner.cs, (35L*SCANNER_STEP_TOUR)/100);
181 laser must point on the ground:
182 we sample laser in order to determine
183 laser cold/warm calibration
186 for (i = 0; i<CALIBRE_LASER_SAMPLE; i++){
187 laser_value += adc_get_value( ADC_REF_AVCC | MUX_ADC13 );
191 laser_value/=CALIBRE_LASER_SAMPLE;
193 SCANNER_NOTICE("laser calibration value %"PRIi32"", laser_value);
195 quadramp_set_1st_order_vars(&sensorboard.scanner.qr, scan_params.speed, scan_params.speed); /* set speed */
196 cs_set_consign(&sensorboard.scanner.cs, 0);
200 TELEMETRE_B = TELEMETRE_B_INIT + ((double)(424 - laser_value))*6./90.;
202 SCANNER_NOTICE("TEL B value %2.2f", TELEMETRE_B);
207 pwm_ng_set(&gen.servo3, SCANNER_POS_IN);
216 void scanner_calibre_mirror(void)
219 sensorboard.flags &= ~DO_CS;
223 /* set arm in calibre position */
224 pwm_ng_set(&gen.servo3, SCANNER_POS_CALIBRE);
229 /* init scanner pos */
230 pwm_ng_set(SCANNER_PWM, 100);
232 /* find rising edge of the mirror*/
234 while (sensor_get(SCANNER_POS_SENSOR));
236 while (!sensor_get(SCANNER_POS_SENSOR));
238 pwm_ng_set(SCANNER_PWM, 0);
242 pid_reset(&sensorboard.scanner.pid);
243 cs_set_consign(&sensorboard.scanner.cs, 0);
246 quadramp_reset(&sensorboard.scanner.qr);
249 sensorboard.flags |= DO_CS;
253 set mirror to set laser point at maximum
256 encoders_spi_set_value_scanner(SCANNER_ENCODER, -SCANNER_OFFSET_CALIBRE);
261 pwm_ng_set(&gen.servo3, SCANNER_POS_IN);
266 /* arm must be in OUT position! */
267 void scanner_do_scan(void){
268 scan_params.working = 1;
269 scan_params.dropzone_h = -1;
272 quadramp_set_1st_order_vars(&sensorboard.scanner.qr, scan_params.speed, scan_params.speed); /* set speed */
274 scan_params.sample_i = MAX_SAMPLE;
275 scan_params.pos_start_scan = encoders_spi_get_value_scanner(SCANNER_ENC);
277 memset(scan_params.sample_tab, 0xff, MAX_SAMPLE*sizeof(uint8_t));
279 cs_set_consign(&sensorboard.scanner.cs, scan_params.pos_start_scan+SCANNER_STEP_TOUR*200L);
281 scan_params.last_col_n = 0;
282 scan_params.last_row_n = 0;
283 scan_params.last_sample = 0;
289 void scanner_end_process(void)
291 int16_t dropzone_x, dropzone_y;
298 SCANNER_NOTICE("process img algo %d", scan_params.algo);
299 if (scan_params.algo == I2C_SCANNER_ALGO_COLUMN_DROPZONE) {
300 SCANNER_NOTICE("column dropzone h: %d x:%d y:%d", scan_params.drop_zone.working_zone,
301 scan_params.drop_zone.center_x, scan_params.drop_zone.center_y);
303 ret = get_column_dropzone(scan_params.sample_tab, PIX_PER_SCAN, MAX_SAMPLE/PIX_PER_SCAN,
304 scan_params.drop_zone.working_zone, scan_params.drop_zone.center_x, scan_params.drop_zone.center_y,
305 &dropzone_x, &dropzone_y);
307 scan_params.dropzone_h = ret;
308 scan_params.dropzone_x = dropzone_x;
309 scan_params.dropzone_y = dropzone_y;
311 SCANNER_NOTICE("best column h:%d x:%d y:%d",
312 scan_params.dropzone_h,
313 scan_params.dropzone_x, scan_params.dropzone_y);
317 else if (scan_params.algo == I2C_SCANNER_ALGO_CHECK_TEMPLE){
318 SCANNER_NOTICE("checktemple h: %d x:%d y:%d", scan_params.check_temple.level,
319 scan_params.check_temple.temple_x, scan_params.check_temple.temple_y);
320 ret = is_temple_there(scan_params.sample_tab, PIX_PER_SCAN, MAX_SAMPLE/PIX_PER_SCAN,
321 scan_params.check_temple.level, scan_params.check_temple.temple_x, scan_params.check_temple.temple_y);
323 scan_params.dropzone_h = ret?scan_params.check_temple.level:-1;
325 else if (scan_params.algo == I2C_SCANNER_ALGO_TEMPLE_DROPZONE){
326 SCANNER_NOTICE("temple dropzone h: %d x:%d y:%d", scan_params.drop_zone.working_zone,
327 scan_params.drop_zone.center_x, scan_params.drop_zone.center_y);
328 ret = find_temple_dropzone(scan_params.sample_tab, PIX_PER_SCAN, MAX_SAMPLE/PIX_PER_SCAN,
329 scan_params.drop_zone.working_zone, scan_params.drop_zone.center_x, scan_params.drop_zone.center_y,
330 &dropzone_x, &dropzone_y);
332 scan_params.dropzone_h = ret;
333 scan_params.dropzone_x = dropzone_x;
334 scan_params.dropzone_y = dropzone_y;
336 SCANNER_NOTICE("best temple h:%d x:%d y:%d",
337 scan_params.dropzone_h,
338 scan_params.dropzone_x, scan_params.dropzone_y);
342 scan_params.working = 0;
345 SCANNER_NOTICE("process total time %"PRIi32"",t2-t1);
351 void scanner_scan_autonomous(void)
354 pwm_ng_set(&gen.servo3, SCANNER_POS_OUT);
359 while(scan_params.sample_i > 0){
364 pwm_ng_set(&gen.servo3, SCANNER_POS_IN);
371 * mode can be off/prepare/start, see in i2c_commands.h
373 void scanner_set_mode(uint8_t mode)
375 if (mode == I2C_SENSORBOARD_SCANNER_PREPARE){
376 /* reset flag max pos */
377 scan_params.max_column_detected = 0;
380 pwm_ng_set(&gen.servo3, SCANNER_POS_OUT);
383 else if (mode == I2C_SENSORBOARD_SCANNER_STOP){
385 pwm_ng_set(&gen.servo3, SCANNER_POS_IN);
386 scan_params.must_stop = 1;
388 else if (mode == I2C_SENSORBOARD_SCANNER_START){
389 scan_params.max_column_detected = 0;
390 scan_params.must_stop = 0;
393 /* start scan in background */
401 void scanner_stop(void)
403 sensorboard.scanner.on = 0;
404 pwm_ng_set(SCANNER_PWM, 0);
410 int32_t encoders_spi_get_scanner_speed(void * dummy)
412 return scanner_speed;
417 //uint8_t sample_tab[MAX_SAMPLE];
418 //uint16_t sample_i = 0;
421 //#define offset_a (75.*M_PI/180.)
425 //int32_t pos_start_scan;
428 /* get motor angle in radian; return mirror angle in radian, cos a sin a */
429 void ang2_a_mirror(float b, float * c_a, float* s_a, float* a)
432 float A, DELTA, B, D;
434 b+=scan_params.offset_b;
438 A = (l3*l3 + x2*x2 + y2*y2 - l2*l2)/(2*l3);
440 DELTA = -(A*A - x2*x2 - y2*y2);
445 *c_a = (x2*A + y2*B)/D;
446 *s_a = -(x2*B - y2*A)/D;
448 *a = atan2(*s_a, *c_a);
450 *a += scan_params.offset_a;
456 /* get telemeter dist , cos a, sin a, a and return H, L of scanned point */
457 void ang2_H_L(float l_telemetre, float c_a, float s_a, float a, float *H, float *L)
460 d = h_mirror*c_a/s_a;
461 *H = (l_telemetre - l_mirror - d)*sin(2*a);
462 *L = l_mirror + d + *H/tan(2*a);
464 //*H+= 8*sin(a-scan_params.offset_a);
469 //int32_t last_col_n;
470 //int32_t last_row_n;
471 //uint8_t last_sample;
473 //uint8_t h_limit[] = {40, 53, 66, 78, 94, 111, 123};
474 //uint8_t h_limit[] = {37, 48, 61, 72, 94, 111, 123};
476 /* last high is dummy, to deal higher columns */
477 uint8_t h_limit[] = {68, 79, 93, 107, 121, 138, 155, 170, 250};
482 void do_scan(void * dummy)
492 int32_t pos, last_pos;
498 float b, c_a, s_a, H, L, m_a;
502 if (scan_params.sample_i==0)
505 mot_pos = encoders_spi_get_value_scanner_interpolation((void *)SCANNER_ENC);
507 if (scan_params.sample_i==1){
508 SCANNER_DEBUG("dump end enc %ld %d ", mot_pos, PIX_PER_SCAN);
509 //scanner.flags &= (~CS_ON);
513 /* stop scan at cur pos + 10 round */
514 mot_pos = encoders_spi_get_value_scanner_interpolation((void *)SCANNER_ENC);
515 mot_pos = SCANNER_STEP_TOUR * ((mot_pos/SCANNER_STEP_TOUR) + 1) ;
517 SCANNER_DEBUG("set to %ld ", mot_pos);
519 cs_set_consign(&sensorboard.scanner.cs, mot_pos);
520 //pwm_ng_set(SCANNER_MOT_PWM, 0);
525 mot_pos-= scan_params.pos_start_scan;
527 a = adc_get_value( ADC_REF_AVCC | MUX_ADC13 );
530 //dist = (a-TELEMETRE_B)/TELEMETRE_A;
531 dist = TELEMETRE_A * a +TELEMETRE_B;
533 //SCANNER_DEBUG("enc val = %ld", encoders_microb_get_value((void *)SCANNER_ENC));
536 //sample_tab[MAX_SAMPLE-sample_i] = a>0x1ff?0x1FF:a;
537 //sample_tab[MAX_SAMPLE-sample_i] |= PINF&2?0x200:0;
540 row_n = (mot_pos)/(SCANNER_STEP_TOUR/2);
542 /* separe scan forward/backword */
552 tour_pos = (mot_pos)%(SCANNER_STEP_TOUR);
554 b = (2.*M_PI)*(float)tour_pos/(float)(SCANNER_STEP_TOUR);
556 ang2_a_mirror(b, &c_a, &s_a, &m_a);
557 ang2_H_L(dist, c_a, s_a, m_a, &H, &L);
560 SCANNER_DEBUG("%ld %d", tour_pos, a);
567 if (dist> SCAN_MAX_DIST){
572 H = H;//m_a*180/M_PI;
573 L = L;//(L-100)*PIX_PER_SCAN;
575 //SCANNER_DEBUG("polling : ADC0 = %i %f",a, dist);
576 //SCANNER_DEBUG("%f %f %2.2f %f", H, L, m_a*180./M_PI, dist);
579 //SCANNER_DEBUG("%f %f", dist, m_a*180/M_PI);
581 H=(H+SCAN_H_MAX)*SCAN_H_COEF;
585 /* XXX may never append */
590 /* first filter => pixel modulo level */
593 H_fin = H;//+SCAN_H_MAX;
594 //H_fin = ((H_fin-H_BASE)/H_MUL)*H_MUL + H_BASE;
596 if (scan_params.filter){
597 H_fin = 11; // default is level 11
598 for (i=0;i<sizeof(h_limit)/sizeof(uint8_t);i++){
599 if (H < h_limit[i]-H_MARGIN){
606 //SCANNER_DEBUG("%f %f", dist, m_a*180/M_PI);
607 //SCANNER_DEBUG("%f %f", m_a*180/M_PI, b*180/M_PI);
609 //SCANNER_DEBUG("%d %f", a, b*180/M_PI);
610 //SCANNER_DEBUG("%f %f %f", H, m_a*180/M_PI, offset_b);
612 //SCANNER_DEBUG("%d %2.2f ", a, tour_pos);
615 //SCANNER_DEBUG("%f %f %ld %d", H, L, tour_pos, a);
620 //tour_pos = ((SCANNER_STEP_TOUR/2)-tour_pos);
621 tour_pos = (tour_pos*PIX_PER_SCAN)/(SCANNER_STEP_TOUR);
624 tour_pos = ((SCANNER_STEP_TOUR-tour_pos)*PIX_PER_SCAN)/(SCANNER_STEP_TOUR);
627 col_n = (PIX_PER_SCAN*L)/(SCAN_L_MAX-SCAN_L_MIN);
628 if (col_n>PIX_PER_SCAN)
629 printf("BUG!!! RECALC MAX L");
631 //col_n = (PIX_PER_SCAN+col_n -5)%PIX_PER_SCAN;
633 //pos = (row_n*SCANNER_STEP_TOUR + tour_pos)/STEP_PER_POS;
634 //pos= row_n*PIX_PER_SCAN+tour_pos;
635 //last_pos= last_row_n*PIX_PER_SCAN+last_tour_pos;
639 pos= row_n*PIX_PER_SCAN+col_n;
640 last_pos= scan_params.last_row_n*PIX_PER_SCAN+scan_params.last_col_n;
642 //SCANNER_DEBUG("%ld %ld %ld %ld", row_n, col_n, pos, H_fin);
648 if (0<= pos && pos <MAX_SAMPLE)// && row_n%2)
649 //sample_tab[pos] = a>0xff?0xFF:a;
650 //sample_tab[(int)L] = H ;
651 scan_params.sample_tab[pos] = H_fin;
653 if ((scan_params.last_row_n == row_n) && ABS(last_pos-pos)>1){
654 /* we have a hole, pad it with minimal hight */
655 if (H_fin>scan_params.last_sample)
656 min_sample = scan_params.last_sample;
660 //printf("(%ld, %ld) (%ld %ld)", last_col_n, last_row_n, col_n, row_n);
662 /* fill grow, avoid erasing curent pos */
666 //printf("loop1 on (%ld, %ld) %ld", pos_tmp, last_pos, last_pos-pos_tmp);
671 //printf("loop1 on (%ld, %ld) %ld", pos_tmp, last_pos, last_pos-pos_tmp);
675 for (;pos_tmp< last_pos;pos_tmp++){
676 if (0< pos_tmp && pos_tmp <MAX_SAMPLE)// && row_n%2)
677 //sample_tab[pos_tmp] = min_sample;
685 scan_params.last_row_n = row_n;
686 scan_params.last_col_n = col_n;
687 scan_params.last_sample = H_fin;
690 //printf("pos : %ld", pos);
691 //sample_tab[sample_i] = a>0x1ff?0x1FF:a;
693 //sample_ok_tab[MAX_SAMPLE-sample_i] = PORTF&2;
696 if (((pos <MAX_SAMPLE)) && (tour_pos<=(SCANNER_STEP_TOUR/2)))
697 sample_tab[pos] = 0xffff;
699 scan_params.sample_i--;
705 void do_scan(void * dummy)
715 int32_t pos, last_pos;
720 int32_t H_fin, L_fin;
731 if (scan_params.sample_i==0)
736 scan_params.max_column_detected = !!sensor_get(SCANNER_MAXCOLUMN_SENSOR);
738 mot_pos = encoders_spi_get_value_scanner_interpolation((void *)SCANNER_ENC);
739 if (scan_params.sample_i==1 || scan_params.max_column_detected || scan_params.must_stop ){
741 /* reset sample num in case of max column detected */
743 scan_params.sample_i = 1;
747 SCANNER_DEBUG("dump end enc %"PRIi32" %d ", mot_pos, PIX_PER_SCAN);
748 //scanner.flags &= (~CS_ON);
750 /* stop scan at cur pos + 10 round */
751 mot_pos = encoders_spi_get_value_scanner_interpolation((void *)SCANNER_ENC);
752 mot_pos = SCANNER_STEP_TOUR * ((mot_pos/SCANNER_STEP_TOUR) + 1) ;
754 SCANNER_DEBUG("set to %"PRIi32" ", mot_pos);
756 cs_set_consign(&sensorboard.scanner.cs, mot_pos);
757 //pwm_ng_set(SCANNER_MOT_PWM, 0);
762 //scanner_end_process();
763 if (!scan_params.must_stop && !scan_params.max_column_detected)
764 process_img_to_zone(scan_params.sample_tab, PIX_PER_SCAN, MAX_SAMPLE/PIX_PER_SCAN);
767 scan_params.working = 0;
771 mot_pos-= scan_params.pos_start_scan;
773 a = adc_get_value( ADC_REF_AVCC | MUX_ADC13 );
776 tour_pos = (mot_pos)%(SCANNER_STEP_TOUR);
779 if (scan_params.debug != 0)
780 SCANNER_DEBUG("%ld %d ", tour_pos, a);
782 /* lookup in precomputed array */
785 j = (DIM_DIST * (telemetre_to_cm(a)-TELEMETRE_MIN_CM))/(TELEMETRE_MAX_CM - TELEMETRE_MIN_CM);
786 i = (DIM_ANGLE * tour_pos)/STEP_PER_ROUND;
798 val.u16 = pgm_read_word_near(&array_h_l[j][i]);
801 //val.u16 = pgm_read_word_near(&array_h_l[(a-TELEMETRE_MIN)/DIST_STEP][mot_pos/ANGLE_STEP]);
802 //val.u16 = pgm_read_word_near(&array_h_l[a][tp]);
806 val.u16 = pgm_read_word_near(&array_h_l[(a-TELEMETRE_MIN)/DIST_STEP][mot_pos/ANGLE_STEP]);
818 col_n = (PIX_PER_SCAN*L)/(SCAN_L_MAX-SCAN_L_MIN);
819 if (col_n>=PIX_PER_SCAN) {
820 //printf("BUG!!! RECALC MAX L");
821 col_n = PIX_PER_SCAN-1;
824 //col_n = (PIX_PER_SCAN+col_n -5)%PIX_PER_SCAN;
826 //pos = (row_n*SCANNER_STEP_TOUR + tour_pos)/STEP_PER_POS;
827 //pos= row_n*PIX_PER_SCAN+tour_pos;
828 //last_pos= last_row_n*PIX_PER_SCAN+last_tour_pos;
830 row_n = (mot_pos)/(SCANNER_STEP_TOUR/2);
833 pos= row_n*PIX_PER_SCAN+col_n;
834 last_pos= scan_params.last_row_n*PIX_PER_SCAN+scan_params.last_col_n;
836 //SCANNER_DEBUG("%ld %ld %ld %ld", row_n, col_n, pos, H_fin);
842 if (0<= pos && pos <MAX_SAMPLE)// && row_n%2)
843 //sample_tab[pos] = a>0xff?0xFF:a;
844 //sample_tab[(int)L] = H ;
845 scan_params.sample_tab[pos] = H_fin;
847 if ((scan_params.last_row_n == row_n) && ABS(last_pos-pos)>1){
848 /* we have a hole, pad it with minimal hight */
849 if (H_fin>scan_params.last_sample)
850 min_sample = scan_params.last_sample;
854 //printf("(%ld, %ld) (%ld %ld)", last_col_n, last_row_n, col_n, row_n);
856 /* fill grow, avoid erasing curent pos */
860 //printf("loop1 on (%ld, %ld) %ld", pos_tmp, last_pos, last_pos-pos_tmp);
865 //printf("loop1 on (%ld, %ld) %ld", pos_tmp, last_pos, last_pos-pos_tmp);
869 for (;pos_tmp< last_pos;pos_tmp++){
870 if (0< pos_tmp && pos_tmp <MAX_SAMPLE){
871 //scan_params.sample_tab[pos_tmp] = min_sample;
880 scan_params.last_row_n = row_n;
881 scan_params.last_col_n = col_n;
882 scan_params.last_sample = H_fin;
885 //printf("pos : %ld", pos);
886 //sample_tab[sample_i] = a>0x1ff?0x1FF:a;
888 //sample_ok_tab[MAX_SAMPLE-sample_i] = PORTF&2;
891 if (((pos <MAX_SAMPLE)) && (tour_pos<=(SCANNER_STEP_TOUR/2)))
892 sample_tab[pos] = 0xffff;
895 scan_params.sample_i--;