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