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