projects/microb2009/sensorboard/commands_scan.c

   1 /*
   2  *  Copyright Droids Corporation (2009)
   3  *
   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.
   8  *
   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.
  13  *
  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
  17  *
  18  *  Revision : $Id: commands_scan.c,v 1.1 2009-05-27 20:04:07 zer0 Exp $
  19  *
  20  *  Olivier MATZ <zer0@droids-corp.org>
  21  */
  22
  23 #include <stdio.h>
  24 #include <string.h>
  25
  26 #include <aversive/pgmspace.h>
  27 #include <aversive/wait.h>
  28 #include <aversive/error.h>
  29 #include <time.h>
  30
  31 #include <ax12.h>
  32 #include <uart.h>
  33 #include <pwm_ng.h>
  34 #include <time.h>
  35 #include <spi.h>
  36
  37 #include <pid.h>
  38 #include <quadramp.h>
  39 #include <control_system_manager.h>
  40 #include <blocking_detection_manager.h>
  41
  42 #include <rdline.h>
  43 #include <parse.h>
  44 #include <parse_string.h>
  45 #include <parse_num.h>
  46
  47 #include <adc.h>
  48
  49 #include <math.h>
  50
  51 #include "main.h"
  52 #include "cmdline.h"
  53 #include "../common/i2c_commands.h"
  54 #include "i2c_protocol.h"
  55 #include "actuator.h"
  56
  57 #include <vect_base.h>
  58 #include <lines.h>
  59 #include <polygon.h>
  60
  61 #include "img_processing.h"
  62 #include "scanner.h"
  63
  64
  65
  66 /**********************************************************/
  67 /* sample ADC */
  68 /*
  69 extern uint16_t sample_i;
  70 extern float scan_offset_a;
  71 extern float scan_offset_b;
  72 */
  73 extern struct scan_params scan_params;
  74
  75 //extern uint16_t sample_tab[MAX_SAMPLE];
  76 /* this structure is filled when cmd_sample is parsed successfully */
  77 struct cmd_sample_result {
  78         fixed_string_t arg0;
  79         fixed_string_t arg1;
  80         uint16_t offset_b;
  81         float offset_a;
  82         uint16_t dump_speed;
  83         uint8_t filter;
  84 };
  85
  86 extern int32_t pos_start_scan;
  87 /* function called when cmd_sample is parsed successfully */
  88
  89 #define MAX_OBJECTS 10
  90 Object_bb sac_obj[MAX_OBJECTS];
  91 static void cmd_sample_parsed(void * parsed_result, void * data)
  92 {
  93         struct cmd_sample_result * res = parsed_result;
  94         uint16_t i;
  95
  96         printf_P(PSTR("cmd sample called\r\n"));
  97         printf_P(PSTR("arg %s %d\r\n"), res->arg1, res->offset_b);
  98
  99         quadramp_set_1st_order_vars(&sensorboard.scanner.qr, res->dump_speed, res->dump_speed); /* set speed */
 100
 101
 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;
 105
 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);
 110
 111                 memset(scan_params.sample_tab, 0xff, MAX_SAMPLE*sizeof(uint8_t));
 112
 113
 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);
 116
 117                 scan_params.last_col_n = 0;
 118                 scan_params.last_row_n = 0;
 119                 scan_params.last_sample = 0;
 120
 121         }
 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;
 129                 }
 130
 131                 //parcour_img(sample_tab, PIX_PER_SCAN, MAX_SAMPLE/PIX_PER_SCAN, sac_obj, MAX_OBJECTS);
 132                 /*
 133                 process_img(scan_params.sample_tab, PIX_PER_SCAN, MAX_SAMPLE/PIX_PER_SCAN,
 134                             4, 1,
 135                             0, 15, 15,
 136                             1);
 137                 */
 138
 139                 for (i=0;i<MAX_OBJECTS;i++){
 140                         printf_P(PSTR("obj: %d %d %d %d\r\n"),
 141                                  sac_obj[i].x_min,
 142                                  sac_obj[i].y_min,
 143                                  sac_obj[i].x_max,
 144                                  sac_obj[i].y_max);
 145                 }
 146
 147                 printf_P(PSTR("start dumping %ld\r\n"), PIX_PER_SCAN);
 148
 149                 for (i=0;i<MAX_SAMPLE;i++)
 150                         printf_P(PSTR("%d\r\n"),scan_params.sample_tab[i]);
 151
 152                 printf_P(PSTR("end dumping  (pos: %ld)\r\n"),
 153                          encoders_spi_get_value_scanner((void *)SCANNER_ENC));
 154         }
 155
 156 }
 157
 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);
 166
 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,
 179                 NULL,
 180         },
 181 };
 182
 183
 184
 185
 186 /**********************************************************/
 187 /* sadc tests */
 188
 189 /* this structure is filled when cmd_sadc is parsed successfully */
 190 struct cmd_sadc_result {
 191         fixed_string_t arg0;
 192 };
 193
 194 /* function called when cmd_sadc is parsed successfully */
 195 static void cmd_sadc_parsed(void *parsed_result, void *data)
 196 {
 197
 198         printf_P(PSTR("Scan ADC values:\r\n"));
 199         do {
 200           printf_P(PSTR("%.4d "), adc_get_value( ADC_REF_AVCC | MUX_ADC13 ));
 201           printf_P(PSTR("\r\n"));
 202           wait_ms(100);
 203         } while (!cmdline_keypressed());
 204 }
 205
 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);
 208
 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,
 216                 NULL,
 217         },
 218 };
 219
 220
 221
 222
 223 /**********************************************************/
 224 /* set scanner params */
 225
 226 /* this structure is filled when cmd_scan_params is parsed successfully */
 227 struct cmd_scan_params_result {
 228         fixed_string_t arg0;
 229         int speed;
 230         uint8_t debug;
 231 };
 232
 233 /* function called when cmd_scan_params is parsed successfully */
 234 static void cmd_scan_params_parsed(void * parsed_result, void * data)
 235 {
 236         struct cmd_scan_params_result * res = parsed_result;
 237
 238         scan_params.speed = res->speed;
 239         scan_params.debug = res->debug;
 240
 241 }
 242
 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);
 247
 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,
 257                 NULL,
 258         },
 259 };
 260
 261
 262
 263 /**********************************************************/
 264 /* set scanner calibration */
 265
 266 /* this structure is filled when cmd_scan_calibre is parsed successfully */
 267 struct cmd_scan_calibre_result {
 268         fixed_string_t arg0;
 269         fixed_string_t arg1;
 270 };
 271
 272 /* function called when cmd_scan_calibre is parsed successfully */
 273 static void cmd_scan_calibre_parsed(void * parsed_result, void * data)
 274 {
 275         struct cmd_scan_calibre_result * res = parsed_result;
 276
 277         printf_P(PSTR("Starting scanner autocalibration\r\n"));
 278
 279
 280         if (!strcmp_P(res->arg1, PSTR("mirror"))) {
 281                 scanner_calibre_mirror();
 282         }
 283         else{
 284                 scanner_calibre_laser();
 285         }
 286 }
 287
 288 prog_char str_scan_calibre_arg0[] = "scan_calibre";
 289
 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);
 293
 294
 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,
 303                 NULL,
 304         },
 305 };
 306
 307
 308
 309 /**********************************************************/
 310 /* start scan */
 311
 312 /* this structure is filled when cmd_scan_do is parsed successfully */
 313 struct cmd_scan_do_result {
 314         fixed_string_t arg0;
 315 };
 316
 317 /* function called when cmd_scan_do is parsed successfully */
 318 static void cmd_scan_do_parsed(void * parsed_result, void * data)
 319 {
 320           printf_P(PSTR("Starting scan\r\n"));
 321           scanner_scan_autonomous();
 322 }
 323
 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);
 326
 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,
 334                 NULL,
 335         },
 336 };
 337
 338
 339
 340
 341 /**********************************************************/
 342 /* set scanner img params */
 343
 344 /* this structure is filled when cmd_scan_img is parsed successfully */
 345 struct cmd_scan_img_result {
 346         fixed_string_t arg0;
 347         uint8_t algo;
 348         uint8_t h;
 349         int16_t x;
 350         int16_t y;
 351
 352 };
 353
 354 /* function called when cmd_scan_img is parsed successfully */
 355 static void cmd_scan_img_parsed(void * parsed_result, void * data)
 356 {
 357         struct cmd_scan_img_result * res = parsed_result;
 358
 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;
 372         }
 373
 374
 375
 376 }
 377
 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);
 384
 385
 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,
 397                 NULL,
 398         },
 399 };
 400
 401