2 * Copyright Droids Corporation (2009)
4 * This program is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation; either version 2 of the License, or
7 * (at your option) any later version.
9 * This program is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
14 * You should have received a copy of the GNU General Public License
15 * along with this program; if not, write to the Free Software
16 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
18 * Revision : $Id: commands_scan.c,v 1.1 2009-05-27 20:04:07 zer0 Exp $
20 * Olivier MATZ <zer0@droids-corp.org>
26 #include <aversive/pgmspace.h>
27 #include <aversive/wait.h>
28 #include <aversive/error.h>
39 #include <control_system_manager.h>
40 #include <blocking_detection_manager.h>
44 #include <parse_string.h>
45 #include <parse_num.h>
53 #include "../common/i2c_commands.h"
54 #include "i2c_protocol.h"
57 #include <vect_base.h>
61 #include "img_processing.h"
66 /**********************************************************/
69 extern uint16_t sample_i;
70 extern float scan_offset_a;
71 extern float scan_offset_b;
73 extern struct scan_params scan_params;
75 //extern uint16_t sample_tab[MAX_SAMPLE];
76 /* this structure is filled when cmd_sample is parsed successfully */
77 struct cmd_sample_result {
86 extern int32_t pos_start_scan;
87 /* function called when cmd_sample is parsed successfully */
89 #define MAX_OBJECTS 10
90 Object_bb sac_obj[MAX_OBJECTS];
91 static void cmd_sample_parsed(void * parsed_result, void * data)
93 struct cmd_sample_result * res = parsed_result;
96 printf_P(PSTR("cmd sample called\r\n"));
97 printf_P(PSTR("arg %s %d\r\n"), res->arg1, res->offset_b);
99 quadramp_set_1st_order_vars(&sensorboard.scanner.qr, res->dump_speed, res->dump_speed); /* set speed */
102 scan_params.offset_b = (((float)res->offset_b)*M_PI/180.);
103 scan_params.offset_a = (((float)res->offset_a)*M_PI/180.);
104 scan_params.filter = res->filter;
106 if (!strcmp_P(res->arg1, PSTR("start"))) {
107 scan_params.sample_i = MAX_SAMPLE;
108 scan_params.pos_start_scan = encoders_spi_get_value_scanner(SCANNER_ENC);
109 //printf_P(PSTR("start scan at pos %ld\r\n"), scan_params.pos_start_scan);
111 memset(scan_params.sample_tab, 0xff, MAX_SAMPLE*sizeof(uint8_t));
114 cs_set_consign(&sensorboard.scanner.cs, scan_params.pos_start_scan+SCANNER_STEP_TOUR*200L);
115 //printf_P(PSTR("scan dst %ld\r\n"), scan_params.pos_start_scan+SCANNER_STEP_TOUR*200L);
117 scan_params.last_col_n = 0;
118 scan_params.last_row_n = 0;
119 scan_params.last_sample = 0;
122 else if (!strcmp_P(res->arg1, PSTR("dump"))) {
123 printf_P(PSTR("start object detection\r\n"));
124 for (i=0;i<MAX_OBJECTS;i++){
125 sac_obj[i].x_min = 0;
126 sac_obj[i].x_max = 0;
127 sac_obj[i].y_min = 0;
128 sac_obj[i].y_max = 0;
131 //parcour_img(sample_tab, PIX_PER_SCAN, MAX_SAMPLE/PIX_PER_SCAN, sac_obj, MAX_OBJECTS);
133 process_img(scan_params.sample_tab, PIX_PER_SCAN, MAX_SAMPLE/PIX_PER_SCAN,
139 for (i=0;i<MAX_OBJECTS;i++){
140 printf_P(PSTR("obj: %d %d %d %d\r\n"),
147 printf_P(PSTR("start dumping %ld\r\n"), PIX_PER_SCAN);
149 for (i=0;i<MAX_SAMPLE;i++)
150 printf_P(PSTR("%d\r\n"),scan_params.sample_tab[i]);
152 printf_P(PSTR("end dumping (pos: %ld)\r\n"),
153 encoders_spi_get_value_scanner((void *)SCANNER_ENC));
158 prog_char str_sample_arg0[] = "sample";
159 parse_pgm_token_string_t cmd_sample_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_sample_result, arg0, str_sample_arg0);
160 prog_char str_sample_arg1[] = "start#dump";
161 parse_pgm_token_string_t cmd_sample_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_sample_result, arg1, str_sample_arg1);
162 parse_pgm_token_num_t cmd_sample_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_sample_result, offset_b, UINT16);
163 parse_pgm_token_num_t cmd_sample_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_sample_result, offset_a, FLOAT);
164 parse_pgm_token_num_t cmd_sample_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_sample_result, dump_speed, UINT16);
165 parse_pgm_token_num_t cmd_sample_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_sample_result, filter, UINT8);
167 prog_char help_sample[] = "Sample func off_a_mot, off_a_angl, dump_speed";
168 parse_pgm_inst_t cmd_sample = {
169 .f = cmd_sample_parsed, /* function to call */
170 .data = NULL, /* 2nd arg of func */
171 .help_str = help_sample,
172 .tokens = { /* token list, NULL terminated */
173 (prog_void *)&cmd_sample_arg0,
174 (prog_void *)&cmd_sample_arg1,
175 (prog_void *)&cmd_sample_arg2,
176 (prog_void *)&cmd_sample_arg3,
177 (prog_void *)&cmd_sample_arg4,
178 (prog_void *)&cmd_sample_arg5,
186 /**********************************************************/
189 /* this structure is filled when cmd_sadc is parsed successfully */
190 struct cmd_sadc_result {
194 /* function called when cmd_sadc is parsed successfully */
195 static void cmd_sadc_parsed(void *parsed_result, void *data)
198 printf_P(PSTR("Scan ADC values:\r\n"));
200 printf_P(PSTR("%.4d "), adc_get_value( ADC_REF_AVCC | MUX_ADC13 ));
201 printf_P(PSTR("\r\n"));
203 } while (!cmdline_keypressed());
206 prog_char str_sadc_arg0[] = "sadc";
207 parse_pgm_token_string_t cmd_sadc_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_sadc_result, arg0, str_sadc_arg0);
209 prog_char help_sadc[] = "Show sadc values";
210 parse_pgm_inst_t cmd_sadc = {
211 .f = cmd_sadc_parsed, /* function to call */
212 .data = NULL, /* 2nd arg of func */
213 .help_str = help_sadc,
214 .tokens = { /* token list, NULL terminated */
215 (prog_void *)&cmd_sadc_arg0,
223 /**********************************************************/
224 /* set scanner params */
226 /* this structure is filled when cmd_scan_params is parsed successfully */
227 struct cmd_scan_params_result {
233 /* function called when cmd_scan_params is parsed successfully */
234 static void cmd_scan_params_parsed(void * parsed_result, void * data)
236 struct cmd_scan_params_result * res = parsed_result;
238 scan_params.speed = res->speed;
239 scan_params.debug = res->debug;
243 prog_char str_scan_params_arg0[] = "scan_params";
244 parse_pgm_token_string_t cmd_scan_params_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scan_params_result, arg0, str_scan_params_arg0);
245 parse_pgm_token_num_t cmd_scan_params_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_scan_params_result, speed, INT16);
246 parse_pgm_token_num_t cmd_scan_params_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_scan_params_result, debug, UINT8);
248 prog_char help_scan_params[] = "Set scanner params (speed, debug)";
249 parse_pgm_inst_t cmd_scan_params = {
250 .f = cmd_scan_params_parsed, /* function to call */
251 .data = NULL, /* 2nd arg of func */
252 .help_str = help_sample,
253 .tokens = { /* token list, NULL terminated */
254 (prog_void *)&cmd_scan_params_arg0,
255 (prog_void *)&cmd_scan_params_arg1,
256 (prog_void *)&cmd_scan_params_arg2,
263 /**********************************************************/
264 /* set scanner calibration */
266 /* this structure is filled when cmd_scan_calibre is parsed successfully */
267 struct cmd_scan_calibre_result {
272 /* function called when cmd_scan_calibre is parsed successfully */
273 static void cmd_scan_calibre_parsed(void * parsed_result, void * data)
275 struct cmd_scan_calibre_result * res = parsed_result;
277 printf_P(PSTR("Starting scanner autocalibration\r\n"));
280 if (!strcmp_P(res->arg1, PSTR("mirror"))) {
281 scanner_calibre_mirror();
284 scanner_calibre_laser();
288 prog_char str_scan_calibre_arg0[] = "scan_calibre";
290 prog_char str_scan_calibre_what_arg1[] = "mirror#laser";
291 parse_pgm_token_string_t cmd_scan_calibre_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scan_calibre_result, arg0, str_scan_calibre_arg0);
292 parse_pgm_token_string_t cmd_scan_calibre_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_scan_calibre_result, arg1, str_scan_calibre_what_arg1);
295 prog_char help_scan_calibre[] = "Scanner auto calibration (mirror|laser)";
296 parse_pgm_inst_t cmd_scan_calibre = {
297 .f = cmd_scan_calibre_parsed, /* function to call */
298 .data = NULL, /* 2nd arg of func */
299 .help_str = help_scan_calibre,
300 .tokens = { /* token list, NULL terminated */
301 (prog_void *)&cmd_scan_calibre_arg0,
302 (prog_void *)&cmd_scan_calibre_arg1,
309 /**********************************************************/
312 /* this structure is filled when cmd_scan_do is parsed successfully */
313 struct cmd_scan_do_result {
317 /* function called when cmd_scan_do is parsed successfully */
318 static void cmd_scan_do_parsed(void * parsed_result, void * data)
320 printf_P(PSTR("Starting scan\r\n"));
321 scanner_scan_autonomous();
324 prog_char str_scan_do_arg0[] = "scan_do";
325 parse_pgm_token_string_t cmd_scan_do_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scan_do_result, arg0, str_scan_do_arg0);
327 prog_char help_scan_do[] = "Scan zone";
328 parse_pgm_inst_t cmd_scan_do = {
329 .f = cmd_scan_do_parsed, /* function to call */
330 .data = NULL, /* 2nd arg of func */
331 .help_str = help_scan_do,
332 .tokens = { /* token list, NULL terminated */
333 (prog_void *)&cmd_scan_do_arg0,
341 /**********************************************************/
342 /* set scanner img params */
344 /* this structure is filled when cmd_scan_img is parsed successfully */
345 struct cmd_scan_img_result {
354 /* function called when cmd_scan_img is parsed successfully */
355 static void cmd_scan_img_parsed(void * parsed_result, void * data)
357 struct cmd_scan_img_result * res = parsed_result;
359 scan_params.algo = res->algo;
360 if (res->algo == I2C_SCANNER_ALGO_COLUMN_DROPZONE) {
361 scan_params.drop_zone.working_zone = res->h;
362 scan_params.drop_zone.center_x = res->x;
363 scan_params.drop_zone.center_y = res->y;
364 } else if (res->algo == I2C_SCANNER_ALGO_CHECK_TEMPLE) {
365 scan_params.check_temple.level = res->h;
366 scan_params.check_temple.temple_x = res->x;
367 scan_params.check_temple.temple_y = res->y;
368 } else if (res->algo == I2C_SCANNER_ALGO_TEMPLE_DROPZONE) {
369 scan_params.drop_zone.working_zone = res->h;
370 scan_params.drop_zone.center_x = res->x;
371 scan_params.drop_zone.center_y = res->y;
378 prog_char str_scan_img_arg0[] = "scan_img";
379 parse_pgm_token_string_t cmd_scan_img_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scan_img_result, arg0, str_scan_img_arg0);
380 parse_pgm_token_num_t cmd_scan_img_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_scan_img_result, algo, UINT8);
381 parse_pgm_token_num_t cmd_scan_img_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_scan_img_result, h, UINT8);
382 parse_pgm_token_num_t cmd_scan_img_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_scan_img_result, x, INT16);
383 parse_pgm_token_num_t cmd_scan_img_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_scan_img_result, y, INT16);
386 prog_char help_scan_img[] = "Set scanner img processing params (algo, H, x, y)";
387 parse_pgm_inst_t cmd_scan_img = {
388 .f = cmd_scan_img_parsed, /* function to call */
389 .data = NULL, /* 2nd arg of func */
390 .help_str = help_scan_img,
391 .tokens = { /* token list, NULL terminated */
392 (prog_void *)&cmd_scan_img_arg0,
393 (prog_void *)&cmd_scan_img_arg1,
394 (prog_void *)&cmd_scan_img_arg2,
395 (prog_void *)&cmd_scan_img_arg3,
396 (prog_void *)&cmd_scan_img_arg4,