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