X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fballboard%2Fcommands_cs.c;h=deea9be9668befd2f3fe0fa5cd6c23f55f8d01d6;hp=098ffb34f529eba54dedd78582082903b1ecc922;hb=ad466314d3aff8f661654d4701b9d12fdeb7811f;hpb=8d6a47e9e21a9a31f4bc12d32fb3d11091a4b305 diff --git a/projects/microb2010/ballboard/commands_cs.c b/projects/microb2010/ballboard/commands_cs.c index 098ffb3..deea9be 100644 --- a/projects/microb2010/ballboard/commands_cs.c +++ b/projects/microb2010/ballboard/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.1 2009-03-29 18:44:54 zer0 Exp $ * - * Olivier MATZ + * Olivier MATZ */ #include @@ -54,10 +54,12 @@ struct csb_list { prog_char csb_roller_str[] = "roller"; prog_char csb_forktrans_str[] = "forktrans"; prog_char csb_forkrot_str[] = "forkrot"; +prog_char csb_beacon_str[] = "beacon"; struct csb_list csb_list[] = { { .name = csb_roller_str, .csb = &ballboard.roller }, { .name = csb_forktrans_str, .csb = &ballboard.forktrans }, { .name = csb_forkrot_str, .csb = &ballboard.forkrot }, + { .name = csb_beacon_str, .csb = &ballboard.beacon }, }; struct cmd_cs_result { @@ -66,7 +68,7 @@ struct cmd_cs_result { }; /* token to be used for all cs-related commands */ -prog_char str_csb_name[] = "roller#forktrans#forkrot"; +prog_char str_csb_name[] = "roller#forktrans#forkrot#beacon"; 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) @@ -79,7 +81,7 @@ struct cs_block *cs_from_name(const char *name) } return NULL; } - + /**********************************************************/ /* Gains for control system */ @@ -103,7 +105,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"), @@ -126,11 +128,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, }, }; @@ -151,8 +153,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, }, @@ -171,7 +173,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); @@ -181,10 +183,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 %lu\r\n"), res->cs.csname, ext.r_b.var_pos); #else @@ -202,9 +204,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, }, }; @@ -224,8 +226,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, }, @@ -252,10 +254,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)); @@ -271,9 +273,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, }, }; @@ -294,8 +296,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, }, @@ -336,9 +338,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, }, }; @@ -359,7 +361,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); @@ -371,7 +373,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 %lu %lu %lu\r\n"), res->cs.csname, pid_get_max_in(&csb->pid), pid_get_max_I(&csb->pid), @@ -390,11 +392,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, }, }; @@ -415,8 +417,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, }, @@ -428,17 +430,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); @@ -452,7 +454,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, @@ -462,10 +464,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 = { @@ -473,13 +475,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, }, }; @@ -500,9 +502,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, }, }; @@ -525,7 +527,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")); @@ -577,9 +579,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, }, }; @@ -601,7 +603,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); @@ -614,7 +616,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 %ld %ld %ld %d\r\n"), res->cs.cmdname, res->cs.csname, csb->bd.k1, @@ -636,11 +638,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, }, @@ -662,9 +664,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, }, };