/*
* Copyright Droids Corporation (2009)
- *
+ *
* 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_mainboard.c,v 1.9 2009-11-08 17:24:33 zer0 Exp $
*
- * Olivier MATZ <zer0@droids-corp.org>
+ * Olivier MATZ <zer0@droids-corp.org>
*/
#include <stdio.h>
#include <quadramp.h>
#include <control_system_manager.h>
#include <trajectory_manager.h>
+#include <trajectory_manager_core.h>
#include <trajectory_manager_utils.h>
#include <vect_base.h>
#include <lines.h>
#include "strat.h"
#include "strat_utils.h"
#include "strat_base.h"
+#include "strat_db.h"
+#include "strat_corn.h"
#include "i2c_protocol.h"
#include "actuator.h"
u08 bit=0;
struct cmd_event_result * res = parsed_result;
-
+
if (!strcmp_P(res->arg1, PSTR("all"))) {
- bit = DO_ENCODERS | DO_CS | DO_RS | DO_POS |
- DO_BD | DO_TIMER | DO_POWER;
+ bit = 0xFF;
if (!strcmp_P(res->arg2, PSTR("on")))
mainboard.flags |= bit;
else if (!strcmp_P(res->arg2, PSTR("off")))
mainboard.flags &= bit;
else { /* show */
- printf_P(PSTR("encoders is %s\r\n"),
+ printf_P(PSTR("encoders is %s\r\n"),
(DO_ENCODERS & mainboard.flags) ? "on":"off");
- printf_P(PSTR("cs is %s\r\n"),
+ printf_P(PSTR("cs is %s\r\n"),
(DO_CS & mainboard.flags) ? "on":"off");
- printf_P(PSTR("rs is %s\r\n"),
+ printf_P(PSTR("rs is %s\r\n"),
(DO_RS & mainboard.flags) ? "on":"off");
- printf_P(PSTR("pos is %s\r\n"),
+ printf_P(PSTR("pos is %s\r\n"),
(DO_POS & mainboard.flags) ? "on":"off");
- printf_P(PSTR("bd is %s\r\n"),
+ printf_P(PSTR("bd is %s\r\n"),
(DO_BD & mainboard.flags) ? "on":"off");
- printf_P(PSTR("timer is %s\r\n"),
+ printf_P(PSTR("timer is %s\r\n"),
(DO_TIMER & mainboard.flags) ? "on":"off");
- printf_P(PSTR("power is %s\r\n"),
+ printf_P(PSTR("power is %s\r\n"),
(DO_POWER & mainboard.flags) ? "on":"off");
+ printf_P(PSTR("errblock is %s\r\n"),
+ (DO_ERRBLOCKING & mainboard.flags) ? "on":"off");
}
return;
}
}
else if (!strcmp_P(res->arg1, PSTR("power")))
bit = DO_POWER;
+ else if (!strcmp_P(res->arg1, PSTR("errblock")))
+ bit = DO_ERRBLOCKING;
if (!strcmp_P(res->arg2, PSTR("on")))
mainboard.flags |= bit;
}
mainboard.flags &= (~bit);
}
- printf_P(PSTR("%s is %s\r\n"), res->arg1,
+ printf_P(PSTR("%s is %s\r\n"), res->arg1,
(bit & mainboard.flags) ? "on":"off");
}
prog_char str_event_arg0[] = "event";
parse_pgm_token_string_t cmd_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg0, str_event_arg0);
-prog_char str_event_arg1[] = "all#encoders#cs#rs#pos#bd#timer#power";
+prog_char str_event_arg1[] = "all#encoders#cs#rs#pos#bd#timer#power#errblock";
parse_pgm_token_string_t cmd_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg1, str_event_arg1);
prog_char str_event_arg2[] = "on#off#show";
parse_pgm_token_string_t cmd_event_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg2, str_event_arg2);
.data = NULL, /* 2nd arg of func */
.help_str = help_event,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_event_arg0,
- (prog_void *)&cmd_event_arg1,
- (prog_void *)&cmd_event_arg2,
+ (prog_void *)&cmd_event_arg0,
+ (prog_void *)&cmd_event_arg1,
+ (prog_void *)&cmd_event_arg2,
NULL,
},
};
printf("not implemented\n");
#else
uint16_t i = 0, ret = 0, ret2 = 0;
-
+
if (mainboard.flags & DO_ENCODERS) {
printf_P(PSTR("Disable encoder event first\r\n"));
return;
.data = NULL, /* 2nd arg of func */
.help_str = help_spi_test,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_spi_test_arg0,
+ (prog_void *)&cmd_spi_test_arg0,
NULL,
},
};
.data = NULL, /* 2nd arg of func */
.help_str = help_opponent,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_opponent_arg0,
- (prog_void *)&cmd_opponent_arg1,
+ (prog_void *)&cmd_opponent_arg0,
+ (prog_void *)&cmd_opponent_arg1,
NULL,
},
};
.data = NULL, /* 2nd arg of func */
.help_str = help_opponent_set,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_opponent_arg0,
+ (prog_void *)&cmd_opponent_arg0,
(prog_void *)&cmd_opponent_arg1_set,
- (prog_void *)&cmd_opponent_arg2,
- (prog_void *)&cmd_opponent_arg3,
+ (prog_void *)&cmd_opponent_arg2,
+ (prog_void *)&cmd_opponent_arg3,
NULL,
},
};
/* function called when cmd_start is parsed successfully */
static void cmd_start_parsed(void *parsed_result, void *data)
{
-#ifdef HOST_VERSION
- printf("not implemented\n");
-#else
struct cmd_start_result *res = parsed_result;
uint8_t old_level = gen.log_level;
gen.logs[NB_LOGS] = E_USER_STRAT;
if (!strcmp_P(res->debug, PSTR("debug"))) {
- strat_infos.dump_enabled = 1;
+ strat_db.dump_enabled = 1;
gen.log_level = 5;
}
else {
- strat_infos.dump_enabled = 0;
+ strat_db.dump_enabled = 0;
gen.log_level = 0;
}
gen.logs[NB_LOGS] = 0;
gen.log_level = old_level;
-#endif
}
prog_char str_start_arg0[] = "start";
.data = NULL, /* 2nd arg of func */
.help_str = help_start,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_start_arg0,
- (prog_void *)&cmd_start_color,
- (prog_void *)&cmd_start_debug,
+ (prog_void *)&cmd_start_arg0,
+ (prog_void *)&cmd_start_color,
+ (prog_void *)&cmd_start_debug,
NULL,
},
};
static void print_cs(void)
{
printf_P(PSTR("cons_d=% .8"PRIi32" cons_a=% .8"PRIi32" fil_d=% .8"PRIi32" fil_a=% .8"PRIi32" "
- "err_d=% .8"PRIi32" err_a=% .8"PRIi32" out_d=% .8"PRIi32" out_a=% .8"PRIi32"\r\n"),
+ "err_d=% .8"PRIi32" err_a=% .8"PRIi32" out_d=% .8"PRIi32" out_a=% .8"PRIi32"\r\n"),
cs_get_consign(&mainboard.distance.cs),
cs_get_consign(&mainboard.angle.cs),
cs_get_filtered_consign(&mainboard.distance.cs),
static void print_pos(void)
{
- printf_P(PSTR("x=% .8d y=% .8d a=% .8d\r\n"),
+ printf_P(PSTR("x=% .8d y=% .8d a=% .8d\r\n"),
position_get_x_s16(&mainboard.pos),
position_get_y_s16(&mainboard.pos),
position_get_a_deg_s16(&mainboard.pos));
wait_ms(10);
continue;
}
-
+
if (cmd == -1) {
switch(c) {
case '1': print ^= PRINT_POS; break;
case '5': print ^= PRINT_TIME; break;
case '6': print ^= PRINT_BLOCKING; break;
- case 'q':
+ case 'q':
if (mainboard.flags & DO_CS)
strat_hardstop();
pwm_set_and_save(LEFT_PWM, 0);
pwm_set_and_save(LEFT_PWM, 0);
pwm_set_and_save(RIGHT_PWM, 0);
break;
- default:
+ default:
break;
}
}
else {
+#ifdef HOST_VERSION
+#define PWM_INTERACT 300
+#else
+#define PWM_INTERACT 1200
+#endif
switch(cmd) {
- case KEY_UP_ARR:
- pwm_set_and_save(LEFT_PWM, 1200);
- pwm_set_and_save(RIGHT_PWM, 1200);
+ case KEY_UP_ARR:
+ pwm_set_and_save(LEFT_PWM, PWM_INTERACT);
+ pwm_set_and_save(RIGHT_PWM, PWM_INTERACT);
break;
- case KEY_LEFT_ARR:
- pwm_set_and_save(LEFT_PWM, -1200);
- pwm_set_and_save(RIGHT_PWM, 1200);
+ case KEY_LEFT_ARR:
+ pwm_set_and_save(LEFT_PWM, -PWM_INTERACT);
+ pwm_set_and_save(RIGHT_PWM, PWM_INTERACT);
break;
- case KEY_DOWN_ARR:
- pwm_set_and_save(LEFT_PWM, -1200);
- pwm_set_and_save(RIGHT_PWM, -1200);
+ case KEY_DOWN_ARR:
+ pwm_set_and_save(LEFT_PWM, -PWM_INTERACT);
+ pwm_set_and_save(RIGHT_PWM, -PWM_INTERACT);
break;
case KEY_RIGHT_ARR:
- pwm_set_and_save(LEFT_PWM, 1200);
- pwm_set_and_save(RIGHT_PWM, -1200);
+ pwm_set_and_save(LEFT_PWM, PWM_INTERACT);
+ pwm_set_and_save(RIGHT_PWM, -PWM_INTERACT);
break;
}
}
.data = NULL, /* 2nd arg of func */
.help_str = help_interact,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_interact_arg0,
+ (prog_void *)&cmd_interact_arg0,
NULL,
},
};
.data = NULL, /* 2nd arg of func */
.help_str = help_color,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_color_arg0,
- (prog_void *)&cmd_color_color,
+ (prog_void *)&cmd_color_arg0,
+ (prog_void *)&cmd_color_color,
NULL,
},
};
{
// struct cmd_rs_result *res = parsed_result;
do {
- printf_P(PSTR("angle cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "),
+ printf_P(PSTR("angle cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "),
cs_get_consign(&mainboard.angle.cs),
cs_get_filtered_feedback(&mainboard.angle.cs),
cs_get_out(&mainboard.angle.cs));
- printf_P(PSTR("distance cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "),
+ printf_P(PSTR("distance cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "),
cs_get_consign(&mainboard.distance.cs),
cs_get_filtered_feedback(&mainboard.distance.cs),
cs_get_out(&mainboard.distance.cs));
.data = NULL, /* 2nd arg of func */
.help_str = help_rs,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_rs_arg0,
- (prog_void *)&cmd_rs_arg1,
+ (prog_void *)&cmd_rs_arg0,
+ (prog_void *)&cmd_rs_arg1,
NULL,
},
};
.data = NULL, /* 2nd arg of func */
.help_str = help_i2cdebug,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_i2cdebug_arg0,
+ (prog_void *)&cmd_i2cdebug_arg0,
NULL,
},
};
.data = NULL, /* 2nd arg of func */
.help_str = help_cobboard_show,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_cobboard_show_arg0,
- (prog_void *)&cmd_cobboard_show_arg1,
+ (prog_void *)&cmd_cobboard_show_arg0,
+ (prog_void *)&cmd_cobboard_show_arg1,
NULL,
},
};
/* function called when cmd_cobboard_setmode1 is parsed successfully */
static void cmd_cobboard_setmode1_parsed(void *parsed_result, void *data)
{
-#ifdef HOST_VERSION
- printf("not implemented\n");
-#else
struct cmd_cobboard_setmode1_result *res = parsed_result;
if (!strcmp_P(res->arg1, PSTR("init")))
- i2c_cobboard_mode_init();
+ i2c_cobboard_set_mode(I2C_COBBOARD_MODE_INIT);
else if (!strcmp_P(res->arg1, PSTR("eject")))
- i2c_cobboard_mode_eject();
-#endif
+ i2c_cobboard_set_mode(I2C_COBBOARD_MODE_EJECT);
}
prog_char str_cobboard_setmode1_arg0[] = "cobboard";
.data = NULL, /* 2nd arg of func */
.help_str = help_cobboard_setmode1,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_cobboard_setmode1_arg0,
- (prog_void *)&cmd_cobboard_setmode1_arg1,
+ (prog_void *)&cmd_cobboard_setmode1_arg0,
+ (prog_void *)&cmd_cobboard_setmode1_arg1,
NULL,
},
};
};
/* function called when cmd_cobboard_setmode2 is parsed successfully */
-static void cmd_cobboard_setmode2_parsed(void * parsed_result, void * data)
+static void cmd_cobboard_setmode2_parsed(void *parsed_result, void *data)
{
-#ifdef HOST_VERSION
- printf("not implemented\n");
-#else
struct cmd_cobboard_setmode2_result *res = parsed_result;
uint8_t side = I2C_LEFT_SIDE;
else if (!strcmp_P(res->arg2, PSTR("right")))
side = I2C_RIGHT_SIDE;
- if (!strcmp_P(res->arg1, PSTR("deploy")))
- i2c_cobboard_mode_deploy(side);
- else if (!strcmp_P(res->arg1, PSTR("harvest")))
- i2c_cobboard_mode_harvest(side);
- else if (!strcmp_P(res->arg1, PSTR("pack")))
- i2c_cobboard_mode_pack(side);
-#endif
+ if (!strcmp_P(res->arg1, PSTR("deploy"))) {
+ i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
+ i2c_cobboard_deploy(side);
+ }
+ else if (!strcmp_P(res->arg1, PSTR("harvest"))) {
+ i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
+ i2c_cobboard_autoharvest(side);
+ }
+ else if (!strcmp_P(res->arg1, PSTR("pack"))) {
+ i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
+ i2c_cobboard_pack(side);
+ }
+ else if (!strcmp_P(res->arg1, PSTR("deploy_nomove"))) {
+ i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
+ i2c_cobboard_deploy_nomove(side);
+ }
+ else if (!strcmp_P(res->arg1, PSTR("harvest_nomove"))) {
+ i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
+ i2c_cobboard_autoharvest_nomove(side);
+ }
}
prog_char str_cobboard_setmode2_arg0[] = "cobboard";
parse_pgm_token_string_t cmd_cobboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg0, str_cobboard_setmode2_arg0);
-prog_char str_cobboard_setmode2_arg1[] = "harvest#deploy#pack";
+prog_char str_cobboard_setmode2_arg1[] = "harvest#deploy#pack#harvest_nomove#deploy_nomove";
parse_pgm_token_string_t cmd_cobboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg1, str_cobboard_setmode2_arg1);
prog_char str_cobboard_setmode2_arg2[] = "left#right";
parse_pgm_token_string_t cmd_cobboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg2, str_cobboard_setmode2_arg2);
.data = NULL, /* 2nd arg of func */
.help_str = help_cobboard_setmode2,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_cobboard_setmode2_arg0,
- (prog_void *)&cmd_cobboard_setmode2_arg1,
- (prog_void *)&cmd_cobboard_setmode2_arg2,
+ (prog_void *)&cmd_cobboard_setmode2_arg0,
+ (prog_void *)&cmd_cobboard_setmode2_arg1,
+ (prog_void *)&cmd_cobboard_setmode2_arg2,
NULL,
},
};
/* function called when cmd_cobboard_setmode3 is parsed successfully */
static void cmd_cobboard_setmode3_parsed(void *parsed_result, void *data)
{
-#ifdef HOST_VERSION
- printf("not implemented\n");
-#else
struct cmd_cobboard_setmode3_result *res = parsed_result;
if (!strcmp_P(res->arg1, PSTR("xxx")))
printf("faux\r\n");
-#endif
}
prog_char str_cobboard_setmode3_arg0[] = "cobboard";
.data = NULL, /* 2nd arg of func */
.help_str = help_cobboard_setmode3,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_cobboard_setmode3_arg0,
- (prog_void *)&cmd_cobboard_setmode3_arg1,
- (prog_void *)&cmd_cobboard_setmode3_arg2,
+ (prog_void *)&cmd_cobboard_setmode3_arg0,
+ (prog_void *)&cmd_cobboard_setmode3_arg1,
+ (prog_void *)&cmd_cobboard_setmode3_arg2,
NULL,
},
};
printf_P(PSTR("mode = %x\r\n"), ballboard.mode);
printf_P(PSTR("status = %x\r\n"), ballboard.status);
printf_P(PSTR("ball_count = %d\r\n"), ballboard.ball_count);
+ printf_P(PSTR("lcob = %d\r\n"), ballboard.lcob);
+ printf_P(PSTR("rcob = %d\r\n"), ballboard.rcob);
#endif
}
.data = NULL, /* 2nd arg of func */
.help_str = help_ballboard_show,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_ballboard_show_arg0,
- (prog_void *)&cmd_ballboard_show_arg1,
+ (prog_void *)&cmd_ballboard_show_arg0,
+ (prog_void *)&cmd_ballboard_show_arg1,
NULL,
},
};
/* function called when cmd_ballboard_setmode1 is parsed successfully */
static void cmd_ballboard_setmode1_parsed(void *parsed_result, void *data)
{
-#ifdef HOST_VERSION
- printf("not implemented\n");
-#else
struct cmd_ballboard_setmode1_result *res = parsed_result;
if (!strcmp_P(res->arg1, PSTR("init")))
i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_HARVEST);
/* other commands */
-#endif
}
prog_char str_ballboard_setmode1_arg0[] = "ballboard";
.data = NULL, /* 2nd arg of func */
.help_str = help_ballboard_setmode1,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_ballboard_setmode1_arg0,
- (prog_void *)&cmd_ballboard_setmode1_arg1,
+ (prog_void *)&cmd_ballboard_setmode1_arg0,
+ (prog_void *)&cmd_ballboard_setmode1_arg1,
NULL,
},
};
/* function called when cmd_ballboard_setmode2 is parsed successfully */
static void cmd_ballboard_setmode2_parsed(void * parsed_result, void * data)
{
-#ifdef HOST_VERSION
- printf("not implemented\n");
-#else
struct cmd_ballboard_setmode2_result *res = parsed_result;
uint8_t mode = I2C_BALLBOARD_MODE_INIT;
mode = I2C_BALLBOARD_MODE_TAKE_R_FORK;
}
i2c_ballboard_set_mode(mode);
-#endif
}
prog_char str_ballboard_setmode2_arg0[] = "ballboard";
.data = NULL, /* 2nd arg of func */
.help_str = help_ballboard_setmode2,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_ballboard_setmode2_arg0,
- (prog_void *)&cmd_ballboard_setmode2_arg1,
- (prog_void *)&cmd_ballboard_setmode2_arg2,
+ (prog_void *)&cmd_ballboard_setmode2_arg0,
+ (prog_void *)&cmd_ballboard_setmode2_arg1,
+ (prog_void *)&cmd_ballboard_setmode2_arg2,
NULL,
},
};
/* function called when cmd_ballboard_setmode3 is parsed successfully */
static void cmd_ballboard_setmode3_parsed(void *parsed_result, void *data)
{
-#ifdef HOST_VERSION
- printf("not implemented\n");
-#else
struct cmd_ballboard_setmode3_result *res = parsed_result;
if (!strcmp_P(res->arg1, PSTR("xxx")))
printf("faux\r\n");
-#endif
}
prog_char str_ballboard_setmode3_arg0[] = "ballboard";
.data = NULL, /* 2nd arg of func */
.help_str = help_ballboard_setmode3,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_ballboard_setmode3_arg0,
- (prog_void *)&cmd_ballboard_setmode3_arg1,
- (prog_void *)&cmd_ballboard_setmode3_arg2,
+ (prog_void *)&cmd_ballboard_setmode3_arg0,
+ (prog_void *)&cmd_ballboard_setmode3_arg1,
+ (prog_void *)&cmd_ballboard_setmode3_arg2,
NULL,
},
};
.data = NULL, /* 2nd arg of func */
.help_str = help_servo_balls,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_servo_balls_arg0,
- (prog_void *)&cmd_servo_balls_arg1,
+ (prog_void *)&cmd_servo_balls_arg0,
+ (prog_void *)&cmd_servo_balls_arg1,
NULL,
},
};
float d_inter_mm;
};
-/**
- * do a superb curve joining line1 to line2 which is composed of:
- * - a clothoid starting from line1
- * - a circle
- * - another clothoid up to line2
- *
- * the function assumes that the initial linear speed is Vd and
- * angular speed is 0.
- *
- * - alpha: total angle
- * - beta: circular part of angle (lower than alpha)
- * - R: the radius of the circle (must be != 0)
- * - Vd: linear speed to use (in imp per cs period)
- * - Amax: maximum angular acceleration
- * - d_inter: distance in mm until the intersection of the
- * 2 lines
- *
- * return 0 on success: in this case these parameters are filled:
- * - Aa_out: the angular acceleration to configure in quadramp
- * - remain_d_mm_out: remaining distance before start to turn
- */
-uint8_t clitoid(double alpha_deg, double beta_deg, double R_mm,
- double Vd, double Amax, double d_inter_mm)
-{
- double Vd_mm_s;
- double Va, Va_rd_s;
- double t, d_mm, alpha_rad, beta_rad;
- double remain_d_mm;
- double Aa, Aa_rd_s2;
- line_t line1, line2;
- double x, y, a_rad;
- point_t robot, intersect, pt2, center, proj;
- vect_t v;
-
- /* param check */
- if (fabs(alpha_deg) <= fabs(beta_deg)) {
- DEBUG(E_USER_STRAT, "alpha is smaller than beta");
- return END_ERROR;
- }
-
- /* get angular speed Va */
- Vd_mm_s = Vd * (CS_HZ/DIST_IMP_MM);
- DEBUG(E_USER_STRAT, "Vd_mm_s=%2.2f", Vd_mm_s);
- Va_rd_s = Vd_mm_s / R_mm;
- Va = Va_rd_s * (DIST_IMP_MM * EXT_TRACK_MM / (2 * CS_HZ));
- DEBUG(E_USER_STRAT, "Va_rd_s=%2.2f Va=%2.2f", Va_rd_s, Va);
-
- /* process 't', the time in seconds that we will take to do
- * the first clothoid */
- alpha_rad = RAD(alpha_deg);
- beta_rad = RAD(beta_deg);
- t = fabs(((alpha_rad - beta_rad) * R_mm) / Vd_mm_s);
- DEBUG(E_USER_STRAT, "R_mm=%2.2f alpha_rad=%2.2f beta_rad=%2.2f t=%2.2f",
- R_mm, alpha_rad, beta_rad, t);
-
- /* process the angular acceleration */
- Aa_rd_s2 = Va_rd_s / t;
- Aa = Aa_rd_s2 * (DIST_IMP_MM * EXT_TRACK_MM /
- (2 * CS_HZ * CS_HZ));
- DEBUG(E_USER_STRAT, "Aa_rd_s2=%2.2f Aa=%2.2f", Aa_rd_s2, Aa);
-
- /* exit if the robot cannot physically do it */
- if (Aa > Amax) {
- DEBUG(E_USER_STRAT, "greater than max acceleration");
- return END_ERROR;
- }
-
- /* the robot position */
- x = position_get_x_double(&mainboard.pos);
- y = position_get_y_double(&mainboard.pos);
- a_rad = position_get_a_rad_double(&mainboard.pos);
-
- /* define line1 and line2 */
- robot.x = x;
- robot.y = y;
- intersect.x = x + cos(a_rad) * d_inter_mm;
- intersect.y = y + sin(a_rad) * d_inter_mm;
- pts2line(&robot, &intersect, &line1);
- pt2.x = intersect.x + cos(a_rad + alpha_rad);
- pt2.y = intersect.y + sin(a_rad + alpha_rad);
- pts2line(&intersect, &pt2, &line2);
- DEBUG(E_USER_STRAT, "intersect=(%2.2f, %2.2f)",
- intersect.x, intersect.y);
-
- /* the center of the circle is at (d_mm, d_mm) when we have to
- * start the clothoid */
- d_mm = R_mm * sqrt(fabs(alpha_rad - beta_rad)) *
- sqrt(M_PI) / 2.;
- DEBUG(E_USER_STRAT, "d_mm=%2.2f", d_mm);
-
- /* translate line1 */
- v.x = intersect.x - robot.x;
- v.y = intersect.y - robot.y;
- if (a_rad > 0)
- vect_rot_trigo(&v);
- else
- vect_rot_retro(&v);
- vect_resize(&v, d_mm);
- line_translate(&line1, &v);
-
- /* translate line2 */
- v.x = intersect.x - pt2.x;
- v.y = intersect.y - pt2.y;
- if (a_rad > 0)
- vect_rot_trigo(&v);
- else
- vect_rot_retro(&v);
- vect_resize(&v, d_mm);
- line_translate(&line2, &v);
-
- /* find the center of the circle, at the intersection of the
- * new translated lines */
- if (intersect_line(&line1, &line2, ¢er) != 1) {
- DEBUG(E_USER_STRAT, "cannot find circle center");
- return END_ERROR;
- }
- DEBUG(E_USER_STRAT, "center=(%2.2f,%2.2f)", center.x, center.y);
-
- /* project center of circle on line1 */
- proj_pt_line(¢er, &line1, &proj);
- DEBUG(E_USER_STRAT, "proj=(%2.2f,%2.2f)", proj.x, proj.y);
-
- /* process remaining distance before start turning */
- remain_d_mm = d_inter_mm - (pt_norm(&proj, &intersect) + d_mm);
- DEBUG(E_USER_STRAT, "remain_d=%2.2f", remain_d_mm);
- if (remain_d_mm < 0) {
- DEBUG(E_USER_STRAT, "too late, cannot turn");
- return END_ERROR;
- }
-
- return END_TRAJ;
-}
-
/* function called when cmd_test is parsed successfully */
static void cmd_clitoid_parsed(void *parsed_result, void *data)
{
struct cmd_clitoid_result *res = parsed_result;
- clitoid(res->alpha_deg, res->beta_deg, res->R_mm,
- res->Vd, res->Amax, res->d_inter_mm);
+/* clitoid(res->alpha_deg, res->beta_deg, res->R_mm, */
+/* res->Vd, res->Amax, res->d_inter_mm); */
+ double x = position_get_x_double(&mainboard.pos);
+ double y = position_get_y_double(&mainboard.pos);
+ double a = position_get_a_rad_double(&mainboard.pos);
+
+ strat_set_speed(res->Vd, SPEED_ANGLE_FAST);
+ trajectory_clitoid(&mainboard.traj, x, y, a, 150.,
+ res->alpha_deg, res->beta_deg, res->R_mm,
+ res->d_inter_mm);
}
prog_char str_clitoid_arg0[] = "clitoid";
},
};
-//////////////////////
+/**********************************************************/
+/* Time_Monitor */
-// 500 -- 5
-// 400 -- 3
-#define TEST_SPEED 400
-#define TEST_ACC 3
+/* this structure is filled when cmd_time_monitor is parsed successfully */
+struct cmd_time_monitor_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
-static void line2line(double line1x1, double line1y1,
- double line1x2, double line1y2,
- double line2x1, double line2y1,
- double line2x2, double line2y2,
- double radius, double dist)
+/* function called when cmd_time_monitor is parsed successfully */
+static void cmd_time_monitor_parsed(void *parsed_result, void *data)
{
- uint8_t err;
- double speed_d, speed_a;
- double distance, angle;
- double line1_angle = atan2(line1y2-line1y1, line1x2-line1x1);
- double line2_angle = atan2(line2y2-line2y1, line2x2-line2x1);
-
- printf_P(PSTR("%s()\r\n"), __FUNCTION__);
-
- strat_set_speed(TEST_SPEED, TEST_SPEED);
- quadramp_set_2nd_order_vars(&mainboard.angle.qr, TEST_ACC, TEST_ACC);
-
- circle_get_da_speed_from_radius(&mainboard.traj, radius,
- &speed_d, &speed_a);
- trajectory_line_abs(&mainboard.traj,
- line1x1, line1y1,
- line1x2, line1y2, 150.);
- err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line1x2, line1y2) <
- dist, TRAJ_FLAGS_NO_NEAR);
- /* circle */
- strat_set_speed(speed_d, speed_a);
- angle = line2_angle - line1_angle;
- distance = angle * radius;
- if (distance < 0)
- distance = -distance;
- angle = simple_modulo_2pi(angle);
- angle = DEG(angle);
- printf_P(PSTR("(%d,%d,%d) "),
- position_get_x_s16(&mainboard.pos),
- position_get_y_s16(&mainboard.pos),
- position_get_a_deg_s16(&mainboard.pos));
- printf_P(PSTR("circle distance=%2.2f angle=%2.2f\r\n"),
- distance, angle);
+#ifndef HOST_VERSION
+ struct cmd_time_monitor_result *res = parsed_result;
+ uint16_t seconds;
- /* take some margin on dist to avoid deceleration */
- trajectory_d_a_rel(&mainboard.traj, distance + 250, angle);
+ if (!strcmp_P(res->arg1, PSTR("reset"))) {
+ eeprom_write_word(EEPROM_TIME_ADDRESS, 0);
+ }
+ seconds = eeprom_read_word(EEPROM_TIME_ADDRESS);
+ printf_P(PSTR("Running since %d mn %d\r\n"), seconds/60, seconds%60);
+#endif
+}
- /* circle exit condition */
- err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
- TRAJ_FLAGS_NO_NEAR);
+prog_char str_time_monitor_arg0[] = "time_monitor";
+parse_pgm_token_string_t cmd_time_monitor_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_time_monitor_result, arg0, str_time_monitor_arg0);
+prog_char str_time_monitor_arg1[] = "show#reset";
+parse_pgm_token_string_t cmd_time_monitor_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_time_monitor_result, arg1, str_time_monitor_arg1);
- strat_set_speed(500, 500);
- printf_P(PSTR("(%d,%d,%d) "),
- position_get_x_s16(&mainboard.pos),
- position_get_y_s16(&mainboard.pos),
- position_get_a_deg_s16(&mainboard.pos));
- printf_P(PSTR("line\r\n"));
- trajectory_line_abs(&mainboard.traj,
- line2x1, line2y1,
- line2x2, line2y2, 150.);
+prog_char help_time_monitor[] = "Show since how long we are running";
+parse_pgm_inst_t cmd_time_monitor = {
+ .f = cmd_time_monitor_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_time_monitor,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_time_monitor_arg0,
+ (prog_void *)&cmd_time_monitor_arg1,
+ NULL,
+ },
+};
+
+
+/**********************************************************/
+/* Strat_Event */
+
+/* this structure is filled when cmd_strat_event is parsed successfully */
+struct cmd_strat_event_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
+
+/* function called when cmd_strat_event is parsed successfully */
+static void cmd_strat_event_parsed(void *parsed_result, void *data)
+{
+ struct cmd_strat_event_result *res = parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("on")))
+ strat_event_enable();
+ else
+ strat_event_disable();
}
-static void halfturn(double line1x1, double line1y1,
- double line1x2, double line1y2,
- double line2x1, double line2y1,
- double line2x2, double line2y2,
- double radius, double dist, double dir)
+prog_char str_strat_event_arg0[] = "strat_event";
+parse_pgm_token_string_t cmd_strat_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_event_result, arg0, str_strat_event_arg0);
+prog_char str_strat_event_arg1[] = "on#off";
+parse_pgm_token_string_t cmd_strat_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_event_result, arg1, str_strat_event_arg1);
+
+prog_char help_strat_event[] = "Enable/disable strat_event callback";
+parse_pgm_inst_t cmd_strat_event = {
+ .f = cmd_strat_event_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_strat_event,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_strat_event_arg0,
+ (prog_void *)&cmd_strat_event_arg1,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Sleep */
+
+/* this structure is filled when cmd_sleep is parsed successfully */
+struct cmd_sleep_result {
+ fixed_string_t arg0;
+ uint32_t ms;
+};
+
+/* function called when cmd_sleep is parsed successfully */
+static void cmd_sleep_parsed(void *parsed_result, void *data)
{
- uint8_t err;
- double speed_d, speed_a;
- double distance, angle;
-
- printf_P(PSTR("%s()\r\n"), __FUNCTION__);
-
- strat_set_speed(TEST_SPEED, TEST_SPEED);
- quadramp_set_2nd_order_vars(&mainboard.angle.qr, TEST_ACC, TEST_ACC);
-
- circle_get_da_speed_from_radius(&mainboard.traj, radius,
- &speed_d, &speed_a);
- trajectory_line_abs(&mainboard.traj,
- line1x1, line1y1,
- line1x2, line1y2, 150.);
- err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line1x2, line1y2) <
- dist, TRAJ_FLAGS_NO_NEAR);
- /* circle */
- strat_set_speed(speed_d, speed_a);
- angle = dir * M_PI/2.;
- distance = angle * radius;
- if (distance < 0)
- distance = -distance;
- angle = simple_modulo_2pi(angle);
- angle = DEG(angle);
-
- /* take some margin on dist to avoid deceleration */
- DEBUG(E_USER_STRAT, "circle1 distance=%2.2f angle=%2.2f",
- distance, angle);
- trajectory_d_a_rel(&mainboard.traj, distance + 500, angle);
-
- /* circle exit condition */
- err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
- TRAJ_FLAGS_NO_NEAR);
-
- DEBUG(E_USER_STRAT, "miniline");
- err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line2x1, line2y1) <
- dist, TRAJ_FLAGS_NO_NEAR);
- DEBUG(E_USER_STRAT, "circle2");
- /* take some margin on dist to avoid deceleration */
- trajectory_d_a_rel(&mainboard.traj, distance + 500, angle);
-
- err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
- TRAJ_FLAGS_NO_NEAR);
-
- strat_set_speed(500, 500);
- DEBUG(E_USER_STRAT, "line");
- trajectory_line_abs(&mainboard.traj,
- line2x1, line2y1,
- line2x2, line2y2, 150.);
+ struct cmd_sleep_result *res = parsed_result;
+ time_wait_ms(res->ms);
}
+prog_char str_sleep_arg0[] = "sleep";
+parse_pgm_token_string_t cmd_sleep_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_sleep_result, arg0, str_sleep_arg0);
+parse_pgm_token_num_t cmd_sleep_ms = TOKEN_NUM_INITIALIZER(struct cmd_sleep_result, ms, UINT32);
+
+prog_char help_sleep[] = "Sleep during some miliseconds";
+parse_pgm_inst_t cmd_sleep = {
+ .f = cmd_sleep_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_sleep,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_sleep_arg0,
+ (prog_void *)&cmd_sleep_ms,
+ NULL,
+ },
+};
+
+
/**********************************************************/
/* Test */
/* function called when cmd_test is parsed successfully */
static void cmd_test_parsed(void *parsed_result, void *data)
{
- // struct cmd_test_result *res = parsed_result;
-#ifdef HOST_VERSION
- strat_reset_pos(400, 400, 90);
- mainboard.angle.on = 1;
- mainboard.distance.on = 1;
-#endif
- printf_P(PSTR("%s()\r\n"), __FUNCTION__);
- while (!cmdline_keypressed()) {
- /****** PASS1 */
-
-#define DIST_HARD_TURN 260
-#define RADIUS_HARD_TURN 100
-#define DIST_EASY_TURN 190
-#define RADIUS_EASY_TURN 190
-#define DIST_HALF_TURN 225
-#define RADIUS_HALF_TURN 130
-
- /* hard turn */
- line2line(375, 597, 375, 1847,
- 375, 1847, 1050, 1472,
- RADIUS_HARD_TURN, DIST_HARD_TURN);
-
- /* easy left and easy right !*/
- line2line(825, 1596, 1050, 1472,
- 1050, 1472, 1500, 1722,
- RADIUS_EASY_TURN, DIST_EASY_TURN);
- line2line(1050, 1472, 1500, 1722,
- 1500, 1722, 2175, 1347,
- RADIUS_EASY_TURN, DIST_EASY_TURN);
- line2line(1500, 1722, 2175, 1347,
- 2175, 1347, 2175, 847,
- RADIUS_EASY_TURN, DIST_EASY_TURN);
-
- /* half turns */
- halfturn(2175, 1347, 2175, 722,
- 2625, 722, 2625, 1597,
- RADIUS_HALF_TURN, DIST_HALF_TURN, 1.);
- halfturn(2625, 847, 2625, 1722,
- 2175, 1722, 2175, 1097,
- RADIUS_HALF_TURN, DIST_HALF_TURN, 1.);
-
- /* easy turns */
- line2line(2175, 1597, 2175, 1097,
- 2175, 1097, 1500, 722,
- RADIUS_EASY_TURN, DIST_EASY_TURN);
- line2line(2175, 1097, 1500, 722,
- 1500, 722, 1050, 972,
- RADIUS_EASY_TURN, DIST_EASY_TURN);
- line2line(1500, 722, 1050, 972,
- 1050, 972, 375, 597,
- RADIUS_EASY_TURN, DIST_EASY_TURN);
-
- /* hard turn */
- line2line(1050, 972, 375, 597,
- 375, 597, 375, 1097,
- RADIUS_HARD_TURN, DIST_HARD_TURN);
-
- /****** PASS2 */
-
- /* easy turn */
- line2line(375, 597, 375, 1097,
- 375, 1097, 1050, 1472,
- RADIUS_EASY_TURN, DIST_EASY_TURN);
-
- /* hard turn */
- line2line(375, 1097, 1050, 1472,
- 1050, 1472, 375, 1847,
- RADIUS_HARD_TURN, DIST_HARD_TURN);
-
- /* hard turn */
- line2line(1050, 1472, 375, 1847,
- 375, 1847, 375, 1347,
- RADIUS_HARD_TURN, DIST_HARD_TURN);
-
- /* easy turn */
- line2line(375, 1847, 375, 1347,
- 375, 1347, 1050, 972,
- RADIUS_EASY_TURN, DIST_EASY_TURN);
-
- /* hard turn */
- line2line(375, 1347, 1050, 972,
- 1050, 972, 375, 597,
- RADIUS_HARD_TURN, DIST_HARD_TURN);
-
- /* hard turn */
- line2line(1050, 972, 375, 597,
- 375, 597, 375, 1847,
- RADIUS_HARD_TURN, DIST_HARD_TURN);
-
- }
- trajectory_hardstop(&mainboard.traj);
+ strat_db.dump_enabled = 1;
+ strat_db_dump(__FUNCTION__);
+
+ corn_set_color(strat_db.corn_table[0], I2C_COB_BLACK);
+ strat_db_dump(__FUNCTION__);
+
+ corn_set_color(strat_db.corn_table[3], I2C_COB_WHITE);
+ strat_db_dump(__FUNCTION__);
+ corn_set_color(strat_db.corn_table[4], I2C_COB_WHITE);
+ strat_db_dump(__FUNCTION__);
+ corn_set_color(strat_db.corn_table[5], I2C_COB_WHITE);
+ strat_db_dump(__FUNCTION__);
}
prog_char str_test_arg0[] = "test";
NULL,
},
};
+
+
+
+
+/**********************************************************/
+/* Climb */
+
+/* this structure is filled when cmd_climb is parsed successfully */
+struct cmd_climb_result {
+ fixed_string_t arg0;
+ int32_t radius;
+ int32_t dist;
+};
+
+
+//#define DIM_BALLS_DOWN 620
+//#define DIM_BALLS_UP 700
+
+#define DIM_BALLS_DOWN 530
+#define DIM_BALLS_UP 700
+
+/* function called when cmd_climb is parsed successfully */
+static void cmd_climb_parsed(void *parsed_result, void *data)
+{
+ uint8_t err;
+
+ printf_P(PSTR("starting Clifenger\r\n"));
+ strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW );
+
+ trajectory_goto_xy_abs(&mainboard.traj, 250, 250);
+ err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ trajectory_a_abs(&mainboard.traj, 0);
+ err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+
+
+
+ trajectory_goto_xy_abs(&mainboard.traj, 1250, 250);
+ err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos)>DIM_BALLS_DOWN, 0xFF);
+ printf_P(PSTR("ball_pack\r\n"));
+ support_balls_pack();
+ err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos)>DIM_BALLS_UP, 0xFF);
+ printf_P(PSTR("ball_deploy\r\n"));
+ support_balls_deploy();
+ err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+
+ /* reach top, go down */
+ trajectory_goto_xy_abs(&mainboard.traj, 250, 250);
+ err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos)<DIM_BALLS_UP, 0xFF);
+ printf_P(PSTR("ball_pack\r\n"));
+ support_balls_pack();
+ err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos)<DIM_BALLS_DOWN, 0xFF);
+ printf_P(PSTR("ball_deploy\r\n"));
+ support_balls_deploy();
+
+ err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+}
+
+prog_char str_climb_arg0[] = "climb";
+parse_pgm_token_string_t cmd_climb_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_climb_result, arg0, str_climb_arg0);
+parse_pgm_token_num_t cmd_climb_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_climb_result, radius, INT32);
+parse_pgm_token_num_t cmd_climb_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_climb_result, dist, INT32);
+
+prog_char help_climb[] = "Climb function";
+parse_pgm_inst_t cmd_climb = {
+ .f = cmd_climb_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_climb,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_climb_arg0,
+ NULL,
+ },
+};
git.droids-corp.org / aversive.git / blobdiff