/*
* 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
*
* 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>
#include <aversive/wait.h>
#include <aversive/error.h>
+#include <hostsim.h>
#include <ax12.h>
#include <uart.h>
#include <pwm_ng.h>
-#include <time.h>
+#include <clock_time.h>
#include <pid.h>
#include <quadramp.h>
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 {
};
/* 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)
}
return NULL;
}
-
+
/**********************************************************/
/* Gains for control system */
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"),
.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,
},
};
.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,
},
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);
}
#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
.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,
},
};
.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,
},
/* this structure is filled when cmd_derivate_filter is parsed successfully */
struct cmd_derivate_filter_result {
struct cmd_cs_result cs;
+ fixed_string_t show;
uint8_t size;
};
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));
prog_char str_derivate_filter_arg0[] = "derivate_filter";
parse_pgm_token_string_t cmd_derivate_filter_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_derivate_filter_result, cs.cmdname, str_derivate_filter_arg0);
-parse_pgm_token_num_t cmd_derivate_filter_size = TOKEN_NUM_INITIALIZER(struct cmd_derivate_filter_result, size, UINT32);
+parse_pgm_token_num_t cmd_derivate_filter_size = TOKEN_NUM_INITIALIZER(struct cmd_derivate_filter_result, size, UINT8);
prog_char help_derivate_filter[] = "Set derivate_filter values for PID (in, I, out)";
parse_pgm_inst_t cmd_derivate_filter = {
.f = cmd_derivate_filter_parsed, /* function to call */
- .data = (void *)1, /* 2nd arg of func */
+ .data = NULL, /* 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,
},
};
/* show */
-struct cmd_derivate_filter_show_result {
- struct cmd_cs_result cs;
- fixed_string_t show;
-};
-
prog_char str_derivate_filter_show_arg[] = "show";
-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);
+parse_pgm_token_string_t cmd_derivate_filter_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_derivate_filter_result, show, str_derivate_filter_show_arg);
prog_char help_derivate_filter_show[] = "Show derivate_filter values for PID";
parse_pgm_inst_t cmd_derivate_filter_show = {
.f = cmd_derivate_filter_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
+ .data = (void *)1, /* 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,
},
.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,
},
};
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);
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),
.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,
},
};
.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,
},
/* 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);
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,
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 = {
.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,
},
};
.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,
},
};
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"));
.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,
},
};
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);
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,
.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,
},
.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,
},
};