projects/microb2010/tests/beacon_tsop/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 <pid.h>
  33 #include <rdline.h>
  34 #include <parse.h>
  35 #include <parse_string.h>
  36 #include <parse_num.h>
  37
  38 #include "main.h"
  39 #include "cmdline.h"
  40
  41 extern uint32_t cs_consign;
  42
  43 /**********************************************************/
  44 /* Gains for control system */
  45
  46 /* this structure is filled when cmd_gain is parsed successfully */
  47 struct cmd_gain_result {
  48         fixed_string_t arg0;
  49         int16_t p;
  50         int16_t i;
  51         int16_t d;
  52 };
  53
  54 /* function called when cmd_gain is parsed successfully */
  55 static void cmd_gain_parsed(void * parsed_result, void *show)
  56 {
  57         struct cmd_gain_result *res = parsed_result;
  58
  59         if (!show)
  60                 pid_set_gains(&beacon_tsop.pid, res->p, res->i, res->d);
  61
  62         printf_P(PSTR("gain %d %d %d\r\n"),
  63                  pid_get_gain_P(&beacon_tsop.pid),
  64                  pid_get_gain_I(&beacon_tsop.pid),
  65                  pid_get_gain_D(&beacon_tsop.pid));
  66 }
  67
  68 prog_char str_gain_arg0[] = "gain";
  69 parse_pgm_token_string_t cmd_gain_arg0 =
  70         TOKEN_STRING_INITIALIZER(struct cmd_gain_result,
  71                                  arg0, str_gain_arg0);
  72 parse_pgm_token_num_t cmd_gain_p = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, p, INT16);
  73 parse_pgm_token_num_t cmd_gain_i = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, i, INT16);
  74 parse_pgm_token_num_t cmd_gain_d = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, d, INT16);
  75
  76 prog_char help_gain[] = "Set gain values for PID";
  77 parse_pgm_inst_t cmd_gain = {
  78         .f = cmd_gain_parsed,  /* function to call */
  79         .data = NULL,      /* 2nd arg of func */
  80         .help_str = help_gain,
  81         .tokens = {        /* token list, NULL terminated */
  82                 (prog_void *)&cmd_gain_arg0,
  83                 (prog_void *)&cmd_gain_p,
  84                 (prog_void *)&cmd_gain_i,
  85                 (prog_void *)&cmd_gain_d,
  86                 NULL,
  87         },
  88 };
  89
  90 /* show */
  91 /* this structure is filled when cmd_gain is parsed successfully */
  92 struct cmd_gain_show_result {
  93         fixed_string_t arg0;
  94         fixed_string_t show;
  95 };
  96
  97 prog_char str_gain_show_arg[] = "show";
  98 parse_pgm_token_string_t cmd_gain_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_gain_show_result, show, str_gain_show_arg);
  99
 100 prog_char help_gain_show[] = "Show gain values for PID";
 101 parse_pgm_inst_t cmd_gain_show = {
 102         .f = cmd_gain_parsed,  /* function to call */
 103         .data = (void *)1,      /* 2nd arg of func */
 104         .help_str = help_gain_show,
 105         .tokens = {        /* token list, NULL terminated */
 106                 (prog_void *)&cmd_gain_arg0,
 107                 (prog_void *)&cmd_gain_show_arg,
 108                 NULL,
 109         },
 110 };
 111
 112 /**********************************************************/
 113 /* Derivate_Filters for control system */
 114
 115 /* this structure is filled when cmd_derivate_filter is parsed successfully */
 116 struct cmd_derivate_filter_result {
 117         fixed_string_t arg0;
 118         uint8_t size;
 119 };
 120
 121 /* function called when cmd_derivate_filter is parsed successfully */
 122 static void cmd_derivate_filter_parsed(void *parsed_result, void *show)
 123 {
 124         struct cmd_derivate_filter_result * res = parsed_result;
 125
 126         if (!show)
 127                 pid_set_derivate_filter(&beacon_tsop.pid, res->size);
 128
 129         printf_P(PSTR("derivate_filter %u\r\n"),
 130                  pid_get_derivate_filter(&beacon_tsop.pid));
 131 }
 132
 133 prog_char str_derivate_filter_arg0[] = "derivate_filter";
 134 parse_pgm_token_string_t cmd_derivate_filter_arg0 =
 135         TOKEN_STRING_INITIALIZER(struct cmd_derivate_filter_result,
 136                                  arg0, str_derivate_filter_arg0);
 137 parse_pgm_token_num_t cmd_derivate_filter_size =
 138         TOKEN_NUM_INITIALIZER(struct cmd_derivate_filter_result, size, UINT32);
 139
 140 prog_char help_derivate_filter[] = "Set derivate_filter values for PID (in, I, out)";
 141 parse_pgm_inst_t cmd_derivate_filter = {
 142         .f = cmd_derivate_filter_parsed,  /* function to call */
 143         .data = (void *)1,      /* 2nd arg of func */
 144         .help_str = help_derivate_filter,
 145         .tokens = {        /* token list, NULL terminated */
 146                 (prog_void *)&cmd_derivate_filter_arg0,
 147                 (prog_void *)&cmd_derivate_filter_size,
 148                 NULL,
 149         },
 150 };
 151
 152 /* show */
 153
 154 struct cmd_derivate_filter_show_result {
 155         fixed_string_t arg0;
 156         fixed_string_t show;
 157 };
 158
 159 prog_char str_derivate_filter_show_arg[] = "show";
 160 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);
 161
 162 prog_char help_derivate_filter_show[] = "Show derivate_filter values for PID";
 163 parse_pgm_inst_t cmd_derivate_filter_show = {
 164         .f = cmd_derivate_filter_parsed,  /* function to call */
 165         .data = NULL,      /* 2nd arg of func */
 166         .help_str = help_derivate_filter_show,
 167         .tokens = {        /* token list, NULL terminated */
 168                 (prog_void *)&cmd_derivate_filter_arg0,
 169                 (prog_void *)&cmd_derivate_filter_show_arg,
 170                 NULL,
 171         },
 172 };
 173
 174
 175 /**********************************************************/
 176 /* Maximums for control system */
 177
 178 /* this structure is filled when cmd_maximum is parsed successfully */
 179 struct cmd_maximum_result {
 180         fixed_string_t arg0;
 181         uint32_t in;
 182         uint32_t i;
 183         uint32_t out;
 184 };
 185
 186 /* function called when cmd_maximum is parsed successfully */
 187 static void cmd_maximum_parsed(void *parsed_result, void *show)
 188 {
 189         struct cmd_maximum_result * res = parsed_result;
 190
 191         if (!show)
 192                 pid_set_maximums(&beacon_tsop.pid, res->in, res->i, res->out);
 193
 194         printf_P(PSTR("maximum %lu %lu %lu\r\n"),
 195                  pid_get_max_in(&beacon_tsop.pid),
 196                  pid_get_max_I(&beacon_tsop.pid),
 197                  pid_get_max_out(&beacon_tsop.pid));
 198 }
 199
 200 prog_char str_maximum_arg0[] = "maximum";
 201 parse_pgm_token_string_t cmd_maximum_arg0 =
 202         TOKEN_STRING_INITIALIZER(struct cmd_maximum_result,
 203                                  arg0, str_maximum_arg0);
 204 parse_pgm_token_num_t cmd_maximum_in =
 205         TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, in, UINT32);
 206 parse_pgm_token_num_t cmd_maximum_i =
 207         TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, i, UINT32);
 208 parse_pgm_token_num_t cmd_maximum_out =
 209         TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, out, UINT32);
 210
 211 prog_char help_maximum[] = "Set maximum values for PID (in, I, out)";
 212 parse_pgm_inst_t cmd_maximum = {
 213         .f = cmd_maximum_parsed,  /* function to call */
 214         .data = NULL,      /* 2nd arg of func */
 215         .help_str = help_maximum,
 216         .tokens = {        /* token list, NULL terminated */
 217                 (prog_void *)&cmd_maximum_arg0,
 218                 (prog_void *)&cmd_maximum_in,
 219                 (prog_void *)&cmd_maximum_i,
 220                 (prog_void *)&cmd_maximum_out,
 221                 NULL,
 222         },
 223 };
 224
 225 /* show */
 226
 227 /* this structure is filled when cmd_maximum is parsed successfully */
 228 struct cmd_maximum_show_result {
 229         fixed_string_t arg0;
 230         fixed_string_t show;
 231 };
 232 prog_char str_maximum_show_arg[] = "show";
 233 parse_pgm_token_string_t cmd_maximum_show_arg =
 234         TOKEN_STRING_INITIALIZER(struct cmd_maximum_show_result, show, str_maximum_show_arg);
 235
 236 prog_char help_maximum_show[] = "Show maximum values for PID";
 237 parse_pgm_inst_t cmd_maximum_show = {
 238         .f = cmd_maximum_parsed,  /* function to call */
 239         .data = (void *)1,      /* 2nd arg of func */
 240         .help_str = help_maximum_show,
 241         .tokens = {        /* token list, NULL terminated */
 242                 (prog_void *)&cmd_maximum_arg0,
 243                 (prog_void *)&cmd_maximum_show_arg,
 244                 NULL,
 245         },
 246 };
 247
 248
 249 /**********************************************************/
 250 /* Consigns for control system */
 251
 252 /* this structure is filled when cmd_consign is parsed successfully */
 253 struct cmd_consign_result {
 254         fixed_string_t arg0;
 255         uint32_t cons;
 256 };
 257
 258 /* function called when cmd_consign is parsed successfully */
 259 static void cmd_consign_parsed(void *parsed_result, void *show)
 260 {
 261         struct cmd_consign_result * res = parsed_result;
 262
 263         if (!show)
 264                 cs_consign = res->cons;
 265
 266         printf_P(PSTR("consign %lu\r\n"), cs_consign);
 267 }
 268
 269 prog_char str_consign_arg0[] = "consign";
 270 parse_pgm_token_string_t cmd_consign_arg0 =
 271         TOKEN_STRING_INITIALIZER(struct cmd_consign_result,
 272                                  arg0, str_consign_arg0);
 273 parse_pgm_token_num_t cmd_consign_cons =
 274         TOKEN_NUM_INITIALIZER(struct cmd_consign_result, cons, UINT32);
 275
 276 prog_char help_consign[] = "Set consign";
 277 parse_pgm_inst_t cmd_consign = {
 278         .f = cmd_consign_parsed,  /* function to call */
 279         .data = NULL,      /* 2nd arg of func */
 280         .help_str = help_consign,
 281         .tokens = {        /* token list, NULL terminated */
 282                 (prog_void *)&cmd_consign_arg0,
 283                 (prog_void *)&cmd_consign_cons,
 284                 NULL,
 285         },
 286 };
 287
 288 /* show */
 289
 290 /* this structure is filled when cmd_consign is parsed successfully */
 291 struct cmd_consign_show_result {
 292         fixed_string_t arg0;
 293         fixed_string_t show;
 294 };
 295 prog_char str_consign_show_arg[] = "show";
 296 parse_pgm_token_string_t cmd_consign_show_arg =
 297         TOKEN_STRING_INITIALIZER(struct cmd_consign_show_result, show, str_consign_show_arg);
 298
 299 prog_char help_consign_show[] = "Show consign values for PID";
 300 parse_pgm_inst_t cmd_consign_show = {
 301         .f = cmd_consign_parsed,  /* function to call */
 302         .data = (void *)1,      /* 2nd arg of func */
 303         .help_str = help_consign_show,
 304         .tokens = {        /* token list, NULL terminated */
 305                 (prog_void *)&cmd_consign_arg0,
 306                 (prog_void *)&cmd_consign_show_arg,
 307                 NULL,
 308         },
 309 };
 310