X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fcommands_cs.c;h=c749c612cfb2b8267e95c07e81fd9d38ac15058a;hp=a39ab071939335af20f410df7bd0648070b77ee1;hb=fa8546ea39c7442ad3bf5a822a72a2b50a41045d;hpb=5918edd6f4f713ef3c8b0b0020dd30a4fb8222ae diff --git a/projects/microb2010/mainboard/commands_cs.c b/projects/microb2010/mainboard/commands_cs.c index a39ab07..c749c61 100644 --- 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 + * Olivier MATZ */ #include @@ -27,10 +27,11 @@ #include #include +#include #include #include #include -#include +#include #include #include @@ -60,9 +61,13 @@ struct csb_list { prog_char csb_angle_str[] = "angle"; prog_char csb_distance_str[] = "distance"; +prog_char csb_left_cobroller_str[] = "left_cobroller"; +prog_char csb_right_cobroller_str[] = "right_cobroller"; struct csb_list csb_list[] = { { .name = csb_angle_str, .csb = &mainboard.angle }, { .name = csb_distance_str, .csb = &mainboard.distance }, + { .name = csb_left_cobroller_str, .csb = &mainboard.left_cobroller }, + { .name = csb_right_cobroller_str, .csb = &mainboard.right_cobroller }, }; struct cmd_cs_result { @@ -71,7 +76,7 @@ struct cmd_cs_result { }; /* token to be used for all cs-related commands */ -prog_char str_csb_name[] = "angle#distance"; +prog_char str_csb_name[] = "angle#distance#left_cobroller#right_cobroller"; parse_pgm_token_string_t cmd_csb_name_tok = TOKEN_STRING_INITIALIZER(struct cmd_cs_result, csname, str_csb_name); struct cs_block *cs_from_name(const char *name) @@ -84,7 +89,7 @@ struct cs_block *cs_from_name(const char *name) } return NULL; } - + /**********************************************************/ /* Gains for control system */ @@ -108,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"), @@ -131,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, }, }; @@ -156,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, }, @@ -176,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); @@ -186,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 %lu\r\n"), + printf_P(PSTR("%s %"PRIu32"\r\n"), res->cs.csname, ext.r_b.var_pos); #else @@ -207,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, }, }; @@ -229,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, }, @@ -257,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)); @@ -276,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, }, }; @@ -299,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, }, @@ -341,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, }, }; @@ -364,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); @@ -376,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 %lu %lu %lu\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), @@ -395,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, }, }; @@ -420,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, }, @@ -433,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); @@ -457,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 %ld %ld %ld %ld\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, @@ -467,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 = { @@ -478,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, }, }; @@ -505,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, }, }; @@ -530,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")); @@ -582,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, }, }; @@ -606,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); @@ -619,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 %ld %ld %ld %d\r\n"), + printf_P(PSTR("%s %s %"PRIi32" %"PRIi32" %"PRIi32" %d\r\n"), res->cs.cmdname, res->cs.csname, csb->bd.k1, @@ -641,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, }, @@ -667,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, }, };