#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"
/* 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";
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)
{
/* function called when cmd_test is parsed successfully */
static void cmd_test_parsed(void *parsed_result, void *data)
{
- trajectory_hardstop(&mainboard.traj);
}
prog_char str_test_arg0[] = "test";
git.droids-corp.org / aversive.git / blobdiff