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