-/*
+/*
* Copyright Droids Corporation (2007)
- *
+ *
* 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
}
/* thresholds */
-void bd_set_current_thresholds(struct blocking_detection * bd,
- int32_t k1, int32_t k2,
+void bd_set_current_thresholds(struct blocking_detection * bd,
+ int32_t k1, int32_t k2,
uint32_t i_thres, uint16_t cpt_thres)
{
uint8_t flags;
}
/* speed threshold */
-void bd_set_speed_threshold(struct blocking_detection * bd,
+void bd_set_speed_threshold(struct blocking_detection * bd,
uint16_t speed)
{
uint8_t flags;
IRQ_UNLOCK(flags);
}
-
-
/** function to be called periodically */
-void bd_manage_from_speed_cmd(struct blocking_detection * bd,
- int32_t speed, int32_t cmd)
+void bd_manage_from_speed_cmd(struct blocking_detection * bd,
+ int32_t speed, int32_t cmd)
{
- int32_t i=0;
+ int32_t i = 0;
+
+ /* disabled */
+ if (bd->cpt_thres == 0)
+ return;
- /* if current-based blocking_detection enabled */
- if ( bd->cpt_thres ) {
- i = bd->k1 * cmd - bd->k2 * speed;
- if ((uint32_t)ABS(i) > bd->i_thres &&
- (bd->speed_thres == 0 || ABS(speed) < bd->speed_thres)) {
- if (bd->cpt == bd->cpt_thres - 1)
- WARNING(E_BLOCKING_DETECTION_MANAGER,
- "BLOCKING cmd=%ld, speed=%ld i=%ld",
- cmd, speed, i);
- if (bd->cpt < bd->cpt_thres)
- bd->cpt++;
- }
- else {
- bd->cpt=0;
- }
+ i = bd->k1 * cmd - bd->k2 * speed;
+
+ /* if i is above threshold, speed is below threshold, and cmd
+ * has the same sign than i */
+ if ((uint32_t)ABS(i) > bd->i_thres &&
+ (bd->speed_thres == 0 || ABS(speed) < bd->speed_thres) &&
+ (i * cmd > 0)) {
+ if (bd->cpt == bd->cpt_thres - 1)
+ WARNING(E_BLOCKING_DETECTION_MANAGER,
+ "BLOCKING cmd=%ld, speed=%ld i=%ld",
+ cmd, speed, i);
+ if (bd->cpt < bd->cpt_thres)
+ bd->cpt++;
+ }
+ else {
+ bd->cpt=0;
+ }
#if BD_DEBUG
- if (bd->debug_cpt++ == BD_DEBUG) {
- DEBUG(E_BLOCKING_DETECTION_MANAGER, "cmd=%ld, speed=%ld i=%ld",
- cmd, speed, i);
- bd->debug_cpt = 0;
- }
+ if (bd->debug_cpt++ == BD_DEBUG) {
+ DEBUG(E_BLOCKING_DETECTION_MANAGER, "cmd=%ld, speed=%ld i=%ld",
+ cmd, speed, i);
+ bd->debug_cpt = 0;
}
#endif
}
/** function to be called periodically */
-void bd_manage_from_pos_cmd(struct blocking_detection * bd,
- int32_t pos, int32_t cmd)
+void bd_manage_from_pos_cmd(struct blocking_detection * bd,
+ int32_t pos, int32_t cmd)
{
int32_t speed = (pos - bd->prev_pos);
- bd_manage_from_speed_cmd(bd, speed, cmd);
+ bd_manage_from_speed_cmd(bd, speed, cmd);
bd->prev_pos = pos;
}
void trajectory_set_speed(struct trajectory *traj, double d_speed, double a_speed);
/** set speed consign */
-void trajectory_set_speed(struct trajectory *traj, double d_acc, double a_acc);
+void trajectory_set_acc(struct trajectory *traj, double d_acc, double a_acc);
/**
* set windows for trajectory.
}
/* position consign is infinite */
- d_consign = (int32_t)(v2pol_target.r * (traj->position->phys.distance_imp_per_mm));
+ d_consign = pos_mm2imp(traj, v2pol_target.r);
d_consign += rs_get_distance(traj->robot);
- /* angle consign */
- a_consign = (int32_t)(v2pol_target.theta *
- (traj->position->phys.distance_imp_per_mm) *
- (traj->position->phys.track_mm) / 2.2);
+ /* angle consign (1.1 to avoid oscillations) */
+ a_consign = pos_rd2imp(traj, v2pol_target.theta) / 1.1;
a_consign += rs_get_angle(traj->robot);
EVT_DEBUG(E_TRAJECTORY, "target.x=%2.2f target.y=%2.2f "
* - Va_out: the angular speed to configure in quadramp
* - remain_d_mm_out: remaining distance before start to turn
*/
-static uint8_t calc_clitoid(struct trajectory *traj,
+static int8_t calc_clitoid(struct trajectory *traj,
double x, double y, double a_rad,
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 t, tau, d_mm, alpha_rad, beta_rad;
double remain_d_mm;
double Aa, Aa_rd_s2;
line_t line1, line2;
- point_t robot, intersect, pt2, center, proj;
+ line_t line1_int, line2_int;
+ point_t robot, intersect, pt2, center, proj, M;
vect_t v;
+ double xm, ym, L, A;
/* param check */
if (fabs(alpha_deg) <= fabs(beta_deg)) {
return -1;
}
- /* 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;
DEBUG(E_TRAJECTORY, "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.;
+ /* L and A are the parameters of the clothoid, xm and ym are
+ * the relative coords (starting from the beginning of
+ * clothoid) of the crossing point between the clothoid and
+ * the circle. */
+ L = Vd_mm_s * t;
+ A = R_mm * sqrt(fabs(alpha_rad - beta_rad));
+ xm =
+ L
+ - (pow(L, 5) / (40. * pow(A, 4)))
+ + (pow(L, 9) / (3456. * pow(A, 8)))
+ - (pow(L, 13) / (599040. * pow(A, 12)));
+ ym =
+ (pow(L, 3) / (6. * pow(A, 2)))
+ - (pow(L, 7) / (336. * pow(A, 6)))
+ + (pow(L, 11) / (42240. * pow(A, 10)))
+ - (pow(L, 15) / (9676800. * pow(A, 14)));
+ DEBUG(E_TRAJECTORY, "relative xm,ym = (%2.2f, %2.2f)",
+ xm, ym);
+
+ /* the center of the circle is at d_mm when we have to start
+ * the clothoid */
+ tau = (alpha_rad - beta_rad) / 2.;
+ d_mm = ym + (R_mm * cos(tau));
DEBUG(E_TRAJECTORY, "d_mm=%2.2f", d_mm);
/* translate line1 */
+ memcpy(&line1_int, &line1, sizeof(line1_int));
+ memcpy(&line2_int, &line2, sizeof(line2_int));
v.x = intersect.x - robot.x;
v.y = intersect.y - robot.y;
- if (a_rad > 0)
+ if (alpha_rad > 0)
vect_rot_trigo(&v);
else
vect_rot_retro(&v);
vect_resize(&v, d_mm);
- line_translate(&line1, &v);
+ line_translate(&line1_int, &v);
+ DEBUG(E_TRAJECTORY, "translate line1 by %2.2f,%2.2f", v.x, v.y);
- /* translate line2 */
+ /* translate line2_int */
v.x = intersect.x - pt2.x;
v.y = intersect.y - pt2.y;
- if (a_rad > 0)
+ if (alpha_rad < 0)
vect_rot_trigo(&v);
else
vect_rot_retro(&v);
vect_resize(&v, d_mm);
- line_translate(&line2, &v);
+ line_translate(&line2_int, &v);
+ DEBUG(E_TRAJECTORY, "translate line2 by %2.2f,%2.2f", v.x, v.y);
/* find the center of the circle, at the intersection of the
* new translated lines */
- if (intersect_line(&line1, &line2, ¢er) != 1) {
+ if (intersect_line(&line1_int, &line2_int, ¢er) != 1) {
DEBUG(E_TRAJECTORY, "cannot find circle center");
return -1;
}
DEBUG(E_TRAJECTORY, "center=(%2.2f,%2.2f)", center.x, center.y);
- /* project center of circle on line1 */
- proj_pt_line(¢er, &line1, &proj);
- DEBUG(E_TRAJECTORY, "proj=(%2.2f,%2.2f)", proj.x, proj.y);
+ /* M is the same point than xm, ym but in absolute coords */
+ M.x = center.x + cos(a_rad - M_PI/2 + tau) * R_mm;
+ M.y = center.y + sin(a_rad - M_PI/2 + tau) * R_mm;
+ DEBUG(E_TRAJECTORY, "absolute M = (%2.2f, %2.2f)", M.x, M.y);
+
+ /* project M on line 1 */
+ proj_pt_line(&M, &line1, &proj);
+ DEBUG(E_TRAJECTORY, "proj M = (%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);
+ remain_d_mm = d_inter_mm - (pt_norm(&proj, &intersect) + xm);
DEBUG(E_TRAJECTORY, "remain_d=%2.2f", remain_d_mm);
if (remain_d_mm < 0) {
DEBUG(E_TRAJECTORY, "too late, cannot turn");
return 0;
}
+/* after the line, start the clothoid */
static void start_clitoid(struct trajectory *traj)
{
double Aa = traj->target.line.Aa;
double d;
delete_event(traj);
+ DEBUG(E_TRAJECTORY, "%s() Va=%2.2f Aa=%2.2f",
+ __FUNCTION__, Va, Aa);
traj->state = RUNNING_CLITOID_CURVE;
- set_quadramp_acc(traj, Aa, Aa);
- set_quadramp_speed(traj, Va, Va);
- d = R_mm * a_rad;
- d *= 2.; /* margin to avoid deceleration */
+ d = fabs(R_mm * a_rad);
+ d *= 3.; /* margin to avoid deceleration */
trajectory_d_a_rel(traj, d, DEG(a_rad));
+ set_quadramp_acc(traj, traj->d_acc, Aa);
+ set_quadramp_speed(traj, traj->d_speed, Va);
}
int8_t trajectory_clitoid(struct trajectory *traj,
double x, double y, double a, double advance,
double alpha_deg, double beta_deg, double R_mm,
- double Vd, double d_inter_mm)
+ double d_inter_mm)
{
- double remain = 0, Aa = 0, Va = 0;
+ double remain = 0, Aa = 0, Va = 0, Vd;
double turnx, turny;
+ Vd = traj->d_speed;
if (calc_clitoid(traj, x, y, a, alpha_deg, beta_deg, R_mm,
Vd, traj->a_acc, d_inter_mm,
&Aa, &Va, &remain) < 0)
traj->target.line.R = R_mm;
traj->target.line.turn_pt.x = turnx;
traj->target.line.turn_pt.y = turny;
+ DEBUG(E_TRAJECTORY, "%s() turn_pt=%2.2f,%2.2f",
+ __FUNCTION__, turnx, turny);
+
__trajectory_line_abs(traj, x, y, turnx, turny,
advance);
traj->state = RUNNING_CLITOID_LINE;
/*********** *CIRCLE */
/* XXX TODO */
+
+/*********** CLITOID */
+
+/**
+ * do a superb curve joining line1 to line2 which is composed of:
+ * - a clothoid starting from line1
+ * - a circle
+ * - another clothoid up to line2
+ * this curve is called a clitoid (hehe)
+ *
+ * the function assumes that the initial linear speed is Vd and
+ * angular speed is 0.
+ *
+ * - x,y,a: starting position
+ * - advance: parameter for line following
+ * - 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 if trajectory can be loaded, then it is processed in
+ * background.
+ */
+int8_t trajectory_clitoid(struct trajectory *traj,
+ double x, double y, double a, double advance,
+ double alpha_deg, double beta_deg, double R_mm,
+ double d_inter_mm);
#include "main.h"
-#define ROLLER_ON 800
+#define ROLLER_ON -1200
#define ROLLER_OFF 0
-#define ROLLER_REVERSE -800
+#define ROLLER_REVERSE 1200
-#define FORKROT_DEPLOYED 1000
+#define FORKROT_DEPLOYED -50000
#define FORKROT_PACKED 0
#define FORKTRANS_LEFT 0
return (uart_recv_nowait(CMDLINE_UART) != -1);
}
-static inline uint8_t cmdline_getchar(void) {
+static inline int16_t cmdline_getchar(void) {
return uart_recv_nowait(CMDLINE_UART);
}
/*
* 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_ballboard.c,v 1.2 2009-04-24 19:30:42 zer0 Exp $
*
- * Olivier MATZ <zer0@droids-corp.org>
+ * Olivier MATZ <zer0@droids-corp.org>
*/
#include <stdio.h>
u08 bit=0;
struct cmd_event_result * res = parsed_result;
-
+
if (!strcmp_P(res->arg1, PSTR("all"))) {
- bit = DO_ENCODERS | DO_CS | DO_BD | DO_POWER;
+ bit = 0xFF;
if (!strcmp_P(res->arg2, PSTR("on")))
ballboard.flags |= bit;
else if (!strcmp_P(res->arg2, PSTR("off")))
ballboard.flags &= bit;
else { /* show */
- printf_P(PSTR("encoders is %s\r\n"),
+ printf_P(PSTR("encoders is %s\r\n"),
(DO_ENCODERS & ballboard.flags) ? "on":"off");
- printf_P(PSTR("cs is %s\r\n"),
+ printf_P(PSTR("cs is %s\r\n"),
(DO_CS & ballboard.flags) ? "on":"off");
- printf_P(PSTR("bd is %s\r\n"),
+ printf_P(PSTR("bd is %s\r\n"),
(DO_BD & ballboard.flags) ? "on":"off");
- printf_P(PSTR("power is %s\r\n"),
+ printf_P(PSTR("power is %s\r\n"),
(DO_POWER & ballboard.flags) ? "on":"off");
+ printf_P(PSTR("errblock is %s\r\n"),
+ (DO_ERRBLOCKING & ballboard.flags) ? "on":"off");
}
return;
}
bit = DO_BD;
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")))
}
ballboard.flags &= (~bit);
}
- printf_P(PSTR("%s is %s\r\n"), res->arg1,
+ printf_P(PSTR("%s is %s\r\n"), res->arg1,
(bit & ballboard.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#bd#power";
+prog_char str_event_arg1[] = "all#encoders#cs#bd#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,
},
};
.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,
},
};
};
/* function called when cmd_state3 is parsed successfully */
-static void cmd_state3_parsed(void *parsed_result,
+static void cmd_state3_parsed(void *parsed_result,
__attribute__((unused)) void *data)
{
struct cmd_state3_result *res = parsed_result;
.data = NULL, /* 2nd arg of func */
.help_str = help_state3,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_state3_arg0,
- (prog_void *)&cmd_state3_arg1,
- (prog_void *)&cmd_state3_arg2,
+ (prog_void *)&cmd_state3_arg0,
+ (prog_void *)&cmd_state3_arg1,
+ (prog_void *)&cmd_state3_arg2,
NULL,
},
};
.data = NULL, /* 2nd arg of func */
.help_str = help_state_machine,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_state_machine_arg0,
+ (prog_void *)&cmd_state_machine_arg0,
NULL,
},
};
.data = NULL, /* 2nd arg of func */
.help_str = help_state_debug,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_state_debug_arg0,
- (prog_void *)&cmd_state_debug_on,
+ (prog_void *)&cmd_state_debug_arg0,
+ (prog_void *)&cmd_state_debug_on,
NULL,
},
};
.data = NULL, /* 2nd arg of func */
.help_str = help_fork,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_fork_arg0,
- (prog_void *)&cmd_fork_arg1,
+ (prog_void *)&cmd_fork_arg0,
+ (prog_void *)&cmd_fork_arg1,
NULL,
},
};
.data = NULL, /* 2nd arg of func */
.help_str = help_roller,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_roller_arg0,
- (prog_void *)&cmd_roller_arg1,
+ (prog_void *)&cmd_roller_arg0,
+ (prog_void *)&cmd_roller_arg1,
NULL,
},
};
.data = NULL, /* 2nd arg of func */
.help_str = help_test,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_test_arg0,
+ (prog_void *)&cmd_test_arg0,
NULL,
},
};
static const prog_char i2cproto_log[] = "i2cproto";
static const prog_char sensor_log[] = "sensor";
static const prog_char block_log[] = "bd";
-static const prog_char beacon_log[] = "beacon";
-static const prog_char scanner_log[] = "scanner";
-static const prog_char imgprocess_log[] = "imgprocess";
+static const prog_char state_log[] = "state";
struct log_name_and_num {
const prog_char * name;
{ i2cproto_log, E_USER_I2C_PROTO },
{ sensor_log, E_USER_SENSOR },
{ block_log, E_BLOCKING_DETECTION_MANAGER },
- { beacon_log, E_USER_BEACON },
- { scanner_log, E_USER_SCANNER },
- { imgprocess_log, E_USER_IMGPROCESS },
+ { state_log, E_USER_ST_MACH },
};
static uint8_t
prog_char str_log_arg1_type[] = "type";
parse_pgm_token_string_t cmd_log_arg1_type = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg1, str_log_arg1_type);
/* keep it sync with log_name_and_num above */
-prog_char str_log_arg2_type[] = "uart#i2c#i2cproto#sensor#bd#beacon#scanner#imgprocess";
+prog_char str_log_arg2_type[] = "uart#i2c#i2cproto#sensor#bd#state";
parse_pgm_token_string_t cmd_log_arg2_type = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg2, str_log_arg2_type);
prog_char str_log_arg3[] = "on#off";
parse_pgm_token_string_t cmd_log_arg3 = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg3, str_log_arg3);
-/*
+/*
* Copyright Droids Corporation
* Olivier Matz <zer0@droids-corp.org>
- *
+ *
* 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
if (ballboard.forkrot.on)
cs_manage(&ballboard.forkrot.cs);
}
- if (ballboard.flags & DO_BD) {
+ if ((ballboard.flags & DO_BD) && (ballboard.flags & DO_POWER)) {
bd_manage_from_cs(&ballboard.roller.bd, &ballboard.roller.cs);
bd_manage_from_cs(&ballboard.forktrans.bd, &ballboard.forktrans.cs);
bd_manage_from_cs(&ballboard.forkrot.bd, &ballboard.forkrot.cs);
+
+ /* urgent case: stop power on blocking */
+ if (ballboard.flags & DO_ERRBLOCKING) {
+ if (bd_get(&ballboard.roller.bd) ||
+ bd_get(&ballboard.forktrans.bd) ||
+ bd_get(&ballboard.forkrot.bd)) {
+ printf_P(PSTR("MOTOR BLOCKED STOP ALL\r\n"));
+ ballboard.flags &= ~(DO_POWER | DO_ERRBLOCKING);
+ }
+ }
}
if (ballboard.flags & DO_POWER)
BRAKE_OFF();
void dump_cs(const char *name, struct cs *cs)
{
printf_P(PSTR("%s cons=% .5ld fcons=% .5ld err=% .5ld "
- "in=% .5ld out=% .5ld\r\n"),
+ "in=% .5ld out=% .5ld\r\n"),
name, cs_get_consign(cs), cs_get_filtered_consign(cs),
cs_get_error(cs), cs_get_filtered_feedback(cs),
cs_get_out(cs));
cs_set_process_out(&ballboard.roller.cs, encoders_spi_update_roller_speed, ROLLER_ENCODER);
cs_set_consign(&ballboard.roller.cs, 0);
+ /* Blocking detection */
+ bd_init(&ballboard.roller.bd);
+ bd_set_speed_threshold(&ballboard.roller.bd, 150);
+ bd_set_current_thresholds(&ballboard.roller.bd, 500, 8000, 1000000, 200);
+
/* ---- CS forktrans */
/* PID */
pid_init(&ballboard.forktrans.pid);
- pid_set_gains(&ballboard.forktrans.pid, 200, 5, 250);
+ pid_set_gains(&ballboard.forktrans.pid, 30, 5, 0);
pid_set_maximums(&ballboard.forktrans.pid, 0, 10000, 2047);
pid_set_out_shift(&ballboard.forktrans.pid, 6);
pid_set_derivate_filter(&ballboard.forktrans.pid, 6);
/* Blocking detection */
bd_init(&ballboard.forktrans.bd);
bd_set_speed_threshold(&ballboard.forktrans.bd, 150);
- bd_set_current_thresholds(&ballboard.forktrans.bd, 500, 8000, 1000000, 40);
+ bd_set_current_thresholds(&ballboard.forktrans.bd, 500, 8000, 1000000, 200);
/* ---- CS forkrot */
/* PID */
pid_init(&ballboard.forkrot.pid);
- pid_set_gains(&ballboard.forkrot.pid, 200, 5, 250);
+ pid_set_gains(&ballboard.forkrot.pid, 30, 5, 0);
pid_set_maximums(&ballboard.forkrot.pid, 0, 10000, 2047);
pid_set_out_shift(&ballboard.forkrot.pid, 6);
pid_set_derivate_filter(&ballboard.forkrot.pid, 6);
/* QUADRAMP */
quadramp_init(&ballboard.forkrot.qr);
- quadramp_set_1st_order_vars(&ballboard.forkrot.qr, 200, 200); /* set speed */
+ quadramp_set_1st_order_vars(&ballboard.forkrot.qr, 800, 800); /* set speed */
quadramp_set_2nd_order_vars(&ballboard.forkrot.qr, 20, 20); /* set accel */
/* CS */
/* Blocking detection */
bd_init(&ballboard.forkrot.bd);
bd_set_speed_threshold(&ballboard.forkrot.bd, 150);
- bd_set_current_thresholds(&ballboard.forkrot.bd, 500, 8000, 1000000, 40);
+ bd_set_current_thresholds(&ballboard.forkrot.bd, 500, 8000, 1000000, 200);
/* set them on !! */
- ballboard.roller.on = 0;
+ ballboard.roller.on = 1;
ballboard.forktrans.on = 1;
ballboard.forkrot.on = 1;
scheduler_add_periodical_event_priority(do_cs, NULL,
- 5000L / SCHEDULER_UNIT,
+ 5000L / SCHEDULER_UNIT,
CS_PRIO);
}
void do_led_blink(__attribute__((unused)) void *dummy)
{
-#if 1 /* simple blink */
- static uint8_t a=0;
+ static uint8_t a = 0;
- if(a)
- LED1_ON();
- else
- LED1_OFF();
-
- a = !a;
-#endif
+ if (ballboard.flags & DO_ERRBLOCKING) {
+ if (a & 1)
+ LED1_ON();
+ else
+ LED1_OFF();
+ }
+ else {
+ if (a & 4)
+ LED1_ON();
+ else
+ LED1_OFF();
+ }
+ a++;
}
static void main_timer_interrupt(void)
LED1_OFF();
memset(&gen, 0, sizeof(gen));
memset(&ballboard, 0, sizeof(ballboard));
- ballboard.flags = DO_ENCODERS | DO_CS | DO_POWER; // DO_BD
+ ballboard.flags = DO_ENCODERS | DO_CS | DO_POWER |
+ DO_ERRBLOCKING | DO_BD;
/* UART */
uart_init();
/* ax12 */
ax12_user_init();
+ gen.logs[0] = E_USER_ST_MACH;
+ gen.log_level = 5;
+
sei();
printf_P(PSTR("\r\n"));
/** ERROR NUMS */
#define E_USER_I2C_PROTO 195
#define E_USER_SENSOR 196
-#define E_USER_BEACON 197
-#define E_USER_SCANNER 198
-#define E_USER_IMGPROCESS 199
+#define E_USER_ST_MACH 197
#define LED_PRIO 170
#define TIME_PRIO 160
#define DO_CS 2
#define DO_BD 4
#define DO_POWER 8
+#define DO_ERRBLOCKING 16
uint8_t flags; /* misc flags */
/* control systems */
int8_t state_set_mode(uint8_t mode)
{
state_mode = mode;
+ STMCH_DEBUG("%s(): mode=%x ", __FUNCTION__, mode);
/* STMCH_DEBUG("%s(): l_deploy=%d l_harvest=%d " */
/* "r_deploy=%d r_harvest=%d eject=%d", */
static void state_do_eject(void)
{
roller_reverse();
+ time_wait_ms(2000);
}
/* main state machine */
cobboard.right_cobroller_speed = 0;
}
+void cobroller_reverse(uint8_t side)
+{
+ if (side == I2C_LEFT_SIDE)
+ cobboard.left_cobroller_speed = -COBROLLER_SPEED;
+ else
+ cobboard.right_cobroller_speed = COBROLLER_SPEED;
+}
+
void actuator_init(void)
{
void cobroller_on(uint8_t side);
void cobroller_off(uint8_t side);
+void cobroller_reverse(uint8_t side);
#endif
/*
* 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_cobboard.c,v 1.6 2009-11-08 17:25:00 zer0 Exp $
*
- * Olivier MATZ <zer0@droids-corp.org>
+ * Olivier MATZ <zer0@droids-corp.org>
*/
#include <stdio.h>
struct cmd_event_result * res = parsed_result;
if (!strcmp_P(res->arg1, PSTR("all"))) {
- bit = DO_ENCODERS | DO_CS | DO_BD | DO_POWER;
+ bit = 0xFF;
if (!strcmp_P(res->arg2, PSTR("on")))
cobboard.flags |= bit;
else if (!strcmp_P(res->arg2, PSTR("off")))
cobboard.flags &= bit;
else { /* show */
- printf_P(PSTR("encoders is %s\r\n"),
+ printf_P(PSTR("encoders is %s\r\n"),
(DO_ENCODERS & cobboard.flags) ? "on":"off");
- printf_P(PSTR("cs is %s\r\n"),
+ printf_P(PSTR("cs is %s\r\n"),
(DO_CS & cobboard.flags) ? "on":"off");
- printf_P(PSTR("bd is %s\r\n"),
+ printf_P(PSTR("bd is %s\r\n"),
(DO_BD & cobboard.flags) ? "on":"off");
- printf_P(PSTR("power is %s\r\n"),
+ printf_P(PSTR("power is %s\r\n"),
(DO_POWER & cobboard.flags) ? "on":"off");
+ printf_P(PSTR("errblock is %s\r\n"),
+ (DO_ERRBLOCKING & cobboard.flags) ? "on":"off");
}
return;
}
bit = DO_BD;
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")))
}
cobboard.flags &= (~bit);
}
- printf_P(PSTR("%s is %s\r\n"), res->arg1,
+ printf_P(PSTR("%s is %s\r\n"), res->arg1,
(bit & cobboard.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#bd#power";
+prog_char str_event_arg1[] = "all#encoders#cs#bd#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,
},
};
.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,
},
};
.data = NULL, /* 2nd arg of func */
.help_str = help_state1,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_state1_arg0,
- (prog_void *)&cmd_state1_arg1,
+ (prog_void *)&cmd_state1_arg0,
+ (prog_void *)&cmd_state1_arg1,
NULL,
},
};
.data = NULL, /* 2nd arg of func */
.help_str = help_state2,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_state2_arg0,
- (prog_void *)&cmd_state2_arg1,
- (prog_void *)&cmd_state2_arg2,
+ (prog_void *)&cmd_state2_arg0,
+ (prog_void *)&cmd_state2_arg1,
+ (prog_void *)&cmd_state2_arg2,
NULL,
},
};
};
/* function called when cmd_state3 is parsed successfully */
-static void cmd_state3_parsed(void *parsed_result,
+static void cmd_state3_parsed(void *parsed_result,
__attribute__((unused)) void *data)
{
struct cmd_state3_result *res = parsed_result;
.data = NULL, /* 2nd arg of func */
.help_str = help_state3,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_state3_arg0,
- (prog_void *)&cmd_state3_arg1,
- (prog_void *)&cmd_state3_arg2,
+ (prog_void *)&cmd_state3_arg0,
+ (prog_void *)&cmd_state3_arg1,
+ (prog_void *)&cmd_state3_arg2,
NULL,
},
};
.data = NULL, /* 2nd arg of func */
.help_str = help_state_machine,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_state_machine_arg0,
+ (prog_void *)&cmd_state_machine_arg0,
NULL,
},
};
.data = NULL, /* 2nd arg of func */
.help_str = help_state_debug,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_state_debug_arg0,
- (prog_void *)&cmd_state_debug_on,
+ (prog_void *)&cmd_state_debug_arg0,
+ (prog_void *)&cmd_state_debug_on,
NULL,
},
};
.data = NULL, /* 2nd arg of func */
.help_str = help_servo_door,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_servo_door_arg0,
- (prog_void *)&cmd_servo_door_arg1,
+ (prog_void *)&cmd_servo_door_arg0,
+ (prog_void *)&cmd_servo_door_arg1,
NULL,
},
};
.data = NULL, /* 2nd arg of func */
.help_str = help_cobroller,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_cobroller_arg0,
- (prog_void *)&cmd_cobroller_arg1,
- (prog_void *)&cmd_cobroller_arg2,
+ (prog_void *)&cmd_cobroller_arg0,
+ (prog_void *)&cmd_cobroller_arg1,
+ (prog_void *)&cmd_cobroller_arg2,
NULL,
},
};
.data = NULL, /* 2nd arg of func */
.help_str = help_shovel,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_shovel_arg0,
- (prog_void *)&cmd_shovel_arg1,
+ (prog_void *)&cmd_shovel_arg0,
+ (prog_void *)&cmd_shovel_arg1,
NULL,
},
};
.data = NULL, /* 2nd arg of func */
.help_str = help_servo_carry,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_servo_carry_arg0,
- (prog_void *)&cmd_servo_carry_arg1,
+ (prog_void *)&cmd_servo_carry_arg0,
+ (prog_void *)&cmd_servo_carry_arg1,
NULL,
},
};
.data = NULL, /* 2nd arg of func */
.help_str = help_spickle,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_spickle_arg0,
- (prog_void *)&cmd_spickle_arg1,
- (prog_void *)&cmd_spickle_arg2,
+ (prog_void *)&cmd_spickle_arg0,
+ (prog_void *)&cmd_spickle_arg1,
+ (prog_void *)&cmd_spickle_arg2,
NULL,
},
};
.data = NULL, /* 2nd arg of func */
.help_str = help_spickle_params_show,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_spickle_params_arg0,
- (prog_void *)&cmd_spickle_params_arg1_show,
+ (prog_void *)&cmd_spickle_params_arg0,
+ (prog_void *)&cmd_spickle_params_arg1_show,
NULL,
},
};
__attribute__((unused)) void *data)
{
struct cmd_spickle_params2_result * res = parsed_result;
-
+
if (!strcmp_P(res->arg1, PSTR("coef"))) {
spickle_set_coefs(res->arg2, res->arg3);
}
.data = NULL, /* 2nd arg of func */
.help_str = help_spickle_params2,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_spickle_params2_arg0,
- (prog_void *)&cmd_spickle_params2_arg1,
- (prog_void *)&cmd_spickle_params2_arg2,
- (prog_void *)&cmd_spickle_params2_arg3,
+ (prog_void *)&cmd_spickle_params2_arg0,
+ (prog_void *)&cmd_spickle_params2_arg1,
+ (prog_void *)&cmd_spickle_params2_arg2,
+ (prog_void *)&cmd_spickle_params2_arg3,
NULL,
},
};
.data = NULL, /* 2nd arg of func */
.help_str = help_spickle_params2_show,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_spickle_params2_arg0,
- (prog_void *)&cmd_spickle_params2_arg1_show,
+ (prog_void *)&cmd_spickle_params2_arg0,
+ (prog_void *)&cmd_spickle_params2_arg1_show,
NULL,
},
};
.data = NULL, /* 2nd arg of func */
.help_str = help_test,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_test_arg0,
+ (prog_void *)&cmd_test_arg0,
NULL,
},
};
}
#endif
- if (cobboard.flags & DO_BD) {
+ if ((cobboard.flags & DO_BD) && (cobboard.flags & DO_POWER)) {
bd_manage_from_cs(&cobboard.left_spickle.bd, &cobboard.left_spickle.cs);
bd_manage_from_cs(&cobboard.right_spickle.bd, &cobboard.right_spickle.cs);
bd_manage_from_cs(&cobboard.shovel.bd, &cobboard.shovel.cs);
+
+ /* urgent case: stop power on blocking */
+ if (cobboard.flags & DO_ERRBLOCKING) {
+ if (bd_get(&cobboard.left_spickle.bd) ||
+ bd_get(&cobboard.right_spickle.bd) ||
+ bd_get(&cobboard.shovel.bd)) {
+ printf_P(PSTR("MOTOR BLOCKED STOP ALL\r\n"));
+ cobboard.flags &= ~(DO_POWER | DO_ERRBLOCKING);
+ }
+ }
}
if (cobboard.flags & DO_POWER)
BRAKE_OFF();
/* Blocking detection */
bd_init(&cobboard.left_spickle.bd);
bd_set_speed_threshold(&cobboard.left_spickle.bd, 150);
- bd_set_current_thresholds(&cobboard.left_spickle.bd, 500, 8000, 1000000, 40);
+ bd_set_current_thresholds(&cobboard.left_spickle.bd, 500, 8000, 1000000, 200);
/* ---- CS right_spickle */
/* PID */
/* Blocking detection */
bd_init(&cobboard.right_spickle.bd);
bd_set_speed_threshold(&cobboard.right_spickle.bd, 150);
- bd_set_current_thresholds(&cobboard.right_spickle.bd, 500, 8000, 1000000, 40);
+ bd_set_current_thresholds(&cobboard.right_spickle.bd, 500, 8000, 1000000, 200);
/* ---- CS shovel */
/* PID */
/* Blocking detection */
bd_init(&cobboard.shovel.bd);
bd_set_speed_threshold(&cobboard.shovel.bd, 150);
- bd_set_current_thresholds(&cobboard.shovel.bd, 500, 8000, 1000000, 40);
+ bd_set_current_thresholds(&cobboard.shovel.bd, 500, 8000, 1000000, 200);
/* set them on (or not) !! */
cobboard.left_spickle.on = 0;
/* status */
ans.mode = state_get_mode();
- ans.status = 0x55; /* TODO */
+ ans.status = state_get_status();
ans.left_cobroller_speed = cobboard.left_cobroller_speed;
ans.right_cobroller_speed = cobboard.right_cobroller_speed;
void do_led_blink(__attribute__((unused)) void *dummy)
{
-#if 1 /* simple blink */
- static uint8_t a=0;
+ static uint8_t a = 0;
- if(a)
- LED1_ON();
- else
- LED1_OFF();
-
- a = !a;
-#endif
+ if (cobboard.flags & DO_ERRBLOCKING) {
+ if (a & 1)
+ LED1_ON();
+ else
+ LED1_OFF();
+ }
+ else {
+ if (a & 4)
+ LED1_ON();
+ else
+ LED1_OFF();
+ }
+ a++;
}
static void main_timer_interrupt(void)
memset(&gen, 0, sizeof(gen));
memset(&cobboard, 0, sizeof(cobboard));
/* cs is enabled after arm_calibrate() */
- cobboard.flags = DO_ENCODERS | DO_POWER; // DO_BD
+ cobboard.flags = DO_ENCODERS | DO_POWER | DO_BD |
+ DO_ERRBLOCKING;
/* UART */
uart_init();
#define DO_CS 2
#define DO_BD 4
#define DO_POWER 8
+#define DO_ERRBLOCKING 16
uint8_t flags; /* misc flags */
/* control systems */
static struct vt100 local_vt100;
static volatile uint8_t state_mode;
+static volatile uint8_t state_status;
static uint8_t cob_count;
/* short aliases */
uint8_t state_debug = 0;
-#if 0
-static void state_dump_sensors(void)
-{
- STMCH_DEBUG("TODO\n");
-}
-#endif
-
uint8_t state_get_cob_count(void)
{
return cob_count;
!sensor_get(S_COB_INSIDE_R);
}
+/* pack/deploy spickles depending on mode */
+static void spickle_prepare(uint8_t side)
+{
+ if (cob_count >= 5)
+ spickle_pack(side);
+ else if (DEPLOY(side, state_mode) && !HARVEST(side, state_mode))
+ spickle_mid(side);
+ else if (DEPLOY(side, state_mode) && HARVEST(side, state_mode))
+ spickle_deploy(side);
+ else
+ spickle_pack(side);
+}
+
/* set a new state, return 0 on success */
int8_t state_set_mode(uint8_t mode)
{
state_mode = mode;
+ /* preempt current action if not busy */
+ if (state_status != I2C_COBBOARD_STATUS_LBUSY)
+ spickle_prepare(I2C_LEFT_SIDE);
+ if (state_status != I2C_COBBOARD_STATUS_RBUSY)
+ spickle_prepare(I2C_RIGHT_SIDE);
+
/* STMCH_DEBUG("%s(): l_deploy=%d l_harvest=%d " */
/* "r_deploy=%d r_harvest=%d eject=%d", */
/* __FUNCTION__, L_DEPLOY(mode), L_HARVEST(mode), */
return state_mode;
}
+uint8_t state_get_status(void)
+{
+ return state_status;
+}
+
/* harvest cobs from area */
static void state_do_harvest(uint8_t side)
{
+ if (side == I2C_LEFT_SIDE)
+ state_status = I2C_COBBOARD_STATUS_LBUSY;
+ else
+ state_status = I2C_COBBOARD_STATUS_RBUSY;
+
/* if there is no cob, return */
if (state_cob_present(side))
return;
time_wait_ms(250);
cobroller_on(side);
+ /* check that cob is correctly in place */
if (WAIT_COND_OR_TIMEOUT(state_cob_inside(), 750) == 0) {
if (state_no_cob_inside()) {
- printf_P(PSTR("NO COB\r\n"));
+ STMCH_DEBUG("no cob");
+ return;
+ }
+ STMCH_DEBUG("bad cob state");
+
+ /* while cob is not correctly placed try to extract
+ * it as much as we can */
+ while (state_cob_inside() == 0 &&
+ state_no_cob_inside() == 0) {
+ uint8_t cpt = 0;
+ if (cpt == 0)
+ cobroller_reverse(side);
+ else if (cpt == 1)
+ cobroller_off(side);
+ else if (cpt == 2)
+ cobroller_on(side);
+ cpt ++;
+ cpt %= 3;
+ shovel_mid();
+ time_wait_ms(250);
+ shovel_down();
+ time_wait_ms(250);
+ }
+
+ STMCH_DEBUG("cob removed");
+ if (state_no_cob_inside()) {
+ STMCH_DEBUG("no cob");
return;
}
- printf_P(PSTR("BAD COB - press a key\r\n"));
- while(!cmdline_keypressed());
}
/* cob is inside, switch off roller */
/* store it */
shovel_up();
- if (WAIT_COND_OR_TIMEOUT(shovel_is_up(), 400) == 0) {
- BRAKE_ON();
- printf_P(PSTR("BLOCKED\r\n"));
- while(!cmdline_keypressed());
+ while (WAIT_COND_OR_TIMEOUT(shovel_is_up(), 400) == 0) {
+ STMCH_DEBUG("shovel blocked");
+ shovel_down();
+ time_wait_ms(250);
+ shovel_up();
}
state_debug_wait_key_pressed();
shovel_down();
- if (WAIT_COND_OR_TIMEOUT(shovel_is_down(), 400) == 0) {
- BRAKE_ON();
- printf_P(PSTR("BLOCKED\r\n"));
- while(!cmdline_keypressed());
+ while (WAIT_COND_OR_TIMEOUT(shovel_is_down(), 400) == 0) {
+ STMCH_DEBUG("shovel blocked");
+ shovel_up();
+ time_wait_ms(250);
+ shovel_down();
}
STMCH_DEBUG("end");
/* eject cobs */
static void state_do_eject(void)
{
+ state_status = I2C_COBBOARD_STATUS_EJECT;
cob_count = 0;
shovel_mid();
servo_carry_open();
{
while (state_want_exit() == 0) {
+ state_status = I2C_COBBOARD_STATUS_READY;
+
/* init */
if (INIT(state_mode)) {
state_mode &= (~I2C_COBBOARD_MODE_INIT);
state_init();
}
+ /* pack/deply spickles, enable/disable roller */
cobroller_off(I2C_LEFT_SIDE);
cobroller_off(I2C_RIGHT_SIDE);
-
- /* pack/deply spickles, enable/disable roller */
- if (cob_count >= 5)
- spickle_pack(I2C_LEFT_SIDE);
- else if (L_DEPLOY(state_mode) && !L_HARVEST(state_mode))
- spickle_mid(I2C_LEFT_SIDE);
- else if (L_DEPLOY(state_mode) && L_HARVEST(state_mode))
- spickle_deploy(I2C_LEFT_SIDE);
- else
- spickle_pack(I2C_LEFT_SIDE);
-
- if (cob_count >= 5)
- spickle_pack(I2C_RIGHT_SIDE);
- else if (R_DEPLOY(state_mode) && !R_HARVEST(state_mode))
- spickle_mid(I2C_RIGHT_SIDE);
- else if (R_DEPLOY(state_mode) && R_HARVEST(state_mode))
- spickle_deploy(I2C_RIGHT_SIDE);
- else
- spickle_pack(I2C_RIGHT_SIDE);
+ spickle_prepare(I2C_LEFT_SIDE);
+ spickle_prepare(I2C_RIGHT_SIDE);
/* harvest */
if (cob_count < 5) {
spickle_pack(I2C_RIGHT_SIDE);
state_mode = 0;
cob_count = 0;
+ state_status = I2C_COBBOARD_STATUS_READY;
}
/* get current state */
uint8_t state_get_mode(void);
+uint8_t state_get_status(void);
uint8_t state_get_cob_count(void);
struct i2c_cmd_led_control {
struct i2c_cmd_hdr hdr;
uint8_t led_num:7;
- uint8_t state:1;
+ uint8_t state:1;
};
/****/
/* mode type are defined above: I2C_COBBOARD_MODE_xxx */
uint8_t mode;
-#define I2C_COBBOARD_STATUS_F_READY 0x00
-#define I2C_COBBOARD_STATUS_F_BUSY 0x01
-#define I2C_COBBOARD_STATUS_F_EXCPT 0x02
+#define I2C_COBBOARD_STATUS_READY 0x00
+#define I2C_COBBOARD_STATUS_LBUSY 0x01
+#define I2C_COBBOARD_STATUS_RBUSY 0x02
+#define I2C_COBBOARD_STATUS_EJECT 0x03
uint8_t status;
uint8_t cob_count;
extern parse_pgm_inst_t cmd_cobboard_setmode1;
extern parse_pgm_inst_t cmd_cobboard_setmode2;
extern parse_pgm_inst_t cmd_cobboard_setmode3;
+extern parse_pgm_inst_t cmd_ballboard_show;
+extern parse_pgm_inst_t cmd_ballboard_setmode1;
+extern parse_pgm_inst_t cmd_ballboard_setmode2;
+extern parse_pgm_inst_t cmd_ballboard_setmode3;
extern parse_pgm_inst_t cmd_beacon_start;
extern parse_pgm_inst_t cmd_servo_balls;
extern parse_pgm_inst_t cmd_clitoid;
/* commands_traj.c */
extern parse_pgm_inst_t cmd_traj_speed;
extern parse_pgm_inst_t cmd_traj_speed_show;
+extern parse_pgm_inst_t cmd_traj_acc;
+extern parse_pgm_inst_t cmd_traj_acc_show;
extern parse_pgm_inst_t cmd_trajectory;
extern parse_pgm_inst_t cmd_trajectory_show;
extern parse_pgm_inst_t cmd_circle_coef;
(parse_pgm_inst_t *)&cmd_cobboard_setmode1,
(parse_pgm_inst_t *)&cmd_cobboard_setmode2,
(parse_pgm_inst_t *)&cmd_cobboard_setmode3,
+ (parse_pgm_inst_t *)&cmd_ballboard_show,
+ (parse_pgm_inst_t *)&cmd_ballboard_setmode1,
+ (parse_pgm_inst_t *)&cmd_ballboard_setmode2,
+ (parse_pgm_inst_t *)&cmd_ballboard_setmode3,
(parse_pgm_inst_t *)&cmd_servo_balls,
(parse_pgm_inst_t *)&cmd_clitoid,
(parse_pgm_inst_t *)&cmd_test,
/* commands_traj.c */
(parse_pgm_inst_t *)&cmd_traj_speed,
(parse_pgm_inst_t *)&cmd_traj_speed_show,
+ (parse_pgm_inst_t *)&cmd_traj_acc,
+ (parse_pgm_inst_t *)&cmd_traj_acc_show,
(parse_pgm_inst_t *)&cmd_trajectory,
(parse_pgm_inst_t *)&cmd_trajectory_show,
(parse_pgm_inst_t *)&cmd_circle_coef,
/*
* 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>
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,
},
};
.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 {
switch(cmd) {
- case KEY_UP_ARR:
+ case KEY_UP_ARR:
pwm_set_and_save(LEFT_PWM, 1200);
pwm_set_and_save(RIGHT_PWM, 1200);
break;
- case KEY_LEFT_ARR:
+ case KEY_LEFT_ARR:
pwm_set_and_save(LEFT_PWM, -1200);
pwm_set_and_save(RIGHT_PWM, 1200);
break;
- case KEY_DOWN_ARR:
+ case KEY_DOWN_ARR:
pwm_set_and_save(LEFT_PWM, -1200);
pwm_set_and_save(RIGHT_PWM, -1200);
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();
else if (!strcmp_P(res->arg1, PSTR("eject")))
i2c_cobboard_mode_eject();
-#endif
}
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)
{
-#ifdef HOST_VERSION
- printf("not implemented\n");
-#else
struct cmd_cobboard_setmode2_result *res = parsed_result;
uint8_t side = I2C_LEFT_SIDE;
i2c_cobboard_mode_harvest(side);
else if (!strcmp_P(res->arg1, PSTR("pack")))
i2c_cobboard_mode_pack(side);
-#endif
}
prog_char str_cobboard_setmode2_arg0[] = "cobboard";
.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,
},
};
.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,
},
};
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";
},
};
-//////////////////////
-
-// 500 -- 5
-// 400 -- 3
-#define TEST_SPEED 400
-#define TEST_ACC 3
-
-static void line2line(double line1x1, double line1y1,
- double line1x2, double line1y2,
- double line2x1, double line2y1,
- double line2x2, double line2y2,
- double radius, double dist)
-{
- 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);
-
- /* take some margin on dist to avoid deceleration */
- trajectory_d_a_rel(&mainboard.traj, distance + 250, angle);
-
- /* circle exit condition */
- err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
- TRAJ_FLAGS_NO_NEAR);
-
- 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.);
-}
-
-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)
-{
- 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.);
-}
-
/**********************************************************/
/* 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);
}
/*
* 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_traj.c,v 1.8 2009-11-08 17:24:33 zer0 Exp $
*
- * Olivier MATZ <zer0@droids-corp.org>
+ * Olivier MATZ <zer0@droids-corp.org>
*/
#include <stdio.h>
struct cmd_traj_speed_result {
fixed_string_t arg0;
fixed_string_t arg1;
- uint16_t s;
+ float s;
};
/* function called when cmd_traj_speed is parsed successfully */
static void cmd_traj_speed_parsed(void *parsed_result, void *data)
{
struct cmd_traj_speed_result * res = parsed_result;
-
+
if (!strcmp_P(res->arg1, PSTR("angle"))) {
trajectory_set_speed(&mainboard.traj, mainboard.traj.d_speed, res->s);
}
}
/* else it is a "show" */
- printf_P(PSTR("angle %u, distance %u\r\n"),
+ printf_P(PSTR("angle %2.2f, distance %2.2f\r\n"),
mainboard.traj.a_speed,
mainboard.traj.d_speed);
}
parse_pgm_token_string_t cmd_traj_speed_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg0, str_traj_speed_arg0);
prog_char str_traj_speed_arg1[] = "angle#distance";
parse_pgm_token_string_t cmd_traj_speed_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg1, str_traj_speed_arg1);
-parse_pgm_token_num_t cmd_traj_speed_s = TOKEN_NUM_INITIALIZER(struct cmd_traj_speed_result, s, UINT16);
+parse_pgm_token_num_t cmd_traj_speed_s = TOKEN_NUM_INITIALIZER(struct cmd_traj_speed_result, s, FLOAT);
prog_char help_traj_speed[] = "Set traj_speed values for trajectory manager";
parse_pgm_inst_t cmd_traj_speed = {
.data = NULL, /* 2nd arg of func */
.help_str = help_traj_speed,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_traj_speed_arg0,
- (prog_void *)&cmd_traj_speed_arg1,
- (prog_void *)&cmd_traj_speed_s,
+ (prog_void *)&cmd_traj_speed_arg0,
+ (prog_void *)&cmd_traj_speed_arg1,
+ (prog_void *)&cmd_traj_speed_s,
NULL,
},
};
.data = NULL, /* 2nd arg of func */
.help_str = help_traj_speed_show,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_traj_speed_arg0,
+ (prog_void *)&cmd_traj_speed_arg0,
(prog_void *)&cmd_traj_speed_show_arg,
NULL,
},
};
+/**********************************************************/
+/* Traj_Accs for trajectory_manager */
+
+/* this structure is filled when cmd_traj_acc is parsed successfully */
+struct cmd_traj_acc_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ float s;
+};
+
+/* function called when cmd_traj_acc is parsed successfully */
+static void cmd_traj_acc_parsed(void *parsed_result, void *data)
+{
+ struct cmd_traj_acc_result * res = parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("angle"))) {
+ trajectory_set_acc(&mainboard.traj, mainboard.traj.d_acc, res->s);
+ }
+ else if (!strcmp_P(res->arg1, PSTR("distance"))) {
+ trajectory_set_acc(&mainboard.traj, res->s, mainboard.traj.a_acc);
+ }
+ /* else it is a "show" */
+
+ printf_P(PSTR("angle %2.2f, distance %2.2f\r\n"),
+ mainboard.traj.a_acc,
+ mainboard.traj.d_acc);
+}
+
+prog_char str_traj_acc_arg0[] = "traj_acc";
+parse_pgm_token_string_t cmd_traj_acc_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_traj_acc_result, arg0, str_traj_acc_arg0);
+prog_char str_traj_acc_arg1[] = "angle#distance";
+parse_pgm_token_string_t cmd_traj_acc_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_traj_acc_result, arg1, str_traj_acc_arg1);
+parse_pgm_token_num_t cmd_traj_acc_s = TOKEN_NUM_INITIALIZER(struct cmd_traj_acc_result, s, FLOAT);
+
+prog_char help_traj_acc[] = "Set traj_acc values for trajectory manager";
+parse_pgm_inst_t cmd_traj_acc = {
+ .f = cmd_traj_acc_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_traj_acc,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_traj_acc_arg0,
+ (prog_void *)&cmd_traj_acc_arg1,
+ (prog_void *)&cmd_traj_acc_s,
+ NULL,
+ },
+};
+
+/* show */
+
+prog_char str_traj_acc_show_arg[] = "show";
+parse_pgm_token_string_t cmd_traj_acc_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_traj_acc_result, arg1, str_traj_acc_show_arg);
+
+prog_char help_traj_acc_show[] = "Show traj_acc values for trajectory manager";
+parse_pgm_inst_t cmd_traj_acc_show = {
+ .f = cmd_traj_acc_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_traj_acc_show,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_traj_acc_arg0,
+ (prog_void *)&cmd_traj_acc_show_arg,
+ NULL,
+ },
+};
+
/**********************************************************/
/* circle coef configuration */
.data = NULL, /* 2nd arg of func */
.help_str = help_circle_coef_show,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_circle_coef_arg0,
+ (prog_void *)&cmd_circle_coef_arg0,
(prog_void *)&cmd_circle_coef_show_arg,
NULL,
},
static void cmd_trajectory_parsed(void * parsed_result, void * data)
{
struct cmd_trajectory_result * res = parsed_result;
-
+
if (!strcmp_P(res->arg1, PSTR("set"))) {
trajectory_set_windows(&mainboard.traj, res->d_win,
res->a_win, res->a_start);
.data = NULL, /* 2nd arg of func */
.help_str = help_trajectory,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_trajectory_arg0,
- (prog_void *)&cmd_trajectory_arg1,
- (prog_void *)&cmd_trajectory_d,
- (prog_void *)&cmd_trajectory_a,
- (prog_void *)&cmd_trajectory_as,
+ (prog_void *)&cmd_trajectory_arg0,
+ (prog_void *)&cmd_trajectory_arg1,
+ (prog_void *)&cmd_trajectory_d,
+ (prog_void *)&cmd_trajectory_a,
+ (prog_void *)&cmd_trajectory_as,
NULL,
},
};
.data = NULL, /* 2nd arg of func */
.help_str = help_trajectory_show,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_trajectory_arg0,
+ (prog_void *)&cmd_trajectory_arg0,
(prog_void *)&cmd_trajectory_show_arg,
NULL,
},
struct cmd_rs_gains_result * res = parsed_result;
if (!strcmp_P(res->arg1, PSTR("set"))) {
- rs_set_left_ext_encoder(&mainboard.rs, encoders_spi_get_value,
+ rs_set_left_ext_encoder(&mainboard.rs, encoders_spi_get_value,
LEFT_ENCODER, res->left); // en augmentant on tourne à gauche
- rs_set_right_ext_encoder(&mainboard.rs, encoders_spi_get_value,
+ rs_set_right_ext_encoder(&mainboard.rs, encoders_spi_get_value,
RIGHT_ENCODER, res->right); //en augmentant on tourne à droite
}
printf_P(PSTR("rs_gains set %2.2f %2.2f\r\n"),
.data = NULL, /* 2nd arg of func */
.help_str = help_rs_gains,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_rs_gains_arg0,
- (prog_void *)&cmd_rs_gains_arg1,
- (prog_void *)&cmd_rs_gains_l,
- (prog_void *)&cmd_rs_gains_r,
+ (prog_void *)&cmd_rs_gains_arg0,
+ (prog_void *)&cmd_rs_gains_arg1,
+ (prog_void *)&cmd_rs_gains_l,
+ (prog_void *)&cmd_rs_gains_r,
NULL,
},
};
.data = NULL, /* 2nd arg of func */
.help_str = help_rs_gains_show,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_rs_gains_arg0,
+ (prog_void *)&cmd_rs_gains_arg0,
(prog_void *)&cmd_rs_gains_show_arg,
NULL,
},
.data = NULL, /* 2nd arg of func */
.help_str = help_track,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_track_arg0,
- (prog_void *)&cmd_track_arg1,
- (prog_void *)&cmd_track_val,
+ (prog_void *)&cmd_track_arg0,
+ (prog_void *)&cmd_track_arg1,
+ (prog_void *)&cmd_track_val,
NULL,
},
};
.data = NULL, /* 2nd arg of func */
.help_str = help_track_show,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_track_arg0,
+ (prog_void *)&cmd_track_arg0,
(prog_void *)&cmd_track_show_arg,
NULL,
},
{
struct cmd_pt_list_result * res = parsed_result;
uint8_t i, why=0;
-
+
if (!strcmp_P(res->arg1, PSTR("append"))) {
res->arg2 = pt_list_len;
}
printf_P(PSTR("List is too large\r\n"));
return;
}
- memmove(&pt_list[res->arg2+1], &pt_list[res->arg2],
+ memmove(&pt_list[res->arg2+1], &pt_list[res->arg2],
PT_LIST_SIZE-1-res->arg2);
pt_list[res->arg2].x = res->arg3;
pt_list[res->arg2].y = res->arg4;
printf_P(PSTR("Index too large\r\n"));
return;
}
- memmove(&pt_list[res->arg2], &pt_list[res->arg2+1],
+ memmove(&pt_list[res->arg2], &pt_list[res->arg2+1],
(PT_LIST_SIZE-1-res->arg2)*sizeof(struct xy_point));
pt_list_len--;
}
else if (!strcmp_P(res->arg1, PSTR("reset"))) {
pt_list_len = 0;
}
-
+
/* else it is a "show" or a "start" */
if (pt_list_len == 0) {
printf_P(PSTR("List empty\r\n"));
.data = NULL, /* 2nd arg of func */
.help_str = help_pt_list,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_pt_list_arg0,
- (prog_void *)&cmd_pt_list_arg1,
- (prog_void *)&cmd_pt_list_arg2,
- (prog_void *)&cmd_pt_list_arg3,
- (prog_void *)&cmd_pt_list_arg4,
+ (prog_void *)&cmd_pt_list_arg0,
+ (prog_void *)&cmd_pt_list_arg1,
+ (prog_void *)&cmd_pt_list_arg2,
+ (prog_void *)&cmd_pt_list_arg3,
+ (prog_void *)&cmd_pt_list_arg4,
NULL,
},
};
.data = NULL, /* 2nd arg of func */
.help_str = help_pt_list_append,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_pt_list_arg0,
- (prog_void *)&cmd_pt_list_arg1_append,
- (prog_void *)&cmd_pt_list_arg3,
- (prog_void *)&cmd_pt_list_arg4,
+ (prog_void *)&cmd_pt_list_arg0,
+ (prog_void *)&cmd_pt_list_arg1_append,
+ (prog_void *)&cmd_pt_list_arg3,
+ (prog_void *)&cmd_pt_list_arg4,
NULL,
},
};
.data = NULL, /* 2nd arg of func */
.help_str = help_pt_list_del,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_pt_list_arg0,
- (prog_void *)&cmd_pt_list_del_arg,
+ (prog_void *)&cmd_pt_list_arg0,
+ (prog_void *)&cmd_pt_list_del_arg,
(prog_void *)&cmd_pt_list_arg2,
NULL,
},
.data = NULL, /* 2nd arg of func */
.help_str = help_pt_list_show,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_pt_list_arg0,
+ (prog_void *)&cmd_pt_list_arg0,
(prog_void *)&cmd_pt_list_show_arg,
NULL,
},
.data = NULL, /* 2nd arg of func */
.help_str = help_goto1,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_goto_arg0,
- (prog_void *)&cmd_goto_arg1_a,
- (prog_void *)&cmd_goto_arg2,
+ (prog_void *)&cmd_goto_arg0,
+ (prog_void *)&cmd_goto_arg1_a,
+ (prog_void *)&cmd_goto_arg2,
NULL,
},
};
.data = NULL, /* 2nd arg of func */
.help_str = help_goto2,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_goto_arg0,
- (prog_void *)&cmd_goto_arg1_b,
+ (prog_void *)&cmd_goto_arg0,
+ (prog_void *)&cmd_goto_arg1_b,
(prog_void *)&cmd_goto_arg2,
- (prog_void *)&cmd_goto_arg3,
+ (prog_void *)&cmd_goto_arg3,
NULL,
},
};
}
/* else it's just a "show" */
- printf_P(PSTR("x=%.2f y=%.2f a=%.2f\r\n"),
+ printf_P(PSTR("x=%.2f y=%.2f a=%.2f\r\n"),
position_get_x_double(&mainboard.pos),
position_get_y_double(&mainboard.pos),
DEG(position_get_a_rad_double(&mainboard.pos)));
.data = NULL, /* 2nd arg of func */
.help_str = help_position,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_position_arg0,
- (prog_void *)&cmd_position_arg1,
+ (prog_void *)&cmd_position_arg0,
+ (prog_void *)&cmd_position_arg1,
NULL,
},
};
.data = NULL, /* 2nd arg of func */
.help_str = help_position_set,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_position_arg0,
- (prog_void *)&cmd_position_arg1_set,
- (prog_void *)&cmd_position_arg2,
- (prog_void *)&cmd_position_arg3,
- (prog_void *)&cmd_position_arg4,
+ (prog_void *)&cmd_position_arg0,
+ (prog_void *)&cmd_position_arg1_set,
+ (prog_void *)&cmd_position_arg2,
+ (prog_void *)&cmd_position_arg3,
+ (prog_void *)&cmd_position_arg4,
NULL,
},
};
.data = NULL, /* 2nd arg of func */
.help_str = help_strat_infos,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_strat_infos_arg0,
- (prog_void *)&cmd_strat_infos_arg1,
+ (prog_void *)&cmd_strat_infos_arg0,
+ (prog_void *)&cmd_strat_infos_arg1,
NULL,
},
};
.data = NULL, /* 2nd arg of func */
.help_str = help_strat_conf,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_strat_conf_arg0,
- (prog_void *)&cmd_strat_conf_arg1,
+ (prog_void *)&cmd_strat_conf_arg0,
+ (prog_void *)&cmd_strat_conf_arg1,
NULL,
},
};
on = 1;
else
on = 0;
-
+
#if 0
if (!strcmp_P(res->arg1, PSTR("one_temple_on_disc")))
bit = STRAT_CONF_ONLY_ONE_ON_DISC;
.data = NULL, /* 2nd arg of func */
.help_str = help_strat_conf2,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_strat_conf2_arg0,
- (prog_void *)&cmd_strat_conf2_arg1,
- (prog_void *)&cmd_strat_conf2_arg2,
+ (prog_void *)&cmd_strat_conf2_arg0,
+ (prog_void *)&cmd_strat_conf2_arg1,
+ (prog_void *)&cmd_strat_conf2_arg2,
NULL,
},
};
.data = NULL, /* 2nd arg of func */
.help_str = help_strat_conf3,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_strat_conf3_arg0,
- (prog_void *)&cmd_strat_conf3_arg1,
- (prog_void *)&cmd_strat_conf3_arg2,
+ (prog_void *)&cmd_strat_conf3_arg0,
+ (prog_void *)&cmd_strat_conf3_arg1,
+ (prog_void *)&cmd_strat_conf3_arg2,
NULL,
},
};
/**********************************************************/
/* Subtraj */
+//////////////////////
+
+// 500 -- 5
+// 400 -- 3
+#define TEST_SPEED 400
+#define TEST_ACC 3
+
+static void line2line(double line1x1, double line1y1,
+ double line1x2, double line1y2,
+ double line2x1, double line2y1,
+ double line2x2, double line2y2,
+ double radius, double dist)
+{
+ 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);
+
+ /* take some margin on dist to avoid deceleration */
+ trajectory_d_a_rel(&mainboard.traj, distance + 250, angle);
+
+ /* circle exit condition */
+ err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
+ TRAJ_FLAGS_NO_NEAR);
+
+ 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.);
+}
+
+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)
+{
+ 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.);
+}
+
+
+/* function called when cmd_test is parsed successfully */
+static void subtraj_test(void)
+{
+#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);
+}
+
+
/* this structure is filled when cmd_subtraj is parsed successfully */
struct cmd_subtraj_result {
fixed_string_t arg0;
/* function called when cmd_subtraj is parsed successfully */
static void cmd_subtraj_parsed(void *parsed_result, void *data)
{
-/* struct cmd_subtraj_result *res = parsed_result; */
+ struct cmd_subtraj_result *res = parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("test")))
+ subtraj_test();
- printf_P(PSTR("TODO\r\n"));
trajectory_hardstop(&mainboard.traj);
}
.data = NULL, /* 2nd arg of func */
.help_str = help_subtraj,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_subtraj_arg0,
- (prog_void *)&cmd_subtraj_arg1,
- (prog_void *)&cmd_subtraj_arg2,
- (prog_void *)&cmd_subtraj_arg3,
- (prog_void *)&cmd_subtraj_arg4,
- (prog_void *)&cmd_subtraj_arg5,
+ (prog_void *)&cmd_subtraj_arg0,
+ (prog_void *)&cmd_subtraj_arg1,
+ (prog_void *)&cmd_subtraj_arg2,
+ (prog_void *)&cmd_subtraj_arg3,
+ (prog_void *)&cmd_subtraj_arg4,
+ (prog_void *)&cmd_subtraj_arg5,
NULL,
},
};
+
* compensation) */
position_manage(&mainboard.pos);
}
- if (mainboard.flags & DO_BD) {
+ if ((mainboard.flags & DO_BD) && (mainboard.flags & DO_POWER)) {
bd_manage_from_cs(&mainboard.angle.bd, &mainboard.angle.cs);
bd_manage_from_cs(&mainboard.distance.bd, &mainboard.distance.cs);
#ifndef HOST_VERSION
bd_manage_from_cs(&mainboard.left_cobroller.bd, &mainboard.left_cobroller.cs);
bd_manage_from_cs(&mainboard.right_cobroller.bd, &mainboard.right_cobroller.cs);
+ if (mainboard.flags & DO_ERRBLOCKING) {
+ if (bd_get(&mainboard.left_cobroller.bd) ||
+ bd_get(&mainboard.left_cobroller.bd)) {
+ printf_P(PSTR("MOTOR BLOCKED STOP ALL\r\n"));
+ mainboard.flags &= ~(DO_POWER | DO_ERRBLOCKING);
+ }
+ }
#endif
}
#ifndef HOST_VERSION
&mainboard.angle.cs);
trajectory_set_robot_params(&mainboard.traj, &mainboard.rs, &mainboard.pos);
trajectory_set_speed(&mainboard.traj, SPEED_DIST_FAST, SPEED_ANGLE_FAST); /* d, a */
- trajectory_set_speed(&mainboard.traj, ACC_DIST, ACC_ANGLE); /* d, a */
+ trajectory_set_acc(&mainboard.traj, ACC_DIST, ACC_ANGLE); /* d, a */
/* distance window, angle window, angle start */
trajectory_set_windows(&mainboard.traj, 200., 5.0, 30.);
-import math, sys, time, os, random
+import math, sys, time, os, random, re
from visual import *
+FLOAT = "([-+]?[0-9]*\.?[0-9]+)"
+INT = "([-+]?[0-9][0-9]*)"
+
AREA_X = 3000.
AREA_Y = 2100.
# all positions of robot every 5ms
save_pos = []
-robot = box(pos = (0, 0, ROBOT_HEIGHT/2),
- size = (250,320,ROBOT_HEIGHT),
- color = (0.3, 0.3, 0.3) )
+robot = box(color=(0.4, 0.4, 0.4))
+lspickle = box(color=(0.4, 0.4, 0.4))
+rspickle = box(color=(0.4, 0.4, 0.4))
-last_pos = robot.pos.x, robot.pos.y, robot.pos.z
+last_pos = (0.,0.,0.)
hcenter_line = curve()
hcenter_line.pos = [(-AREA_X/2, 0., 0.3), (AREA_X/2, 0., 0.3)]
vcenter_line = curve()
sq1 = square(250)
sq2 = square(500)
+robot_x = 0.
+robot_y = 0.
+robot_a = 0.
+robot_lspickle = 0
+robot_rspickle = 0
robot_trail = curve()
robot_trail_list = []
max_trail = 500
def toggle_obj_disp():
global area_objects
+ """
+ if area_objects == []:
+ c = sphere(radius=5, color=(0., 0.,1.),
+ pos=(1238.-AREA_X/2, 1313.-AREA_Y/2, 5))
+ area_objects.append(c)
+ c = sphere(radius=5, color=(0., 0.,1.),
+ pos=(1364.-AREA_X/2, 1097.-AREA_Y/2, 5))
+ area_objects.append(c)
+ c = sphere(radius=5, color=(0., 0.,1.),
+ pos=(1453.-AREA_X/2, 1176.-AREA_Y/2, 5))
+ area_objects.append(c)
+ c = sphere(radius=5, color=(0., 0.,1.),
+ pos=(1109.-AREA_X/2, 1050.-AREA_Y/2, 5))
+ area_objects.append(c)
+"""
if area_objects == []:
i = 0
j = 0
o.visible = 1
-def set_robot(x, y, a):
+def set_robot():
global robot, last_pos, robot_trail, robot_trail_list
- global save_pos
+ global save_pos, robot_x, robot_y, robot_a
- robot.pos = (x - AREA_X/2, y - AREA_Y/2, ROBOT_HEIGHT/2)
- robot.axis = (math.cos(a*math.pi/180),
- math.sin(a*math.pi/180),
- 0)
+ axis = (math.cos(robot_a*math.pi/180),
+ math.sin(robot_a*math.pi/180),
+ 0)
+
+ robot.pos = (robot_x - AREA_X/2, robot_y - AREA_Y/2, ROBOT_HEIGHT/2)
+ robot.axis = axis
robot.size = (250, 320, ROBOT_HEIGHT)
+ lspickle.pos = (robot_x - AREA_X/2 + (robot_lspickle*70) * math.cos((robot_a-90)*math.pi/180),
+ robot_y - AREA_Y/2 + (robot_lspickle*70) * math.sin((robot_a-90)*math.pi/180),
+ ROBOT_HEIGHT/2)
+ lspickle.axis = axis
+ lspickle.size = (20, 320, 5)
+
+ rspickle.pos = (robot_x - AREA_X/2 + (robot_rspickle*70) * math.cos((robot_a+90)*math.pi/180),
+ robot_y - AREA_Y/2 + (robot_rspickle*70) * math.sin((robot_a+90)*math.pi/180),
+ ROBOT_HEIGHT/2)
+ rspickle.axis = axis
+ rspickle.size = (20, 320, 5)
+
# save position
- save_pos.append((robot.pos.x, robot.pos, a))
+ save_pos.append((robot.pos.x, robot.pos, robot_a))
pos = robot.pos.x, robot.pos.y, 0.3
if pos != last_pos:
fw = open("/tmp/.robot_dis2sim", "w", 0)
while True:
l = fr.readline()
- try:
- x,y,a = map(lambda x:int(x), l[:-1].split(" "))
- set_robot(x,y,a)
- except ValueError:
- pass
+ m = re.match("pos=%s,%s,%s"%(INT,INT,INT), l)
+ if m:
+ robot_x = int(m.groups()[0])
+ robot_y = int(m.groups()[1])
+ robot_a = int(m.groups()[2])
+ set_robot()
+ m = re.match("ballboard=%s"%(INT), l)
+ if m:
+ print int(m.groups()[0])
+ m = re.match("cobboard=%s"%(INT), l)
+ if m:
+ mode = int(m.groups()[0])
+ if (mode & 0x01) == 0:
+ robot_lspickle = 0
+ elif (mode & 0x02) == 0:
+ robot_lspickle = 1
+ else:
+ robot_lspickle = 2
+ if (mode & 0x04) == 0:
+ robot_rspickle = 0
+ elif (mode & 0x08) == 0:
+ robot_rspickle = 1
+ else:
+ robot_rspickle = 2
if scene.kb.keys == 0:
continue
#include "main.h"
#include "sensor.h"
#include "i2c_protocol.h"
+#ifdef HOST_VERSION
+#include "robotsim.h"
+#endif
#define I2C_STATE_MAX 4
i2c_send_command(uint8_t addr, uint8_t * buf, uint8_t size)
{
#ifdef HOST_VERSION
- return 0;
+ return robotsim_i2c(addr, buf, size);
#else
uint8_t flags;
microseconds us = time_get_us2();
struct i2c_cmd_ballboard_set_mode buf;
buf.hdr.cmd = I2C_CMD_BALLBOARD_SET_MODE;
buf.mode = mode;
- return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+ return i2c_send_command(I2C_BALLBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
}
void do_led_blink(void *dummy)
{
-#if 1 /* simple blink */
- LED1_TOGGLE();
-#endif
+ static uint8_t a = 0;
+
+ if (mainboard.flags & DO_ERRBLOCKING) {
+ if (a & 1)
+ LED1_ON();
+ else
+ LED1_OFF();
+ }
+ else {
+ if (a & 4)
+ LED1_ON();
+ else
+ LED1_OFF();
+ }
+ a++;
}
static void main_timer_interrupt(void)
memset(&gen, 0, sizeof(gen));
memset(&mainboard, 0, sizeof(mainboard));
mainboard.flags = DO_ENCODERS | DO_CS | DO_RS |
- DO_POS | DO_POWER | DO_BD;
+ DO_POS | DO_POWER | DO_BD | DO_ERRBLOCKING;
/* UART */
uart_init();
/* all cs management */
microb_cs_init();
+ /* TIME */
+ time_init(TIME_PRIO);
+
#ifndef HOST_VERSION
/* sensors, will also init hardware adc */
sensor_init();
- /* TIME */
- time_init(TIME_PRIO);
-
/* start i2c slave polling */
scheduler_add_periodical_event_priority(i2c_poll_slaves, NULL,
8000L / SCHEDULER_UNIT, I2C_POLL_PRIO);
/* strat-related event */
scheduler_add_periodical_event_priority(strat_event, NULL,
- 25000L / SCHEDULER_UNIT,
+ 10000L / SCHEDULER_UNIT,
STRAT_PRIO);
#ifndef HOST_VERSION
#define DO_BD 16
#define DO_TIMER 32
#define DO_POWER 64
+#define DO_ERRBLOCKING 128
uint8_t flags; /* misc flags */
/* control systems */
#include <parse.h>
#include <rdline.h>
+#include "../common/i2c_commands.h"
#include "strat.h"
#include "strat_utils.h"
#include "main.h"
char buf[BUFSIZ];
int len;
- len = snprintf(buf, sizeof(buf), "%d %d %d\n",
+ len = snprintf(buf, sizeof(buf), "pos=%d,%d,%d\n",
position_get_x_s16(&mainboard.pos),
position_get_y_s16(&mainboard.pos),
position_get_a_deg_s16(&mainboard.pos));
hostsim_unlock();
}
+static int8_t
+robotsim_i2c_ballboard_set_mode(struct i2c_cmd_ballboard_set_mode *cmd)
+{
+ char buf[BUFSIZ];
+ int len;
+
+ ballboard.mode = cmd->mode;
+ len = snprintf(buf, sizeof(buf), "ballboard=%d\n", cmd->mode);
+ hostsim_lock();
+ write(fdw, buf, len);
+ hostsim_unlock();
+ return 0;
+}
+
+static int8_t
+robotsim_i2c_cobboard_set_mode(struct i2c_cmd_cobboard_set_mode *cmd)
+{
+ char buf[BUFSIZ];
+ int len;
+
+ cobboard.mode = cmd->mode;
+ len = snprintf(buf, sizeof(buf), "cobboard=%d\n", cmd->mode);
+ hostsim_lock();
+ write(fdw, buf, len);
+ hostsim_unlock();
+ return 0;
+}
+
+static int8_t
+robotsim_i2c_ballboard(uint8_t addr, uint8_t *buf, uint8_t size)
+{
+ void *void_cmd = buf;
+
+ switch (buf[0]) {
+ case I2C_CMD_BALLBOARD_SET_MODE:
+ {
+ struct i2c_cmd_ballboard_set_mode *cmd = void_cmd;
+ robotsim_i2c_ballboard_set_mode(cmd);
+ break;
+ }
+
+ default:
+ break;
+ }
+ return 0;
+}
+
+static int8_t
+robotsim_i2c_cobboard(uint8_t addr, uint8_t *buf, uint8_t size)
+{
+ void *void_cmd = buf;
+
+ switch (buf[0]) {
+ case I2C_CMD_COBBOARD_SET_MODE:
+ {
+ struct i2c_cmd_cobboard_set_mode *cmd = void_cmd;
+ robotsim_i2c_cobboard_set_mode(cmd);
+ break;
+ }
+ default:
+ break;
+ }
+ return 0;
+}
+
+int8_t
+robotsim_i2c(uint8_t addr, uint8_t *buf, uint8_t size)
+{
+ if (addr == I2C_BALLBOARD_ADDR)
+ return robotsim_i2c_ballboard(addr, buf, size);
+ else if (addr == I2C_COBBOARD_ADDR)
+ return robotsim_i2c_cobboard(addr, buf, size);
+ return 0;
+}
+
/* must be called periodically */
void robotsim_update(void)
{
-/*
+/*
* Copyright Droids Corporation, Microb Technology, Eirbot (2005)
- *
+ *
* 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
*
*/
+int8_t robotsim_i2c(uint8_t addr, uint8_t *buf, uint8_t size);
void robotsim_update(void);
void robotsim_pwm(void *arg, int32_t val);
int32_t robotsim_encoder_get(void *arg);
-/*
+/*
* Copyright Droids, Microb Technology (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: strat.c,v 1.6 2009-11-08 17:24:33 zer0 Exp $
*
- * Olivier MATZ <zer0@droids-corp.org>
+ * Olivier MATZ <zer0@droids-corp.org>
*/
#include <stdio.h>
.conf = {
.flags = 0,
},
-
+ /* status */
+ .status = {
+ .flags = 0,
+ },
};
/*************************************************************/
interrupt_traj_reset();
/* used in strat_base for END_TIMER */
- mainboard.flags = DO_ENCODERS | DO_CS | DO_RS |
+ mainboard.flags = DO_ENCODERS | DO_CS | DO_RS |
DO_POS | DO_BD | DO_TIMER | DO_POWER;
}
/* called periodically */
void strat_event(void *dummy)
{
+#if 0
+ /* pack or deploy spickle */
+ if (strat_infos.status.flags & STRAT_STATUS_LHARVEST) {
+ if (sensor_get(S_LCOB_PRESENT)) {
+ if (sensor_get(S_LCOB_WHITE))
+ i2c_ballboard_set_mode();
+ else
+ ;
+ }
+ }
+#endif
/* limit speed when opponent is close */
strat_limit_speed();
}
{
uint8_t err;
- /* do static cols + first temple */
+ /* */
err = strat_beginning();
return err;
/* strat infos structures */
-struct bbox {
+struct strat_bbox {
int32_t x1;
int32_t y1;
int32_t x2;
int32_t y2;
};
-struct conf {
+struct strat_conf {
#define STRAT_CONF_XXX 0x01
uint8_t flags;
};
+struct strat_status {
+#define STRAT_STATUS_LHARVEST 0x01
+#define STRAT_STATUS_RHARVEST 0x02
+ uint8_t flags;
+};
+
/* all infos related to strat */
struct strat_infos {
uint8_t dump_enabled;
- struct conf conf;
- struct bbox area_bbox;
+ struct strat_conf conf;
+ struct strat_bbox area_bbox;
+ struct strat_status status;
};
extern struct strat_infos strat_infos;
git.droids-corp.org / aversive.git / commitdiff