projects/microb2010/mainboard/commands_cs.c

   1 /*
   2  *  Copyright Droids Corporation (2008)
   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_cs.c,v 1.4 2009-05-02 10:08:09 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
  30 #include <ax12.h>
  31 #include <uart.h>
  32 #include <pwm_ng.h>
  33 #include <clock_time.h>
  34
  35 #include <pid.h>
  36 #include <quadramp.h>
  37 #include <control_system_manager.h>
  38 #include <trajectory_manager.h>
  39 #include <vect_base.h>
  40 #include <lines.h>
  41 #include <polygon.h>
  42 #include <obstacle_avoidance.h>
  43 #include <blocking_detection_manager.h>
  44 #include <robot_system.h>
  45 #include <position_manager.h>
  46
  47 #include <rdline.h>
  48 #include <parse.h>
  49 #include <parse_string.h>
  50 #include <parse_num.h>
  51
  52 #include "main.h"
  53 #include "cs.h"
  54 #include "cmdline.h"
  55
  56 struct csb_list {
  57         const prog_char *name;
  58         struct cs_block *csb;
  59 };
  60
  61 prog_char csb_angle_str[] = "angle";
  62 prog_char csb_distance_str[] = "distance";
  63 prog_char csb_left_cobroller_str[] = "left_cobroller";
  64 prog_char csb_right_cobroller_str[] = "right_cobroller";
  65 struct csb_list csb_list[] = {
  66         { .name = csb_angle_str, .csb = &mainboard.angle },
  67         { .name = csb_distance_str, .csb = &mainboard.distance },
  68         { .name = csb_left_cobroller_str, .csb = &mainboard.left_cobroller },
  69         { .name = csb_right_cobroller_str, .csb = &mainboard.right_cobroller },
  70 };
  71
  72 struct cmd_cs_result {
  73         fixed_string_t cmdname;
  74         fixed_string_t csname;
  75 };
  76
  77 /* token to be used for all cs-related commands */
  78 prog_char str_csb_name[] = "angle#distance#left_cobroller#right_cobroller";
  79 parse_pgm_token_string_t cmd_csb_name_tok = TOKEN_STRING_INITIALIZER(struct cmd_cs_result, csname, str_csb_name);
  80
  81 struct cs_block *cs_from_name(const char *name)
  82 {
  83         int i;
  84
  85         for (i=0; i<(sizeof(csb_list)/sizeof(*csb_list)); i++) {
  86                 if (!strcmp_P(name, csb_list[i].name))
  87                         return csb_list[i].csb;
  88         }
  89         return NULL;
  90 }
  91
  92 /**********************************************************/
  93 /* Gains for control system */
  94
  95 /* this structure is filled when cmd_gain is parsed successfully */
  96 struct cmd_gain_result {
  97         struct cmd_cs_result cs;
  98         int16_t p;
  99         int16_t i;
 100         int16_t d;
 101 };
 102
 103 /* function called when cmd_gain is parsed successfully */
 104 static void cmd_gain_parsed(void * parsed_result, void *show)
 105 {
 106         struct cmd_gain_result *res = parsed_result;
 107         struct cs_block *csb;
 108
 109         csb = cs_from_name(res->cs.csname);
 110         if (csb == NULL) {
 111                 printf_P(PSTR("null csb\r\n"));
 112                 return;
 113         }
 114
 115         if (!show)
 116                 pid_set_gains(&csb->pid, res->p, res->i, res->d);
 117
 118         printf_P(PSTR("%s %s %d %d %d\r\n"),
 119                  res->cs.cmdname,
 120                  res->cs.csname,
 121                  pid_get_gain_P(&csb->pid),
 122                  pid_get_gain_I(&csb->pid),
 123                  pid_get_gain_D(&csb->pid));
 124 }
 125
 126 prog_char str_gain_arg0[] = "gain";
 127 parse_pgm_token_string_t cmd_gain_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_gain_result, cs.cmdname, str_gain_arg0);
 128 parse_pgm_token_num_t cmd_gain_p = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, p, INT16);
 129 parse_pgm_token_num_t cmd_gain_i = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, i, INT16);
 130 parse_pgm_token_num_t cmd_gain_d = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, d, INT16);
 131
 132 prog_char help_gain[] = "Set gain values for PID";
 133 parse_pgm_inst_t cmd_gain = {
 134         .f = cmd_gain_parsed,  /* function to call */
 135         .data = NULL,      /* 2nd arg of func */
 136         .help_str = help_gain,
 137         .tokens = {        /* token list, NULL terminated */
 138                 (prog_void *)&cmd_gain_arg0,
 139                 (prog_void *)&cmd_csb_name_tok,
 140                 (prog_void *)&cmd_gain_p,
 141                 (prog_void *)&cmd_gain_i,
 142                 (prog_void *)&cmd_gain_d,
 143                 NULL,
 144         },
 145 };
 146
 147 /* show */
 148 /* this structure is filled when cmd_gain is parsed successfully */
 149 struct cmd_gain_show_result {
 150         struct cmd_cs_result cs;
 151         fixed_string_t show;
 152 };
 153
 154 prog_char str_gain_show_arg[] = "show";
 155 parse_pgm_token_string_t cmd_gain_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_gain_show_result, show, str_gain_show_arg);
 156
 157 prog_char help_gain_show[] = "Show gain values for PID";
 158 parse_pgm_inst_t cmd_gain_show = {
 159         .f = cmd_gain_parsed,  /* function to call */
 160         .data = (void *)1,      /* 2nd arg of func */
 161         .help_str = help_gain_show,
 162         .tokens = {        /* token list, NULL terminated */
 163                 (prog_void *)&cmd_gain_arg0,
 164                 (prog_void *)&cmd_csb_name_tok,
 165                 (prog_void *)&cmd_gain_show_arg,
 166                 NULL,
 167         },
 168 };
 169
 170 /**********************************************************/
 171 /* Speeds for control system */
 172
 173 /* this structure is filled when cmd_speed is parsed successfully */
 174 struct cmd_speed_result {
 175         struct cmd_cs_result cs;
 176         uint16_t s;
 177 };
 178
 179 /* function called when cmd_speed is parsed successfully */
 180 static void cmd_speed_parsed(void *parsed_result, void *show)
 181 {
 182         struct cmd_speed_result * res = parsed_result;
 183
 184         struct cs_block *csb;
 185
 186         csb = cs_from_name(res->cs.csname);
 187         if (csb == NULL) {
 188                 printf_P(PSTR("null csb\r\n"));
 189                 return;
 190         }
 191
 192 #if notyet
 193         if (!show)
 194                 ramp_set_vars(&csb->ramp, res->s, res->s); /* set speed */
 195
 196         printf_P(PSTR("%s %lu\r\n"),
 197                  res->cs.csname,
 198                  ext.r_b.var_pos);
 199 #else
 200         printf_P(PSTR("TODO\r\n"));
 201 #endif
 202 }
 203
 204 prog_char str_speed_arg0[] = "speed";
 205 parse_pgm_token_string_t cmd_speed_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_speed_result, cs.cmdname, str_speed_arg0);
 206 parse_pgm_token_num_t cmd_speed_s = TOKEN_NUM_INITIALIZER(struct cmd_speed_result, s, UINT16);
 207
 208 prog_char help_speed[] = "Set speed values for ramp filter";
 209 parse_pgm_inst_t cmd_speed = {
 210         .f = cmd_speed_parsed,  /* function to call */
 211         .data = NULL,      /* 2nd arg of func */
 212         .help_str = help_speed,
 213         .tokens = {        /* token list, NULL terminated */
 214                 (prog_void *)&cmd_speed_arg0,
 215                 (prog_void *)&cmd_csb_name_tok,
 216                 (prog_void *)&cmd_speed_s,
 217                 NULL,
 218         },
 219 };
 220
 221 /* show */
 222 struct cmd_speed_show_result {
 223         struct cmd_cs_result cs;
 224         fixed_string_t show;
 225 };
 226
 227 prog_char str_speed_show_arg[] = "show";
 228 parse_pgm_token_string_t cmd_speed_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_speed_show_result, show, str_speed_show_arg);
 229
 230 prog_char help_speed_show[] = "Show speed values for ramp filter";
 231 parse_pgm_inst_t cmd_speed_show = {
 232         .f = cmd_speed_parsed,  /* function to call */
 233         .data = (void *)1,      /* 2nd arg of func */
 234         .help_str = help_speed_show,
 235         .tokens = {        /* token list, NULL terminated */
 236                 (prog_void *)&cmd_speed_arg0,
 237                 (prog_void *)&cmd_csb_name_tok,
 238                 (prog_void *)&cmd_speed_show_arg,
 239                 NULL,
 240         },
 241 };
 242
 243 /**********************************************************/
 244 /* Derivate_Filters for control system */
 245
 246 /* this structure is filled when cmd_derivate_filter is parsed successfully */
 247 struct cmd_derivate_filter_result {
 248         struct cmd_cs_result cs;
 249         uint8_t size;
 250 };
 251
 252 /* function called when cmd_derivate_filter is parsed successfully */
 253 static void cmd_derivate_filter_parsed(void *parsed_result, void *show)
 254 {
 255         struct cmd_derivate_filter_result * res = parsed_result;
 256         struct cs_block *csb;
 257
 258         csb = cs_from_name(res->cs.csname);
 259         if (csb == NULL) {
 260                 printf_P(PSTR("null csb\r\n"));
 261                 return;
 262         }
 263
 264         if (!show)
 265                 pid_set_derivate_filter(&csb->pid, res->size);
 266
 267         printf_P(PSTR("%s %s %u\r\n"),
 268                  res->cs.cmdname,
 269                  res->cs.csname,
 270                  pid_get_derivate_filter(&csb->pid));
 271 }
 272
 273 prog_char str_derivate_filter_arg0[] = "derivate_filter";
 274 parse_pgm_token_string_t cmd_derivate_filter_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_derivate_filter_result, cs.cmdname, str_derivate_filter_arg0);
 275 parse_pgm_token_num_t cmd_derivate_filter_size = TOKEN_NUM_INITIALIZER(struct cmd_derivate_filter_result, size, UINT32);
 276
 277 prog_char help_derivate_filter[] = "Set derivate_filter values for PID (in, I, out)";
 278 parse_pgm_inst_t cmd_derivate_filter = {
 279         .f = cmd_derivate_filter_parsed,  /* function to call */
 280         .data = (void *)1,      /* 2nd arg of func */
 281         .help_str = help_derivate_filter,
 282         .tokens = {        /* token list, NULL terminated */
 283                 (prog_void *)&cmd_derivate_filter_arg0,
 284                 (prog_void *)&cmd_csb_name_tok,
 285                 (prog_void *)&cmd_derivate_filter_size,
 286                 NULL,
 287         },
 288 };
 289
 290 /* show */
 291
 292 struct cmd_derivate_filter_show_result {
 293         struct cmd_cs_result cs;
 294         fixed_string_t show;
 295 };
 296
 297 prog_char str_derivate_filter_show_arg[] = "show";
 298 parse_pgm_token_string_t cmd_derivate_filter_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_derivate_filter_show_result, show, str_derivate_filter_show_arg);
 299
 300 prog_char help_derivate_filter_show[] = "Show derivate_filter values for PID";
 301 parse_pgm_inst_t cmd_derivate_filter_show = {
 302         .f = cmd_derivate_filter_parsed,  /* function to call */
 303         .data = NULL,      /* 2nd arg of func */
 304         .help_str = help_derivate_filter_show,
 305         .tokens = {        /* token list, NULL terminated */
 306                 (prog_void *)&cmd_derivate_filter_arg0,
 307                 (prog_void *)&cmd_csb_name_tok,
 308                 (prog_void *)&cmd_derivate_filter_show_arg,
 309                 NULL,
 310         },
 311 };
 312
 313
 314 /**********************************************************/
 315 /* Consign for control system */
 316
 317 /* this structure is filled when cmd_consign is parsed successfully */
 318 struct cmd_consign_result {
 319         struct cmd_cs_result cs;
 320         int32_t p;
 321 };
 322
 323 /* function called when cmd_consign is parsed successfully */
 324 static void cmd_consign_parsed(void * parsed_result, void *data)
 325 {
 326         struct cmd_consign_result * res = parsed_result;
 327         struct cs_block *csb;
 328
 329         csb = cs_from_name(res->cs.csname);
 330         if (csb == NULL) {
 331                 printf_P(PSTR("null csb\r\n"));
 332                 return;
 333         }
 334
 335         cs_set_consign(&csb->cs, res->p);
 336 }
 337
 338 prog_char str_consign_arg0[] = "consign";
 339 parse_pgm_token_string_t cmd_consign_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_consign_result, cs.cmdname, str_consign_arg0);
 340 parse_pgm_token_num_t cmd_consign_p = TOKEN_NUM_INITIALIZER(struct cmd_consign_result, p, INT32);
 341
 342 prog_char help_consign[] = "Set consign value";
 343 parse_pgm_inst_t cmd_consign = {
 344         .f = cmd_consign_parsed,  /* function to call */
 345         .data = NULL,      /* 2nd arg of func */
 346         .help_str = help_consign,
 347         .tokens = {        /* token list, NULL terminated */
 348                 (prog_void *)&cmd_consign_arg0,
 349                 (prog_void *)&cmd_csb_name_tok,
 350                 (prog_void *)&cmd_consign_p,
 351                 NULL,
 352         },
 353 };
 354
 355
 356 /**********************************************************/
 357 /* Maximums for control system */
 358
 359 /* this structure is filled when cmd_maximum is parsed successfully */
 360 struct cmd_maximum_result {
 361         struct cmd_cs_result cs;
 362         uint32_t in;
 363         uint32_t i;
 364         uint32_t out;
 365 };
 366
 367 /* function called when cmd_maximum is parsed successfully */
 368 static void cmd_maximum_parsed(void *parsed_result, void *show)
 369 {
 370         struct cmd_maximum_result * res = parsed_result;
 371
 372         struct cs_block *csb;
 373
 374         csb = cs_from_name(res->cs.csname);
 375         if (csb == NULL) {
 376                 printf_P(PSTR("null csb\r\n"));
 377                 return;
 378         }
 379
 380         if (!show)
 381                 pid_set_maximums(&csb->pid, res->in, res->i, res->out);
 382
 383         printf_P(PSTR("maximum %s %lu %lu %lu\r\n"),
 384                  res->cs.csname,
 385                  pid_get_max_in(&csb->pid),
 386                  pid_get_max_I(&csb->pid),
 387                  pid_get_max_out(&csb->pid));
 388 }
 389
 390 prog_char str_maximum_arg0[] = "maximum";
 391 parse_pgm_token_string_t cmd_maximum_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_maximum_result, cs.cmdname, str_maximum_arg0);
 392 parse_pgm_token_num_t cmd_maximum_in = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, in, UINT32);
 393 parse_pgm_token_num_t cmd_maximum_i = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, i, UINT32);
 394 parse_pgm_token_num_t cmd_maximum_out = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, out, UINT32);
 395
 396 prog_char help_maximum[] = "Set maximum values for PID (in, I, out)";
 397 parse_pgm_inst_t cmd_maximum = {
 398         .f = cmd_maximum_parsed,  /* function to call */
 399         .data = NULL,      /* 2nd arg of func */
 400         .help_str = help_maximum,
 401         .tokens = {        /* token list, NULL terminated */
 402                 (prog_void *)&cmd_maximum_arg0,
 403                 (prog_void *)&cmd_csb_name_tok,
 404                 (prog_void *)&cmd_maximum_in,
 405                 (prog_void *)&cmd_maximum_i,
 406                 (prog_void *)&cmd_maximum_out,
 407                 NULL,
 408         },
 409 };
 410
 411 /* show */
 412
 413 /* this structure is filled when cmd_maximum is parsed successfully */
 414 struct cmd_maximum_show_result {
 415         struct cmd_cs_result cs;
 416         fixed_string_t show;
 417 };
 418 prog_char str_maximum_show_arg[] = "show";
 419 parse_pgm_token_string_t cmd_maximum_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_maximum_show_result, show, str_maximum_show_arg);
 420
 421 prog_char help_maximum_show[] = "Show maximum values for PID";
 422 parse_pgm_inst_t cmd_maximum_show = {
 423         .f = cmd_maximum_parsed,  /* function to call */
 424         .data = (void *)1,      /* 2nd arg of func */
 425         .help_str = help_maximum_show,
 426         .tokens = {        /* token list, NULL terminated */
 427                 (prog_void *)&cmd_maximum_arg0,
 428                 (prog_void *)&cmd_csb_name_tok,
 429                 (prog_void *)&cmd_maximum_show_arg,
 430                 NULL,
 431         },
 432 };
 433
 434 /**********************************************************/
 435 /* Quadramp for control system */
 436
 437 /* this structure is filled when cmd_quadramp is parsed successfully */
 438 struct cmd_quadramp_result {
 439         struct cmd_cs_result cs;
 440         uint32_t ap;
 441         uint32_t an;
 442         uint32_t sp;
 443         uint32_t sn;
 444 };
 445
 446 /* function called when cmd_quadramp is parsed successfully */
 447 static void cmd_quadramp_parsed(void *parsed_result, void *show)
 448 {
 449         struct cmd_quadramp_result * res = parsed_result;
 450
 451         struct cs_block *csb;
 452
 453         csb = cs_from_name(res->cs.csname);
 454         if (csb == NULL) {
 455                 printf_P(PSTR("null csb\r\n"));
 456                 return;
 457         }
 458
 459         if (!show)  {
 460                 quadramp_set_1st_order_vars(&csb->qr, res->sp, res->sn);
 461                 quadramp_set_2nd_order_vars(&csb->qr, res->ap, res->an);
 462         }
 463
 464         printf_P(PSTR("quadramp %s %ld %ld %ld %ld\r\n"),
 465                  res->cs.csname,
 466                  csb->qr.var_2nd_ord_pos,
 467                  csb->qr.var_2nd_ord_neg,
 468                  csb->qr.var_1st_ord_pos,
 469                  csb->qr.var_1st_ord_neg);
 470 }
 471
 472 prog_char str_quadramp_arg0[] = "quadramp";
 473 parse_pgm_token_string_t cmd_quadramp_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_quadramp_result, cs.cmdname, str_quadramp_arg0);
 474 parse_pgm_token_num_t cmd_quadramp_ap = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, ap, UINT32);
 475 parse_pgm_token_num_t cmd_quadramp_an = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, an, UINT32);
 476 parse_pgm_token_num_t cmd_quadramp_sp = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sp, UINT32);
 477 parse_pgm_token_num_t cmd_quadramp_sn = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sn, UINT32);
 478
 479 prog_char help_quadramp[] = "Set quadramp values (acc+, acc-, speed+, speed-)";
 480 parse_pgm_inst_t cmd_quadramp = {
 481         .f = cmd_quadramp_parsed,  /* function to call */
 482         .data = NULL,      /* 2nd arg of func */
 483         .help_str = help_quadramp,
 484         .tokens = {        /* token list, NULL terminated */
 485                 (prog_void *)&cmd_quadramp_arg0,
 486                 (prog_void *)&cmd_csb_name_tok,
 487                 (prog_void *)&cmd_quadramp_ap,
 488                 (prog_void *)&cmd_quadramp_an,
 489                 (prog_void *)&cmd_quadramp_sp,
 490                 (prog_void *)&cmd_quadramp_sn,
 491
 492                 NULL,
 493         },
 494 };
 495
 496 /* show */
 497
 498 struct cmd_quadramp_show_result {
 499         struct cmd_cs_result cs;
 500         fixed_string_t show;
 501 };
 502
 503 prog_char str_quadramp_show_arg[] = "show";
 504 parse_pgm_token_string_t cmd_quadramp_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_quadramp_show_result, show, str_quadramp_show_arg);
 505
 506 prog_char help_quadramp_show[] = "Get quadramp values for control system";
 507 parse_pgm_inst_t cmd_quadramp_show = {
 508         .f = cmd_quadramp_parsed,  /* function to call */
 509         .data = (void *)1,      /* 2nd arg of func */
 510         .help_str = help_quadramp_show,
 511         .tokens = {        /* token list, NULL terminated */
 512                 (prog_void *)&cmd_quadramp_arg0,
 513                 (prog_void *)&cmd_csb_name_tok,
 514                 (prog_void *)&cmd_quadramp_show_arg,
 515                 NULL,
 516         },
 517 };
 518
 519
 520
 521 /**********************************************************/
 522 /* cs_status show for control system */
 523
 524 /* this structure is filled when cmd_cs_status is parsed successfully */
 525 struct cmd_cs_status_result {
 526         struct cmd_cs_result cs;
 527         fixed_string_t arg;
 528 };
 529
 530 /* function called when cmd_cs_status is parsed successfully */
 531 static void cmd_cs_status_parsed(void *parsed_result, void *data)
 532 {
 533         struct cmd_cs_status_result *res = parsed_result;
 534         struct cs_block *csb;
 535         uint8_t loop = 0;
 536         uint8_t print_pid = 0, print_cs = 0;
 537
 538         csb = cs_from_name(res->cs.csname);
 539         if (csb == NULL) {
 540                 printf_P(PSTR("null csb\r\n"));
 541                 return;
 542         }
 543         if (strcmp_P(res->arg, PSTR("on")) == 0) {
 544                 csb->on = 1;
 545                 printf_P(PSTR("%s is on\r\n"), res->cs.csname);
 546                 return;
 547         }
 548         else if (strcmp_P(res->arg, PSTR("off")) == 0) {
 549                 csb->on = 0;
 550                 printf_P(PSTR("%s is off\r\n"), res->cs.csname);
 551                 return;
 552         }
 553         else if (strcmp_P(res->arg, PSTR("show")) == 0) {
 554                 print_cs = 1;
 555         }
 556         else if (strcmp_P(res->arg, PSTR("loop_show")) == 0) {
 557                 loop = 1;
 558                 print_cs = 1;
 559         }
 560         else if (strcmp_P(res->arg, PSTR("pid_show")) == 0) {
 561                 print_pid = 1;
 562         }
 563         else if (strcmp_P(res->arg, PSTR("pid_loop_show")) == 0) {
 564                 print_pid = 1;
 565                 loop = 1;
 566         }
 567
 568         printf_P(PSTR("%s cs is %s\r\n"), res->cs.csname, csb->on ? "on":"off");
 569         do {
 570                 if (print_cs)
 571                         dump_cs(res->cs.csname, &csb->cs);
 572                 if (print_pid)
 573                         dump_pid(res->cs.csname, &csb->pid);
 574                 wait_ms(100);
 575         } while(loop && !cmdline_keypressed());
 576 }
 577
 578 prog_char str_cs_status_arg0[] = "cs_status";
 579 parse_pgm_token_string_t cmd_cs_status_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cs_status_result, cs.cmdname, str_cs_status_arg0);
 580 prog_char str_cs_status_arg[] = "pid_show#pid_loop_show#show#loop_show#on#off";
 581 parse_pgm_token_string_t cmd_cs_status_arg = TOKEN_STRING_INITIALIZER(struct cmd_cs_status_result, arg, str_cs_status_arg);
 582
 583 prog_char help_cs_status[] = "Show cs status";
 584 parse_pgm_inst_t cmd_cs_status = {
 585         .f = cmd_cs_status_parsed,  /* function to call */
 586         .data = NULL,      /* 2nd arg of func */
 587         .help_str = help_cs_status,
 588         .tokens = {        /* token list, NULL terminated */
 589                 (prog_void *)&cmd_cs_status_arg0,
 590                 (prog_void *)&cmd_csb_name_tok,
 591                 (prog_void *)&cmd_cs_status_arg,
 592                 NULL,
 593         },
 594 };
 595
 596
 597 /**********************************************************/
 598 /* Blocking_I for control system */
 599
 600 /* this structure is filled when cmd_blocking_i is parsed successfully */
 601 struct cmd_blocking_i_result {
 602         struct cmd_cs_result cs;
 603         int32_t k1;
 604         int32_t k2;
 605         uint32_t i;
 606         uint16_t cpt;
 607 };
 608
 609 /* function called when cmd_blocking_i is parsed successfully */
 610 static void cmd_blocking_i_parsed(void *parsed_result, void *show)
 611 {
 612         struct cmd_blocking_i_result * res = parsed_result;
 613
 614         struct cs_block *csb;
 615
 616         csb = cs_from_name(res->cs.csname);
 617         if (csb == NULL) {
 618                 printf_P(PSTR("null csb\r\n"));
 619                 return;
 620         }
 621
 622         if (!show)
 623                 bd_set_current_thresholds(&csb->bd, res->k1, res->k2,
 624                                           res->i, res->cpt);
 625
 626         printf_P(PSTR("%s %s %ld %ld %ld %d\r\n"),
 627                  res->cs.cmdname,
 628                  res->cs.csname,
 629                  csb->bd.k1,
 630                  csb->bd.k2,
 631                  csb->bd.i_thres,
 632                  csb->bd.cpt_thres);
 633 }
 634
 635 prog_char str_blocking_i_arg0[] = "blocking";
 636 parse_pgm_token_string_t cmd_blocking_i_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_blocking_i_result, cs.cmdname, str_blocking_i_arg0);
 637 parse_pgm_token_num_t cmd_blocking_i_k1 = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, k1, INT32);
 638 parse_pgm_token_num_t cmd_blocking_i_k2 = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, k2, INT32);
 639 parse_pgm_token_num_t cmd_blocking_i_i = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, i, UINT32);
 640 parse_pgm_token_num_t cmd_blocking_i_cpt = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, cpt, UINT16);
 641
 642 prog_char help_blocking_i[] = "Set blocking detection values (k1, k2, i, cpt)";
 643 parse_pgm_inst_t cmd_blocking_i = {
 644         .f = cmd_blocking_i_parsed,  /* function to call */
 645         .data = NULL,      /* 2nd arg of func */
 646         .help_str = help_blocking_i,
 647         .tokens = {        /* token list, NULL terminated */
 648                 (prog_void *)&cmd_blocking_i_arg0,
 649                 (prog_void *)&cmd_csb_name_tok,
 650                 (prog_void *)&cmd_blocking_i_k1,
 651                 (prog_void *)&cmd_blocking_i_k2,
 652                 (prog_void *)&cmd_blocking_i_i,
 653                 (prog_void *)&cmd_blocking_i_cpt,
 654                 NULL,
 655         },
 656 };
 657
 658 /* show */
 659
 660 struct cmd_blocking_i_show_result {
 661         struct cmd_cs_result cs;
 662         fixed_string_t show;
 663 };
 664
 665 prog_char str_blocking_i_show_arg[] = "show";
 666 parse_pgm_token_string_t cmd_blocking_i_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_blocking_i_show_result, show, str_blocking_i_show_arg);
 667
 668 prog_char help_blocking_i_show[] = "Show blocking detection values";
 669 parse_pgm_inst_t cmd_blocking_i_show = {
 670         .f = cmd_blocking_i_parsed,  /* function to call */
 671         .data = (void *)1,      /* 2nd arg of func */
 672         .help_str = help_blocking_i_show,
 673         .tokens = {        /* token list, NULL terminated */
 674                 (prog_void *)&cmd_blocking_i_arg0,
 675                 (prog_void *)&cmd_csb_name_tok,
 676                 (prog_void *)&cmd_blocking_i_show_arg,
 677                 NULL,
 678         },
 679 };
 680
 681