work on trajectory, update cobboard and ballboard too

[aversive.git] / projects / microb2010 / mainboard / commands_cs.c

diff --git a/projects/microb2010/mainboard/commands_cs.c b/projects/microb2010/mainboard/commands_cs.c
index 272f1e9..c749c61 100644 (file)
--- a/projects/microb2010/mainboard/commands_cs.c
+++ b/projects/microb2010/mainboard/commands_cs.c
@@ -1,6 +1,6 @@
 /*
  *  Copyright Droids Corporation (2008)
- * 
+ *
  *  This program is free software; you can redistribute it and/or modify
  *  it under the terms of the GNU General Public License as published by
  *  the Free Software Foundation; either version 2 of the License, or
@@ -17,7 +17,7 @@
  *
  *  Revision : $Id: commands_cs.c,v 1.4 2009-05-02 10:08:09 zer0 Exp $
  *
- *  Olivier MATZ <zer0@droids-corp.org> 
+ *  Olivier MATZ <zer0@droids-corp.org>
  */
 
 #include <stdio.h>
@@ -89,7 +89,7 @@ struct cs_block *cs_from_name(const char *name)
        }
        return NULL;
 }
-               
+
 /**********************************************************/
 /* Gains for control system */
 
@@ -113,7 +113,7 @@ static void cmd_gain_parsed(void * parsed_result, void *show)
                return;
        }
 
-       if (!show) 
+       if (!show)
                pid_set_gains(&csb->pid, res->p, res->i, res->d);
 
        printf_P(PSTR("%s %s %d %d %d\r\n"),
@@ -136,11 +136,11 @@ parse_pgm_inst_t cmd_gain = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_gain,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_gain_arg0, 
-               (prog_void *)&cmd_csb_name_tok, 
-               (prog_void *)&cmd_gain_p, 
-               (prog_void *)&cmd_gain_i, 
-               (prog_void *)&cmd_gain_d, 
+               (prog_void *)&cmd_gain_arg0,
+               (prog_void *)&cmd_csb_name_tok,
+               (prog_void *)&cmd_gain_p,
+               (prog_void *)&cmd_gain_i,
+               (prog_void *)&cmd_gain_d,
                NULL,
        },
 };
@@ -161,8 +161,8 @@ parse_pgm_inst_t cmd_gain_show = {
        .data = (void *)1,      /* 2nd arg of func */
        .help_str = help_gain_show,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_gain_arg0, 
-               (prog_void *)&cmd_csb_name_tok, 
+               (prog_void *)&cmd_gain_arg0,
+               (prog_void *)&cmd_csb_name_tok,
                (prog_void *)&cmd_gain_show_arg,
                NULL,
        },
@@ -181,7 +181,7 @@ struct cmd_speed_result {
 static void cmd_speed_parsed(void *parsed_result, void *show)
 {
        struct cmd_speed_result * res = parsed_result;
-       
+
        struct cs_block *csb;
 
        csb = cs_from_name(res->cs.csname);
@@ -191,10 +191,10 @@ static void cmd_speed_parsed(void *parsed_result, void *show)
        }
 
 #if notyet
-       if (!show) 
+       if (!show)
                ramp_set_vars(&csb->ramp, res->s, res->s); /* set speed */
 
-       printf_P(PSTR("%s %"PRIu32"\r\n"), 
+       printf_P(PSTR("%s %"PRIu32"\r\n"),
                 res->cs.csname,
                 ext.r_b.var_pos);
 #else
@@ -212,9 +212,9 @@ parse_pgm_inst_t cmd_speed = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_speed,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_speed_arg0, 
-               (prog_void *)&cmd_csb_name_tok, 
-               (prog_void *)&cmd_speed_s, 
+               (prog_void *)&cmd_speed_arg0,
+               (prog_void *)&cmd_csb_name_tok,
+               (prog_void *)&cmd_speed_s,
                NULL,
        },
 };
@@ -234,8 +234,8 @@ parse_pgm_inst_t cmd_speed_show = {
        .data = (void *)1,      /* 2nd arg of func */
        .help_str = help_speed_show,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_speed_arg0, 
-               (prog_void *)&cmd_csb_name_tok, 
+               (prog_void *)&cmd_speed_arg0,
+               (prog_void *)&cmd_csb_name_tok,
                (prog_void *)&cmd_speed_show_arg,
                NULL,
        },
@@ -262,10 +262,10 @@ static void cmd_derivate_filter_parsed(void *parsed_result, void *show)
                return;
        }
 
-       if (!show) 
+       if (!show)
                pid_set_derivate_filter(&csb->pid, res->size);
 
-       printf_P(PSTR("%s %s %u\r\n"), 
+       printf_P(PSTR("%s %s %u\r\n"),
                 res->cs.cmdname,
                 res->cs.csname,
                 pid_get_derivate_filter(&csb->pid));
@@ -281,9 +281,9 @@ parse_pgm_inst_t cmd_derivate_filter = {
        .data = (void *)1,      /* 2nd arg of func */
        .help_str = help_derivate_filter,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_derivate_filter_arg0, 
-               (prog_void *)&cmd_csb_name_tok, 
-               (prog_void *)&cmd_derivate_filter_size, 
+               (prog_void *)&cmd_derivate_filter_arg0,
+               (prog_void *)&cmd_csb_name_tok,
+               (prog_void *)&cmd_derivate_filter_size,
                NULL,
        },
 };
@@ -304,8 +304,8 @@ parse_pgm_inst_t cmd_derivate_filter_show = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_derivate_filter_show,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_derivate_filter_arg0, 
-               (prog_void *)&cmd_csb_name_tok, 
+               (prog_void *)&cmd_derivate_filter_arg0,
+               (prog_void *)&cmd_csb_name_tok,
                (prog_void *)&cmd_derivate_filter_show_arg,
                NULL,
        },
@@ -346,9 +346,9 @@ parse_pgm_inst_t cmd_consign = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_consign,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_consign_arg0, 
-               (prog_void *)&cmd_csb_name_tok, 
-               (prog_void *)&cmd_consign_p, 
+               (prog_void *)&cmd_consign_arg0,
+               (prog_void *)&cmd_csb_name_tok,
+               (prog_void *)&cmd_consign_p,
                NULL,
        },
 };
@@ -369,7 +369,7 @@ struct cmd_maximum_result {
 static void cmd_maximum_parsed(void *parsed_result, void *show)
 {
        struct cmd_maximum_result * res = parsed_result;
-       
+
        struct cs_block *csb;
 
        csb = cs_from_name(res->cs.csname);
@@ -381,7 +381,7 @@ static void cmd_maximum_parsed(void *parsed_result, void *show)
        if (!show)
                pid_set_maximums(&csb->pid, res->in, res->i, res->out);
 
-       printf_P(PSTR("maximum %s %"PRIu32" %"PRIu32" %"PRIu32"\r\n"), 
+       printf_P(PSTR("maximum %s %"PRIu32" %"PRIu32" %"PRIu32"\r\n"),
                 res->cs.csname,
                 pid_get_max_in(&csb->pid),
                 pid_get_max_I(&csb->pid),
@@ -400,11 +400,11 @@ parse_pgm_inst_t cmd_maximum = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_maximum,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_maximum_arg0, 
-               (prog_void *)&cmd_csb_name_tok, 
-               (prog_void *)&cmd_maximum_in, 
-               (prog_void *)&cmd_maximum_i, 
-               (prog_void *)&cmd_maximum_out, 
+               (prog_void *)&cmd_maximum_arg0,
+               (prog_void *)&cmd_csb_name_tok,
+               (prog_void *)&cmd_maximum_in,
+               (prog_void *)&cmd_maximum_i,
+               (prog_void *)&cmd_maximum_out,
                NULL,
        },
 };
@@ -425,8 +425,8 @@ parse_pgm_inst_t cmd_maximum_show = {
        .data = (void *)1,      /* 2nd arg of func */
        .help_str = help_maximum_show,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_maximum_arg0, 
-               (prog_void *)&cmd_csb_name_tok, 
+               (prog_void *)&cmd_maximum_arg0,
+               (prog_void *)&cmd_csb_name_tok,
                (prog_void *)&cmd_maximum_show_arg,
                NULL,
        },
@@ -438,17 +438,17 @@ parse_pgm_inst_t cmd_maximum_show = {
 /* this structure is filled when cmd_quadramp is parsed successfully */
 struct cmd_quadramp_result {
        struct cmd_cs_result cs;
-       uint32_t ap;
-       uint32_t an;
-       uint32_t sp;
-       uint32_t sn;
+       double ap;
+       double an;
+       double sp;
+       double sn;
 };
 
 /* function called when cmd_quadramp is parsed successfully */
 static void cmd_quadramp_parsed(void *parsed_result, void *show)
 {
        struct cmd_quadramp_result * res = parsed_result;
-       
+
        struct cs_block *csb;
 
        csb = cs_from_name(res->cs.csname);
@@ -462,7 +462,7 @@ static void cmd_quadramp_parsed(void *parsed_result, void *show)
                quadramp_set_2nd_order_vars(&csb->qr, res->ap, res->an);
        }
 
-       printf_P(PSTR("quadramp %s %"PRIi32" %"PRIi32" %"PRIi32" %"PRIi32"\r\n"), 
+       printf_P(PSTR("quadramp %s %2.2f %2.2f %2.2f %2.2f\r\n"),
                 res->cs.csname,
                 csb->qr.var_2nd_ord_pos,
                 csb->qr.var_2nd_ord_neg,
@@ -472,10 +472,10 @@ static void cmd_quadramp_parsed(void *parsed_result, void *show)
 
 prog_char str_quadramp_arg0[] = "quadramp";
 parse_pgm_token_string_t cmd_quadramp_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_quadramp_result, cs.cmdname, str_quadramp_arg0);
-parse_pgm_token_num_t cmd_quadramp_ap = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, ap, UINT32);
-parse_pgm_token_num_t cmd_quadramp_an = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, an, UINT32);
-parse_pgm_token_num_t cmd_quadramp_sp = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sp, UINT32);
-parse_pgm_token_num_t cmd_quadramp_sn = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sn, UINT32);
+parse_pgm_token_num_t cmd_quadramp_ap = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, ap, FLOAT);
+parse_pgm_token_num_t cmd_quadramp_an = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, an, FLOAT);
+parse_pgm_token_num_t cmd_quadramp_sp = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sp, FLOAT);
+parse_pgm_token_num_t cmd_quadramp_sn = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sn, FLOAT);
 
 prog_char help_quadramp[] = "Set quadramp values (acc+, acc-, speed+, speed-)";
 parse_pgm_inst_t cmd_quadramp = {
@@ -483,13 +483,13 @@ parse_pgm_inst_t cmd_quadramp = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_quadramp,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_quadramp_arg0, 
-               (prog_void *)&cmd_csb_name_tok, 
-               (prog_void *)&cmd_quadramp_ap, 
-               (prog_void *)&cmd_quadramp_an, 
-               (prog_void *)&cmd_quadramp_sp, 
-               (prog_void *)&cmd_quadramp_sn, 
-               
+               (prog_void *)&cmd_quadramp_arg0,
+               (prog_void *)&cmd_csb_name_tok,
+               (prog_void *)&cmd_quadramp_ap,
+               (prog_void *)&cmd_quadramp_an,
+               (prog_void *)&cmd_quadramp_sp,
+               (prog_void *)&cmd_quadramp_sn,
+
                NULL,
        },
 };
@@ -510,9 +510,9 @@ parse_pgm_inst_t cmd_quadramp_show = {
        .data = (void *)1,      /* 2nd arg of func */
        .help_str = help_quadramp_show,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_quadramp_arg0, 
-               (prog_void *)&cmd_csb_name_tok, 
-               (prog_void *)&cmd_quadramp_show_arg, 
+               (prog_void *)&cmd_quadramp_arg0,
+               (prog_void *)&cmd_csb_name_tok,
+               (prog_void *)&cmd_quadramp_show_arg,
                NULL,
        },
 };
@@ -535,7 +535,7 @@ static void cmd_cs_status_parsed(void *parsed_result, void *data)
        struct cs_block *csb;
        uint8_t loop = 0;
        uint8_t print_pid = 0, print_cs = 0;
-       
+
        csb = cs_from_name(res->cs.csname);
        if (csb == NULL) {
                printf_P(PSTR("null csb\r\n"));
@@ -587,9 +587,9 @@ parse_pgm_inst_t cmd_cs_status = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_cs_status,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_cs_status_arg0, 
-               (prog_void *)&cmd_csb_name_tok, 
-               (prog_void *)&cmd_cs_status_arg, 
+               (prog_void *)&cmd_cs_status_arg0,
+               (prog_void *)&cmd_csb_name_tok,
+               (prog_void *)&cmd_cs_status_arg,
                NULL,
        },
 };
@@ -611,7 +611,7 @@ struct cmd_blocking_i_result {
 static void cmd_blocking_i_parsed(void *parsed_result, void *show)
 {
        struct cmd_blocking_i_result * res = parsed_result;
-       
+
        struct cs_block *csb;
 
        csb = cs_from_name(res->cs.csname);
@@ -624,7 +624,7 @@ static void cmd_blocking_i_parsed(void *parsed_result, void *show)
                bd_set_current_thresholds(&csb->bd, res->k1, res->k2,
                                          res->i, res->cpt);
 
-       printf_P(PSTR("%s %s %"PRIi32" %"PRIi32" %"PRIi32" %d\r\n"), 
+       printf_P(PSTR("%s %s %"PRIi32" %"PRIi32" %"PRIi32" %d\r\n"),
                 res->cs.cmdname,
                 res->cs.csname,
                 csb->bd.k1,
@@ -646,11 +646,11 @@ parse_pgm_inst_t cmd_blocking_i = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_blocking_i,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_blocking_i_arg0, 
-               (prog_void *)&cmd_csb_name_tok, 
-               (prog_void *)&cmd_blocking_i_k1, 
-               (prog_void *)&cmd_blocking_i_k2, 
-               (prog_void *)&cmd_blocking_i_i, 
+               (prog_void *)&cmd_blocking_i_arg0,
+               (prog_void *)&cmd_csb_name_tok,
+               (prog_void *)&cmd_blocking_i_k1,
+               (prog_void *)&cmd_blocking_i_k2,
+               (prog_void *)&cmd_blocking_i_i,
                (prog_void *)&cmd_blocking_i_cpt,
                NULL,
        },
@@ -672,9 +672,9 @@ parse_pgm_inst_t cmd_blocking_i_show = {
        .data = (void *)1,      /* 2nd arg of func */
        .help_str = help_blocking_i_show,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_blocking_i_arg0, 
-               (prog_void *)&cmd_csb_name_tok, 
-               (prog_void *)&cmd_blocking_i_show_arg, 
+               (prog_void *)&cmd_blocking_i_arg0,
+               (prog_void *)&cmd_csb_name_tok,
+               (prog_void *)&cmd_blocking_i_show_arg,
                NULL,
        },
 };