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