* p argument is the crossing point coordinates (dummy for 0 or 2
* result)
*/
-uint8_t
+uint8_t
intersect_line(const line_t *l1, const line_t *l2, point_t *p)
-{
+{
double tmp1, tmp2;
- debug_printf("l1:%2.2f,%2.2f,%2.2f l2:%2.2f,%2.2f,%2.2f\n",
+ debug_printf("l1:%2.2f,%2.2f,%2.2f l2:%2.2f,%2.2f,%2.2f\n",
l1->a, l1->b, l1->c, l2->a, l2->b, l2->c);
/* if dummy lines */
if ((l1->a == 0 && l1->b == 0) || (l2->a == 0 && l2->b == 0))
return 0;
-
+
if (l1->a == 0) {
if (l2->a == 0) {
if (l1->b*l2->c == l2->b*l1->c)
return 2;
return 0;
}
-
+
/* by + c = 0
* a'x + b'y + c' = 0 */
/*
p->x = -(l2->b*(-l1->c) + l2->c*l1->b)/(l2->a*l1->b);
return 1;
}
-
+
if (l1->b == 0) {
if (l2->b == 0) {
if (l1->a*l2->c == l2->a*l1->c)
p1y = p1->y;
p2x = p2->x;
p2y = p2->y;
-
+
l->a = -(p2y - p1y);
l->b = (p2x - p1x);
l->c = -(l->a * p1x + l->b * p1y);
-
+
debug_printf("%s: %2.2f, %2.2f, %2.2f\r\n",
__FUNCTION__, l->a, l->b, l->c);
}
/* return values:
* 0 dont cross
- * 1 cross
+ * 1 cross
* 2 cross on point
* 3 parallel and one point in
*
* p argument is the crossing point coordinates (dummy for 0 1 or 3
* result)
*/
-uint8_t
-intersect_segment(const point_t *s1, const point_t *s2,
- const point_t *t1, const point_t *t2,
+uint8_t
+intersect_segment(const point_t *s1, const point_t *s2,
+ const point_t *t1, const point_t *t2,
point_t *p)
{
line_t l1, l2;
*p = *s2;
return 2;
}
-
+
debug_printf("px=%" PRIi32 " py=%" PRIi32 "\n", p->x, p->y);
/* Consider as parallel if intersection is too far */
return 1;
}
+
+void line_translate(line_t *l, vect_t *v)
+{
+ l->c -= (l->a * v->x + l->b * v->y);
+}
const point_t *t1, const point_t *t2,
point_t *p);
+/* translate the line */
+void
+line_translate(line_t *l, vect_t *v);
#endif /* _LINES_H_ */
#include <uart.h>
#include <uart_private.h>
+#include <fcntl.h>
+
/* this file os a stub for host */
void uart_init(void)
int uart_recv(uint8_t num)
{
+ fcntl(0, F_SETFL, 0);
return getchar();
}
-/* XXX should not wait */
int uart_recv_nowait(uint8_t num)
{
+ fcntl(0, F_SETFL, O_NONBLOCK);
return getchar();
}
#include <aversive.h>
#include <quadramp.h>
-#define NEXT(n, i) (((n) + (i)/(n)) >> 1)
-
-void quadramp_init(struct quadramp_filter * q)
+void quadramp_init(struct quadramp_filter *q)
{
uint8_t flags;
IRQ_LOCK(flags);
}
-void quadramp_reset(struct quadramp_filter * q)
+void quadramp_reset(struct quadramp_filter *q)
{
q->previous_var = 0;
q->previous_out = 0;
q->previous_in = 0;
}
-void quadramp_set_2nd_order_vars(struct quadramp_filter * q,
- uint32_t var_2nd_ord_pos,
- uint32_t var_2nd_ord_neg)
+void quadramp_set_2nd_order_vars(struct quadramp_filter *q,
+ double var_2nd_ord_pos,
+ double var_2nd_ord_neg)
{
uint8_t flags;
IRQ_LOCK(flags);
IRQ_UNLOCK(flags);
}
-void quadramp_set_1st_order_vars(struct quadramp_filter * q,
- uint32_t var_1st_ord_pos,
- uint32_t var_1st_ord_neg)
+void quadramp_set_1st_order_vars(struct quadramp_filter *q,
+ double var_1st_ord_pos,
+ double var_1st_ord_neg)
{
uint8_t flags;
IRQ_LOCK(flags);
int32_t quadramp_do_filter(void * data, int32_t in)
{
struct quadramp_filter * q = data;
- int32_t d ;
+ int32_t d; /* remaining distance */
int32_t pos_target;
- int32_t var_1st_ord_pos = 0;
- int32_t var_1st_ord_neg = 0;
- int32_t var_2nd_ord_pos = 0;
- int32_t var_2nd_ord_neg = 0;
- int32_t previous_var, previous_out ;
-
- if ( q->var_1st_ord_pos )
- var_1st_ord_pos = q->var_1st_ord_pos ;
-
- if ( q->var_1st_ord_neg )
- var_1st_ord_neg = -q->var_1st_ord_neg ;
-
- if ( q->var_2nd_ord_pos )
- var_2nd_ord_pos = q->var_2nd_ord_pos ;
-
- if ( q->var_2nd_ord_neg )
- var_2nd_ord_neg = -q->var_2nd_ord_neg ;
+ double var_1st_ord_pos = q->var_1st_ord_pos;
+ double var_1st_ord_neg = -q->var_1st_ord_neg;
+ double var_2nd_ord_pos = q->var_2nd_ord_pos;
+ double var_2nd_ord_neg = -q->var_2nd_ord_neg;
+ double previous_var, d_float;
+ int32_t previous_out;
previous_var = q->previous_var;
previous_out = q->previous_out;
d = in - previous_out ;
+ d_float = d;
/* Deceleration ramp */
- if ( d > 0 && var_2nd_ord_neg) {
- int32_t ramp_pos;
+ if (d > 0 && var_2nd_ord_neg) {
+ double ramp_pos;
/* var_2nd_ord_neg < 0 */
- /* real EQ : sqrt( var_2nd_ord_neg^2/4 - 2.d.var_2nd_ord_neg ) + var_2nd_ord_neg/2 */
- ramp_pos = sqrt( (var_2nd_ord_neg*var_2nd_ord_neg)/4 - 2*d*var_2nd_ord_neg ) + var_2nd_ord_neg/2;
+ ramp_pos = sqrt((var_2nd_ord_neg*var_2nd_ord_neg)/4 -
+ 2*d_float*var_2nd_ord_neg) +
+ var_2nd_ord_neg/2;
- if(ramp_pos < var_1st_ord_pos)
- var_1st_ord_pos = ramp_pos ;
+ if (ramp_pos < var_1st_ord_pos)
+ var_1st_ord_pos = ramp_pos;
}
else if (d < 0 && var_2nd_ord_pos) {
- int32_t ramp_neg;
+ double ramp_neg;
/* var_2nd_ord_pos > 0 */
- /* real EQ : sqrt( var_2nd_ord_pos^2/4 - 2.d.var_2nd_ord_pos ) - var_2nd_ord_pos/2 */
- ramp_neg = -sqrt( (var_2nd_ord_pos*var_2nd_ord_pos)/4 - 2*d*var_2nd_ord_pos ) - var_2nd_ord_pos/2;
+ ramp_neg = -sqrt( (var_2nd_ord_pos*var_2nd_ord_pos)/4 -
+ 2*d_float*var_2nd_ord_pos ) -
+ var_2nd_ord_pos/2;
/* ramp_neg < 0 */
- if(ramp_neg > var_1st_ord_neg)
+ if (ramp_neg > var_1st_ord_neg)
var_1st_ord_neg = ramp_neg ;
}
/* try to set the speed : can we reach the speed with our acceleration ? */
/* si on va moins vite que la Vmax */
- if ( previous_var < var_1st_ord_pos ) {
+ if (previous_var < var_1st_ord_pos) {
/* acceleration would be to high, we reduce the speed */
/* si rampe acceleration active ET qu'on ne peut pas atteindre Vmax,
* on sature Vmax a Vcourante + acceleration */
- if (var_2nd_ord_pos && ( var_1st_ord_pos - previous_var > var_2nd_ord_pos) )
- var_1st_ord_pos = previous_var + var_2nd_ord_pos ;
+ if (var_2nd_ord_pos &&
+ (var_1st_ord_pos - previous_var) > var_2nd_ord_pos)
+ var_1st_ord_pos = previous_var + var_2nd_ord_pos;
}
/* si on va plus vite que Vmax */
- else if ( previous_var > var_1st_ord_pos ) {
+ else if (previous_var > var_1st_ord_pos) {
/* deceleration would be to high, we increase the speed */
/* si rampe deceleration active ET qu'on ne peut pas atteindre Vmax,
* on sature Vmax a Vcourante + deceleration */
- if (var_2nd_ord_neg && ( var_1st_ord_pos - previous_var < var_2nd_ord_neg) )
+ if (var_2nd_ord_neg &&
+ (var_1st_ord_pos - previous_var) < var_2nd_ord_neg)
var_1st_ord_pos = previous_var + var_2nd_ord_neg;
}
/* same for the neg */
/* si on va plus vite que la Vmin (en negatif : en vrai la vitesse absolue est inferieure) */
- if ( previous_var > var_1st_ord_neg ) {
+ if (previous_var > var_1st_ord_neg) {
/* acceleration would be to high, we reduce the speed */
/* si rampe deceleration active ET qu'on ne peut pas atteindre Vmin,
* on sature Vmax a Vcourante + deceleration */
- if (var_2nd_ord_neg && ( var_1st_ord_neg - previous_var < var_2nd_ord_neg) )
- var_1st_ord_neg = previous_var + var_2nd_ord_neg ;
+ if (var_2nd_ord_neg &&
+ (var_1st_ord_neg - previous_var) < var_2nd_ord_neg)
+ var_1st_ord_neg = previous_var + var_2nd_ord_neg;
}
/* si on va moins vite que Vmin (mais vitesse absolue superieure) */
- else if ( previous_var < var_1st_ord_neg ) {
+ else if (previous_var < var_1st_ord_neg) {
/* deceleration would be to high, we increase the speed */
/* si rampe acceleration active ET qu'on ne peut pas atteindre Vmin,
* on sature Vmax a Vcourante + deceleration */
- if (var_2nd_ord_pos && (var_1st_ord_neg - previous_var > var_2nd_ord_pos) )
+ if (var_2nd_ord_pos &&
+ (var_1st_ord_neg - previous_var) > var_2nd_ord_pos)
var_1st_ord_neg = previous_var + var_2nd_ord_pos;
}
/*
* Position consign : can we reach the position with our speed ?
*/
- if ( /* var_1st_ord_pos && */d > var_1st_ord_pos ) {
+ if (d_float > var_1st_ord_pos) {
pos_target = previous_out + var_1st_ord_pos ;
previous_var = var_1st_ord_pos ;
}
- else if ( /* var_1st_ord_neg && */d < var_1st_ord_neg ) {
+ else if (d_float < var_1st_ord_neg) {
pos_target = previous_out + var_1st_ord_neg ;
previous_var = var_1st_ord_neg ;
}
else {
pos_target = previous_out + d ;
- previous_var = d ;
+ previous_var = d_float ;
}
// update previous_out and previous_var
q->previous_out = pos_target;
q->previous_in = in;
- return pos_target ;
+ return pos_target;
}
struct quadramp_filter
{
- uint32_t var_2nd_ord_pos;
- uint32_t var_2nd_ord_neg;
- uint32_t var_1st_ord_pos;
- uint32_t var_1st_ord_neg;
+ double var_2nd_ord_pos;
+ double var_2nd_ord_neg;
+ double var_1st_ord_pos;
+ double var_1st_ord_neg;
- int32_t previous_var;
+ double previous_var;
int32_t previous_out;
int32_t previous_in;
};
void quadramp_reset(struct quadramp_filter *q);
void quadramp_set_2nd_order_vars(struct quadramp_filter *q,
- uint32_t var_2nd_ord_pos,
- uint32_t var_2nd_ord_neg);
+ double var_2nd_ord_pos,
+ double var_2nd_ord_neg);
void quadramp_set_1st_order_vars(struct quadramp_filter *q,
- uint32_t var_1st_ord_pos,
- uint32_t var_1st_ord_neg);
+ double var_1st_ord_pos,
+ double var_1st_ord_neg);
/**
* Return 1 when (filter_input == filter_output && 1st_ord variation
/************ INIT FUNCS */
/** structure initialization */
-void trajectory_init(struct trajectory *traj)
+void trajectory_init(struct trajectory *traj, double cs_hz)
{
uint8_t flags;
IRQ_LOCK(flags);
memset(traj, 0, sizeof(struct trajectory));
+ traj->cs_hz = cs_hz;
traj->state = READY;
traj->scheduler_task = -1;
IRQ_UNLOCK(flags);
}
/** set speed consign */
-void trajectory_set_speed(struct trajectory *traj, int16_t d_speed, int16_t a_speed)
+void trajectory_set_speed(struct trajectory *traj, double d_speed, double a_speed)
{
uint8_t flags;
IRQ_LOCK(flags);
IRQ_UNLOCK(flags);
}
+/** set acc consign */
+void trajectory_set_acc(struct trajectory *traj, double d_acc, double a_acc)
+{
+ uint8_t flags;
+ IRQ_LOCK(flags);
+ traj->d_acc = d_acc;
+ traj->a_acc = a_acc;
+ IRQ_UNLOCK(flags);
+}
+
/** set windows for trajectory */
void trajectory_set_windows(struct trajectory *traj, double d_win,
double a_win_deg, double a_start_deg)
/* line */
RUNNING_LINE,
+
+ /* clitoid */
+ RUNNING_CLITOID_LINE,
+ RUNNING_CLITOID_CURVE,
};
struct circle_target {
uint8_t flags; /**< flags for this trajectory */
};
+/* for line and clitoid */
struct line_target {
line_t line;
double angle;
double advance;
+
+ /* only for clitoid */
+ point_t turn_pt;
+ double Aa;
+ double Va;
+ double alpha;
+ double R;
};
struct trajectory {
* when a_target < a_start */
double circle_coef;/**<< corrective circle coef */
- uint16_t d_speed; /**<< distance speed consign */
- uint16_t a_speed; /**<< angle speed consign */
+ double d_speed; /**<< distance speed consign */
+ double a_speed; /**<< angle speed consign */
+
+ double d_acc; /**<< distance acceleration consign */
+ double a_acc; /**<< angle acceleration consign */
struct robot_position *position; /**<< associated robot_position */
struct robot_system *robot; /**<< associated robot_system */
struct cs *csm_angle; /**<< associated control system (angle) */
struct cs *csm_distance; /**<< associated control system (distance) */
+ double cs_hz;
+
int8_t scheduler_task; /**<< id of current task (-1 if no running task) */
};
/** structure initialization */
-void trajectory_init(struct trajectory *traj);
+void trajectory_init(struct trajectory *traj, double cs_hz);
/** structure initialization */
void trajectory_set_cs(struct trajectory *traj, struct cs *cs_d,
struct robot_position *pos) ;
/** set speed consign */
-void trajectory_set_speed(struct trajectory *traj, int16_t d_speed, int16_t a_speed);
+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);
/**
* set windows for trajectory.
* position consign (after quadramp filter), for angle and
* distance. */
uint8_t trajectory_finished(struct trajectory *traj);
+uint8_t trajectory_angle_finished(struct trajectory *traj);
+uint8_t trajectory_distance_finished(struct trajectory *traj);
/** return true if traj is nearly finished depending on specified
* parameters */
} \
} while (0)
+static void start_clitoid(struct trajectory *traj);
/**
* update angle and/or distance
/************ FUNCS FOR GETTING TRAJ STATE */
+uint8_t trajectory_angle_finished(struct trajectory *traj)
+{
+ return cs_get_consign(traj->csm_angle) ==
+ cs_get_filtered_consign(traj->csm_angle);
+}
+
+uint8_t trajectory_distance_finished(struct trajectory *traj)
+{
+ if (traj->state == RUNNING_CLITOID_CURVE)
+ return 1;
+
+ return cs_get_consign(traj->csm_distance) ==
+ cs_get_filtered_consign(traj->csm_distance) ;
+}
+
/** return true if the position consign is equal to the filtered
* position consign (after quadramp filter), for angle and
* distance. */
uint8_t trajectory_finished(struct trajectory *traj)
{
- return cs_get_consign(traj->csm_angle) == cs_get_filtered_consign(traj->csm_angle) &&
- cs_get_consign(traj->csm_distance) == cs_get_filtered_consign(traj->csm_distance) ;
+ return trajectory_angle_finished(traj) &&
+ trajectory_distance_finished(traj);
}
/** return true if traj is nearly finished */
cs_set_consign(traj->csm_angle, a_consign);
cs_set_consign(traj->csm_distance, d_consign);
+
+ /* we reached dest, start clitoid */
+ if (traj->state == RUNNING_CLITOID_LINE &&
+ xy_norm(proj.x,
+ proj.y,
+ traj->target.line.turn_pt.x,
+ traj->target.line.turn_pt.y) <
+ xy_norm(proj.x + cos(traj->target.line.angle),
+ proj.y + sin(traj->target.line.angle),
+ traj->target.line.turn_pt.x,
+ traj->target.line.turn_pt.y)) {
+ start_clitoid(traj);
+ }
}
break;
case RUNNING_LINE:
+ case RUNNING_CLITOID_LINE:
trajectory_manager_line_event(traj);
break;
/*********** *LINE */
/* Follow a line */
-void trajectory_line_abs(struct trajectory *traj,
- double x1, double y1,
- double x2, double y2,
- double advance)
+static void __trajectory_line_abs(struct trajectory *traj,
+ double x1, double y1,
+ double x2, double y2,
+ double advance)
{
point_t p1, p2;
- delete_event(traj);
-
/* find the line EQ */
p1.x = x1;
p1.y = y1;
traj->target.line.line.c,
traj->target.line.angle);
+}
+
+/* Follow a line */
+void trajectory_line_abs(struct trajectory *traj,
+ double x1, double y1,
+ double x2, double y2,
+ double advance)
+{
+ delete_event(traj);
+ __trajectory_line_abs(traj, x1, y1, x2, y2, advance);
traj->state = RUNNING_LINE;
trajectory_manager_event(traj);
schedule_event(traj);
}
+/*** CLOTHOID */
+
+/**
+ * process clitoid parameters
+ *
+ * - 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
+ * - 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,
+ 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 *Aa_out, double *Va_out, double *remain_d_mm_out)
+{
+ 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;
+ point_t robot, intersect, pt2, center, proj;
+ vect_t v;
+
+ /* param check */
+ if (fabs(alpha_deg) <= fabs(beta_deg)) {
+ DEBUG(E_TRAJECTORY, "alpha is smaller than beta");
+ return -1;
+ }
+
+ /* get angular speed Va */
+ Vd_mm_s = speed_imp2mm(traj, Vd);
+ DEBUG(E_TRAJECTORY, "Vd_mm_s=%2.2f", Vd_mm_s);
+ Va_rd_s = Vd_mm_s / R_mm;
+ Va = speed_rd2imp(traj, Va_rd_s);
+ DEBUG(E_TRAJECTORY, "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_TRAJECTORY, "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 = acc_rd2imp(traj, Aa_rd_s2);
+ DEBUG(E_TRAJECTORY, "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_TRAJECTORY, "greater than max acceleration");
+ 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;
+ 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_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.;
+ DEBUG(E_TRAJECTORY, "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_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);
+
+ /* process remaining distance before start turning */
+ remain_d_mm = d_inter_mm - (pt_norm(&proj, &intersect) + d_mm);
+ DEBUG(E_TRAJECTORY, "remain_d=%2.2f", remain_d_mm);
+ if (remain_d_mm < 0) {
+ DEBUG(E_TRAJECTORY, "too late, cannot turn");
+ return -1;
+ }
+
+ /* return result */
+ *Aa_out = Aa;
+ *Va_out = Va;
+ *remain_d_mm_out = remain_d_mm;
+ return 0;
+}
+
+static void start_clitoid(struct trajectory *traj)
+{
+ double Aa = traj->target.line.Aa;
+ double Va = traj->target.line.Va;
+ double a_rad = traj->target.line.alpha;
+ double R_mm = traj->target.line.R;
+ double d;
+
+ delete_event(traj);
+ 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 */
+ trajectory_d_a_rel(traj, d, DEG(a_rad));
+}
+
+
+/**
+ * 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 Vd, double d_inter_mm)
+{
+ double remain = 0, Aa = 0, Va = 0;
+ double turnx, turny;
+
+ if (calc_clitoid(traj, x, y, a, alpha_deg, beta_deg, R_mm,
+ Vd, traj->a_acc, d_inter_mm,
+ &Aa, &Va, &remain) < 0)
+ return -1;
+
+ delete_event(traj);
+ turnx = x + cos(a) * remain;
+ turny = y + sin(a) * remain;
+ traj->target.line.Aa = Aa;
+ traj->target.line.Va = Va;
+ traj->target.line.alpha = RAD(alpha_deg);
+ traj->target.line.R = R_mm;
+ traj->target.line.turn_pt.x = turnx;
+ traj->target.line.turn_pt.y = turny;
+ __trajectory_line_abs(traj, x, y, turnx, turny,
+ advance);
+ traj->state = RUNNING_CLITOID_LINE;
+ trajectory_manager_event(traj);
+ schedule_event(traj);
+ return 0;
+}
#include "trajectory_manager_core.h"
/** set speed consign in quadramp filter */
-void set_quadramp_speed(struct trajectory *traj, int16_t d_speed, int16_t a_speed)
+void set_quadramp_speed(struct trajectory *traj, double d_speed, double a_speed)
{
struct quadramp_filter * q_d, * q_a;
q_d = traj->csm_distance->consign_filter_params;
}
/** get angle speed consign in quadramp filter */
-uint32_t get_quadramp_angle_speed(struct trajectory *traj)
+double get_quadramp_angle_speed(struct trajectory *traj)
{
struct quadramp_filter *q_a;
q_a = traj->csm_angle->consign_filter_params;
}
/** get distance speed consign in quadramp filter */
-uint32_t get_quadramp_distance_speed(struct trajectory *traj)
+double get_quadramp_distance_speed(struct trajectory *traj)
{
struct quadramp_filter *q_d;
q_d = traj->csm_distance->consign_filter_params;
return q_d->var_1st_ord_pos;
}
+/** set speed consign in quadramp filter */
+void set_quadramp_acc(struct trajectory *traj, double d_acc, double a_acc)
+{
+ struct quadramp_filter * q_d, * q_a;
+ q_d = traj->csm_distance->consign_filter_params;
+ q_a = traj->csm_angle->consign_filter_params;
+ quadramp_set_2nd_order_vars(q_d, ABS(d_acc), ABS(d_acc));
+ quadramp_set_2nd_order_vars(q_a, ABS(a_acc), ABS(a_acc));
+}
+
/** remove event if any */
void delete_event(struct trajectory *traj)
{
set_quadramp_speed(traj, traj->d_speed, traj->a_speed);
+ set_quadramp_acc(traj, traj->d_acc, traj->a_acc);
if ( traj->scheduler_task != -1) {
DEBUG(E_TRAJECTORY, "Delete event");
scheduler_del_event(traj->scheduler_task);
a *= 2.;
return ABS(a) < a_win_rad;
}
+
+/* distance unit conversions */
+
+double pos_mm2imp(struct trajectory *traj, double pos)
+{
+ return pos * traj->position->phys.distance_imp_per_mm;
+}
+
+double pos_imp2mm(struct trajectory *traj, double pos)
+{
+ return pos / traj->position->phys.distance_imp_per_mm;
+}
+
+double speed_mm2imp(struct trajectory *traj, double speed)
+{
+ return speed *
+ traj->position->phys.distance_imp_per_mm /
+ traj->cs_hz;
+}
+
+double speed_imp2mm(struct trajectory *traj, double speed)
+{
+ return speed * traj->cs_hz /
+ traj->position->phys.distance_imp_per_mm;
+}
+
+double acc_mm2imp(struct trajectory *traj, double acc)
+{
+ return acc * traj->position->phys.distance_imp_per_mm /
+ (traj->cs_hz * traj->cs_hz);
+}
+
+double acc_imp2mm(struct trajectory *traj, double acc)
+{
+ return acc * traj->cs_hz * traj->cs_hz /
+ traj->position->phys.distance_imp_per_mm;
+}
+
+/* angle unit conversions */
+
+double pos_rd2imp(struct trajectory *traj, double pos)
+{
+ return pos *
+ (traj->position->phys.distance_imp_per_mm *
+ traj->position->phys.track_mm / 2.);
+}
+
+double pos_imp2rd(struct trajectory *traj, double pos)
+{
+ return pos /
+ (traj->position->phys.distance_imp_per_mm *
+ traj->position->phys.track_mm / 2.);
+}
+
+double speed_rd2imp(struct trajectory *traj, double speed)
+{
+ return speed *
+ (traj->position->phys.distance_imp_per_mm *
+ traj->position->phys.track_mm /
+ (2. * traj->cs_hz));
+}
+
+double speed_imp2rd(struct trajectory *traj, double speed)
+{
+ return speed /
+ (traj->position->phys.distance_imp_per_mm *
+ traj->position->phys.track_mm /
+ (2. * traj->cs_hz));
+}
+
+double acc_rd2imp(struct trajectory *traj, double acc)
+{
+ return acc *
+ (traj->position->phys.distance_imp_per_mm *
+ traj->position->phys.track_mm /
+ (2. * traj->cs_hz * traj->cs_hz));
+}
+
+double acc_imp2rd(struct trajectory *traj, double acc)
+{
+ return acc /
+ (traj->position->phys.distance_imp_per_mm *
+ traj->position->phys.track_mm /
+ (2. * traj->cs_hz * traj->cs_hz));
+}
+
#define TRAJ_EVT_PERIOD (25000UL/SCHEDULER_UNIT)
/** set speed consign in quadramp filter */
-void set_quadramp_speed(struct trajectory *traj, int16_t d_speed, int16_t a_speed);
+void set_quadramp_speed(struct trajectory *traj, double d_speed, double a_speed);
+
+/** set acc consign in quadramp filter */
+void set_quadramp_acc(struct trajectory *traj, double d_acc, double a_acc);
/** get angle speed consign in quadramp filter */
-uint32_t get_quadramp_angle_speed(struct trajectory *traj);
+double get_quadramp_angle_speed(struct trajectory *traj);
/** get distance speed consign in quadramp filter */
-uint32_t get_quadramp_distance_speed(struct trajectory *traj);
+double get_quadramp_distance_speed(struct trajectory *traj);
/** remove event if any */
void delete_event(struct trajectory *traj);
* traj->target.pol.angle is set (i.e. an angle command, not an xy
* command) */
uint8_t is_robot_in_angle_window(struct trajectory *traj, double a_win_rad);
+
+double pos_mm2imp(struct trajectory *traj, double pos);
+double pos_imp2mm(struct trajectory *traj, double pos);
+double speed_mm2imp(struct trajectory *traj, double speed);
+double speed_imp2mm(struct trajectory *traj, double speed);
+double acc_mm2imp(struct trajectory *traj, double acc);
+double acc_imp2mm(struct trajectory *traj, double acc);
+double pos_rd2imp(struct trajectory *traj, double pos);
+double pos_imp2rd(struct trajectory *traj, double pos);
+double speed_rd2imp(struct trajectory *traj, double speed);
+double speed_imp2rd(struct trajectory *traj, double speed);
+double acc_rd2imp(struct trajectory *traj, double acc);
+double acc_imp2rd(struct trajectory *traj, double acc);
/* this structure is filled when cmd_quadramp is parsed successfully */
struct cmd_quadramp_result {
struct cmd_cs_result cs;
- uint32_t ap;
- uint32_t an;
- uint32_t sp;
- uint32_t sn;
+ double ap;
+ double an;
+ double sp;
+ double sn;
};
/* function called when cmd_quadramp is parsed successfully */
static void cmd_quadramp_parsed(void *parsed_result, void *show)
{
struct cmd_quadramp_result * res = parsed_result;
-
+
struct cs_block *csb;
csb = cs_from_name(res->cs.csname);
quadramp_set_2nd_order_vars(&csb->qr, res->ap, res->an);
}
- printf_P(PSTR("quadramp %s %ld %ld %ld %ld\r\n"),
+ printf_P(PSTR("quadramp %s %2.2f %2.2f %2.2f %2.2f\r\n"),
res->cs.csname,
csb->qr.var_2nd_ord_pos,
csb->qr.var_2nd_ord_neg,
prog_char str_quadramp_arg0[] = "quadramp";
parse_pgm_token_string_t cmd_quadramp_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_quadramp_result, cs.cmdname, str_quadramp_arg0);
-parse_pgm_token_num_t cmd_quadramp_ap = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, ap, UINT32);
-parse_pgm_token_num_t cmd_quadramp_an = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, an, UINT32);
-parse_pgm_token_num_t cmd_quadramp_sp = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sp, UINT32);
-parse_pgm_token_num_t cmd_quadramp_sn = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sn, UINT32);
+parse_pgm_token_num_t cmd_quadramp_ap = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, ap, FLOAT);
+parse_pgm_token_num_t cmd_quadramp_an = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, an, FLOAT);
+parse_pgm_token_num_t cmd_quadramp_sp = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sp, FLOAT);
+parse_pgm_token_num_t cmd_quadramp_sn = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sn, FLOAT);
prog_char help_quadramp[] = "Set quadramp values (acc+, acc-, speed+, speed-)";
parse_pgm_inst_t cmd_quadramp = {
.data = NULL, /* 2nd arg of func */
.help_str = help_quadramp,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_quadramp_arg0,
- (prog_void *)&cmd_csb_name_tok,
- (prog_void *)&cmd_quadramp_ap,
- (prog_void *)&cmd_quadramp_an,
- (prog_void *)&cmd_quadramp_sp,
- (prog_void *)&cmd_quadramp_sn,
-
+ (prog_void *)&cmd_quadramp_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_quadramp_ap,
+ (prog_void *)&cmd_quadramp_an,
+ (prog_void *)&cmd_quadramp_sp,
+ (prog_void *)&cmd_quadramp_sn,
+
NULL,
},
};
/* PID */
pid_init(&ballboard.roller.pid);
pid_set_gains(&ballboard.roller.pid, 80, 80, 250);
- pid_set_maximums(&ballboard.roller.pid, 0, 10000, 2000);
+ pid_set_maximums(&ballboard.roller.pid, 0, 10000, 4095);
pid_set_out_shift(&ballboard.roller.pid, 6);
pid_set_derivate_filter(&ballboard.roller.pid, 6);
# error not supported
#endif
- eeprom_write_byte(EEPROM_MAGIC_ADDRESS, EEPROM_MAGIC_BALLBOARD);
+ //eeprom_write_byte(EEPROM_MAGIC_ADDRESS, EEPROM_MAGIC_BALLBOARD);
/* check eeprom to avoid to run the bad program */
if (eeprom_read_byte(EEPROM_MAGIC_ADDRESS) !=
EEPROM_MAGIC_BALLBOARD) {
PWM_NG_TIMER_16BITS_INIT(4, TIMER_16_MODE_PWM_10,
TIMER4_PRESCALER_DIV_1);
- PWM_NG_INIT16(&gen.pwm1_4A, 4, A, 10, PWM_NG_MODE_SIGNED |
- PWM_NG_MODE_SIGN_INVERTED,
+ PWM_NG_INIT16(&gen.pwm1_4A, 4, A, 10, PWM_NG_MODE_SIGNED,
&PORTD, 4);
PWM_NG_INIT16(&gen.pwm2_4B, 4, B, 10, PWM_NG_MODE_SIGNED,
&PORTD, 5);
-/*
+/*
* 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
//#define COBROLLER_SPEED 400
#define SERVO_DOOR_OPEN 260
-#define SERVO_DOOR_CLOSED 490
+#define SERVO_DOOR_CLOSED 510
+#define SERVO_DOOR_BLOCK 510
-#define SERVO_CARRY_L_OPEN 280
-#define SERVO_CARRY_L_CLOSED 510
+#define SERVO_CARRY_L_OPEN 295
+#define SERVO_CARRY_L_CLOSED 400 // 510
-#define SERVO_CARRY_R_OPEN 470
-#define SERVO_CARRY_R_CLOSED 250
+#define SERVO_CARRY_R_OPEN 455
+#define SERVO_CARRY_R_CLOSED 350 // 250
void actuator_init(void);
pwm_ng_set(SERVO_DOOR_PWM, SERVO_DOOR_CLOSED);
}
-void left_cobroller_on(void)
-{
- cobboard.left_cobroller_speed = COBROLLER_SPEED;
-}
-
-void right_cobroller_on(void)
+void servo_door_block(void)
{
- cobboard.right_cobroller_speed = COBROLLER_SPEED;
+ pwm_ng_set(SERVO_DOOR_PWM, SERVO_DOOR_BLOCK);
}
-void left_cobroller_off(void)
+void cobroller_on(uint8_t side)
{
- cobboard.left_cobroller_speed = 0;
+ if (side == I2C_LEFT_SIDE)
+ cobboard.left_cobroller_speed = COBROLLER_SPEED;
+ else
+ cobboard.right_cobroller_speed = -COBROLLER_SPEED;
}
-void right_cobroller_off(void)
+void cobroller_off(uint8_t side)
{
- cobboard.right_cobroller_speed = 0;
+ if (side == I2C_LEFT_SIDE)
+ cobboard.left_cobroller_speed = 0;
+ else
+ cobboard.right_cobroller_speed = 0;
}
void actuator_init(void)
void servo_carry_open(void);
void servo_carry_close(void);
+
void servo_door_open(void);
void servo_door_close(void);
+void servo_door_block(void);
-void left_cobroller_on(void);
-void right_cobroller_on(void);
-void left_cobroller_off(void);
-void right_cobroller_off(void);
+void cobroller_on(uint8_t side);
+void cobroller_off(uint8_t side);
#endif
servo_door_open();
else if (!strcmp_P(res->arg1, PSTR("closed")))
servo_door_close();
+ else if (!strcmp_P(res->arg1, PSTR("block")))
+ servo_door_close();
}
prog_char str_servo_door_arg0[] = "door";
parse_pgm_token_string_t cmd_servo_door_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_servo_door_result, arg0, str_servo_door_arg0);
-prog_char str_servo_door_arg1[] = "open#closed";
+prog_char str_servo_door_arg1[] = "open#closed#block";
parse_pgm_token_string_t cmd_servo_door_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_servo_door_result, arg1, str_servo_door_arg1);
prog_char help_servo_door[] = "Servo door function";
__attribute__((unused)) void *data)
{
struct cmd_cobroller_result *res = parsed_result;
+ uint8_t side;
- if (!strcmp_P(res->arg1, PSTR("left"))) {
- if (!strcmp_P(res->arg2, PSTR("on")))
- left_cobroller_on();
- else if (!strcmp_P(res->arg2, PSTR("off")))
- left_cobroller_off();
- }
- else if (!strcmp_P(res->arg1, PSTR("right"))) {
- if (!strcmp_P(res->arg2, PSTR("on")))
- right_cobroller_on();
- else if (!strcmp_P(res->arg2, PSTR("off")))
- right_cobroller_off();
- }
+ if (!strcmp_P(res->arg1, PSTR("left")))
+ side = I2C_LEFT_SIDE;
+ else
+ side = I2C_RIGHT_SIDE;
+
+ if (!strcmp_P(res->arg2, PSTR("on")))
+ cobroller_on(side);
+ else if (!strcmp_P(res->arg2, PSTR("off")))
+ cobroller_off(side);
}
prog_char str_cobroller_arg0[] = "cobroller";
else if (!strcmp_P(res->arg2, PSTR("pack"))) {
spickle_pack(side);
}
+ else if (!strcmp_P(res->arg2, PSTR("mid"))) {
+ spickle_mid(side);
+ }
printf_P(PSTR("done\r\n"));
}
prog_char str_spickle_arg1[] = "left#right";
parse_pgm_token_string_t cmd_spickle_arg1 =
TOKEN_STRING_INITIALIZER(struct cmd_spickle_result, arg1, str_spickle_arg1);
-prog_char str_spickle_arg2[] = "deploy#pack";
+prog_char str_spickle_arg2[] = "deploy#pack#mid";
parse_pgm_token_string_t cmd_spickle_arg2 =
TOKEN_STRING_INITIALIZER(struct cmd_spickle_result, arg2, str_spickle_arg2);
fixed_string_t arg2;
int32_t arg3;
int32_t arg4;
+ int32_t arg5;
};
/* function called when cmd_spickle_params is parsed successfully */
side = I2C_RIGHT_SIDE;
if (!strcmp_P(res->arg2, PSTR("pos")))
- spickle_set_pos(side, res->arg3, res->arg4);
- else if (!strcmp_P(res->arg2, PSTR("delay")))
- spickle_set_delay(side, res->arg3, res->arg4);
+ spickle_set_pos(side, res->arg3, res->arg4, res->arg5);
}
prog_char str_spickle_params_arg0[] = "spickle_params";
prog_char str_spickle_params_arg1[] = "left#right";
parse_pgm_token_string_t cmd_spickle_params_arg1 =
TOKEN_STRING_INITIALIZER(struct cmd_spickle_params_result, arg1, str_spickle_params_arg1);
-prog_char str_spickle_params_arg2[] = "pos#delay";
+prog_char str_spickle_params_arg2[] = "pos";
parse_pgm_token_string_t cmd_spickle_params_arg2 =
TOKEN_STRING_INITIALIZER(struct cmd_spickle_params_result, arg2, str_spickle_params_arg2);
parse_pgm_token_num_t cmd_spickle_params_arg3 =
TOKEN_NUM_INITIALIZER(struct cmd_spickle_params_result, arg3, INT32);
parse_pgm_token_num_t cmd_spickle_params_arg4 =
TOKEN_NUM_INITIALIZER(struct cmd_spickle_params_result, arg4, INT32);
+parse_pgm_token_num_t cmd_spickle_params_arg5 =
+ TOKEN_NUM_INITIALIZER(struct cmd_spickle_params_result, arg5, INT32);
-prog_char help_spickle_params[] = "Set spickle pos values";
+prog_char help_spickle_params[] = "Set spickle pos values: left|right pos INTPACK INTMID INTDEPL";
parse_pgm_inst_t cmd_spickle_params = {
.f = cmd_spickle_params_parsed, /* function to call */
.data = NULL, /* 2nd arg of func */
.help_str = help_spickle_params,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_spickle_params_arg0,
- (prog_void *)&cmd_spickle_params_arg1,
- (prog_void *)&cmd_spickle_params_arg2,
- (prog_void *)&cmd_spickle_params_arg3,
- (prog_void *)&cmd_spickle_params_arg4,
+ (prog_void *)&cmd_spickle_params_arg0,
+ (prog_void *)&cmd_spickle_params_arg1,
+ (prog_void *)&cmd_spickle_params_arg2,
+ (prog_void *)&cmd_spickle_params_arg3,
+ (prog_void *)&cmd_spickle_params_arg4,
+ (prog_void *)&cmd_spickle_params_arg5,
NULL,
},
};
/* this structure is filled when cmd_quadramp is parsed successfully */
struct cmd_quadramp_result {
struct cmd_cs_result cs;
- uint32_t ap;
- uint32_t an;
- uint32_t sp;
- uint32_t sn;
+ double ap;
+ double an;
+ double sp;
+ double sn;
};
/* function called when cmd_quadramp is parsed successfully */
quadramp_set_2nd_order_vars(&csb->qr, res->ap, res->an);
}
- printf_P(PSTR("quadramp %s %ld %ld %ld %ld\r\n"),
+ printf_P(PSTR("quadramp %s %2.2f %2.2f %2.2f %2.2f\r\n"),
res->cs.csname,
csb->qr.var_2nd_ord_pos,
csb->qr.var_2nd_ord_neg,
prog_char str_quadramp_arg0[] = "quadramp";
parse_pgm_token_string_t cmd_quadramp_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_quadramp_result, cs.cmdname, str_quadramp_arg0);
-parse_pgm_token_num_t cmd_quadramp_ap = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, ap, UINT32);
-parse_pgm_token_num_t cmd_quadramp_an = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, an, UINT32);
-parse_pgm_token_num_t cmd_quadramp_sp = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sp, UINT32);
-parse_pgm_token_num_t cmd_quadramp_sn = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sn, UINT32);
+parse_pgm_token_num_t cmd_quadramp_ap = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, ap, FLOAT);
+parse_pgm_token_num_t cmd_quadramp_an = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, an, FLOAT);
+parse_pgm_token_num_t cmd_quadramp_sp = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sp, FLOAT);
+parse_pgm_token_num_t cmd_quadramp_sn = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sn, FLOAT);
prog_char help_quadramp[] = "Set quadramp values (acc+, acc-, speed+, speed-)";
parse_pgm_inst_t cmd_quadramp = {
error_register_notice(mylog);
error_register_debug(mylog);
- wait_ms(3000);
-
/* SPI + ENCODERS */
encoders_spi_init(); /* this will also init spi hardware */
PWM_NG_INIT16(&gen.pwm1_4A, 4, A, 10, PWM_NG_MODE_SIGNED |
PWM_NG_MODE_SIGN_INVERTED, &PORTD, 4);
- PWM_NG_INIT16(&gen.pwm2_4B, 4, B, 10, PWM_NG_MODE_SIGNED,
- &PORTD, 5);
+ PWM_NG_INIT16(&gen.pwm2_4B, 4, B, 10, PWM_NG_MODE_SIGNED |
+ PWM_NG_MODE_SIGN_INVERTED, &PORTD, 5);
PWM_NG_INIT16(&gen.pwm3_1A, 1, A, 10, PWM_NG_MODE_SIGNED,
&PORTD, 6);
PWM_NG_INIT16(&gen.pwm4_1B, 1, B, 10, PWM_NG_MODE_SIGNED |
sei();
- /* actuators */
- actuator_init();
+ printf_P(PSTR("cobboard start\r\n"));
/* spickle, shovel */
spickle_init();
shovel_init();
+ /* actuators */
+ actuator_init();
+
state_init();
printf_P(PSTR("\r\n"));
* CAP 1,5,6,7,8
*/
static struct sensor_filter sensor_filter[SENSOR_MAX] = {
- [S_CAP1] = { 10, 0, 3, 7, 0, 0 }, /* 0 */
- [S_COB_INSIDE_L] = { 5, 0, 4, 1, 0, 0 }, /* 1 */
+ [S_COB_INSIDE_L] = { 5, 0, 4, 1, 0, 1 }, /* 0 */
+ [S_CAP2] = { 10, 0, 3, 7, 0, 0 }, /* 1 */
[S_COB_INSIDE_R] = { 5, 0, 4, 1, 0, 0 }, /* 2 */
[S_CAP4] = { 1, 0, 0, 1, 0, 0 }, /* 3 */
[S_LCOB] = { 1, 0, 0, 1, 0, 0 }, /* 4 */
[S_LEFT] = { 5, 0, 4, 1, 0, 0 }, /* 5 */
[S_RIGHT] = { 5, 0, 4, 1, 0, 1 }, /* 6 */
- [S_COL_RIGHT] = { 5, 0, 4, 1, 0, 1 }, /* 7 */
+ [S_RCOB] = { 1, 0, 0, 1, 0, 0 }, /* 7 */
[S_RESERVED1] = { 10, 0, 3, 7, 0, 0 }, /* 8 */
[S_RESERVED2] = { 10, 0, 3, 7, 0, 0 }, /* 9 */
[S_RESERVED3] = { 1, 0, 0, 1, 0, 0 }, /* 10 */
-/*
+/*
* Copyright Droids Corporation (2009)
* 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
#define ADC_MAX 4
/* synchronize with sensor.c */
-#define S_CAP1 0
-#define S_COB_INSIDE_L 1
+#define S_COB_INSIDE_L 0
+#define S_CAP2 1
#define S_COB_INSIDE_R 2
#define S_CAP4 3
#define S_LCOB 4
#define S_LEFT 5
#define S_RIGHT 6
-#define S_COL_RIGHT 7
+#define S_RCOB 7
#define S_RESERVED1 8
#define S_RESERVED2 9
#define S_RESERVED3 10
#include <rdline.h>
#include "main.h"
+#include "shovel.h"
/* init spickle position at beginning */
static void shovel_autopos(void)
{
+ printf_P(PSTR("shovel autopos..."));
pwm_ng_set(SHOVEL_PWM, -500);
wait_ms(1000);
pwm_ng_set(LEFT_SPICKLE_PWM, 0);
encoders_spi_set_value(SHOVEL_ENCODER, 0);
+ printf_P(PSTR("ok\r\n"));
+}
+
+static uint8_t shovel_is_at_pos(int32_t pos)
+{
+ int32_t diff;
+ diff = pos - encoders_spi_get_value(SHOVEL_ENCODER);
+ if (diff < 0)
+ diff = -diff;
+ if (diff < 500)
+ return 1;
+ return 0;
+}
+
+uint8_t shovel_is_up(void)
+{
+ return shovel_is_at_pos(SHOVEL_UP);
+}
+
+uint8_t shovel_is_down(void)
+{
+ return shovel_is_at_pos(SHOVEL_DOWN);
}
void shovel_init(void)
#define _SHOVEL_H_
#define SHOVEL_DOWN 100
-#define SHOVEL_MID 6000
+#define SHOVEL_MID 4900
#define SHOVEL_UP 10000
void shovel_init(void);
cs_set_consign(&cobboard.shovel.cs, SHOVEL_UP);
}
+uint8_t shovel_is_up(void);
+uint8_t shovel_is_down(void);
+
#endif /* _SHOVEL_H_ */
struct cs_block * const csb[2];
/* params */
- int16_t delay_deployed[2];
- int16_t delay_packed[2];
int32_t pos_deployed[2];
+ int32_t pos_mid[2];
int32_t pos_packed[2];
};
&cobboard.left_spickle,
&cobboard.right_spickle,
},
- .delay_deployed = {
- 500, /* left */
- 500, /* right */
- },
- .delay_packed = {
- 500, /* left */
- 500, /* right */
- },
.pos_deployed = {
- 35000, /* left */
- 35000, /* right */
+ 40000, /* left */
+ -40000, /* right */
+ },
+ .pos_mid = {
+ 20000, /* left */
+ -20000, /* right */
},
.pos_packed = {
0, /* left */
/* init spickle position at beginning */
static void spickle_autopos(void)
{
+ printf_P(PSTR("spickle autopos..."));
pwm_ng_set(LEFT_SPICKLE_PWM, -500);
- //pwm_ng_set(RIGHT_SPICKLE_PWM, -500);
+ pwm_ng_set(RIGHT_SPICKLE_PWM, 500);
wait_ms(1000);
pwm_ng_set(LEFT_SPICKLE_PWM, 0);
pwm_ng_set(RIGHT_SPICKLE_PWM, 0);
encoders_spi_set_value(LEFT_SPICKLE_ENCODER, 0);
encoders_spi_set_value(RIGHT_SPICKLE_ENCODER, 0);
+ printf_P(PSTR("ok\r\n"));
}
/* Set CS command for spickle. Called by CS manager. */
spickle.k2 = k2;
}
-void spickle_set_pos(uint8_t side, uint32_t pos_deployed, uint32_t pos_packed)
+
+void spickle_set_pos(uint8_t side, int32_t pos_packed,
+ int32_t pos_mid, int32_t pos_deployed)
{
spickle.pos_deployed[side] = pos_deployed;
+ spickle.pos_mid[side] = pos_mid;
spickle.pos_packed[side] = pos_packed;
}
-void spickle_set_delay(uint8_t side, uint32_t delay_deployed, uint32_t delay_packed)
-{
- spickle.delay_deployed[side] = delay_deployed;
- spickle.delay_packed[side] = delay_packed;
-}
-
void spickle_dump_params(void)
{
printf_P(PSTR("coef %ld %ld\r\n"), spickle.k1, spickle.k2);
printf_P(PSTR("left pos %ld %ld\r\n"),
- spickle.pos_deployed[I2C_LEFT_SIDE],
- spickle.pos_packed[I2C_LEFT_SIDE]);
- printf_P(PSTR("left delay %ld %ld\r\n"),
- spickle.delay_deployed[I2C_LEFT_SIDE],
- spickle.delay_packed[I2C_LEFT_SIDE]);
+ spickle.pos_packed[I2C_LEFT_SIDE],
+ spickle.pos_mid[I2C_LEFT_SIDE],
+ spickle.pos_deployed[I2C_LEFT_SIDE]);
printf_P(PSTR("right pos %ld %ld\r\n"),
- spickle.pos_deployed[I2C_RIGHT_SIDE],
- spickle.pos_packed[I2C_RIGHT_SIDE]);
- printf_P(PSTR("right delay %ld %ld\r\n"),
- spickle.delay_deployed[I2C_RIGHT_SIDE],
- spickle.delay_packed[I2C_RIGHT_SIDE]);
+ spickle.pos_packed[I2C_RIGHT_SIDE],
+ spickle.pos_mid[I2C_RIGHT_SIDE],
+ spickle.pos_deployed[I2C_RIGHT_SIDE]);
}
void spickle_deploy(uint8_t side)
cs_set_consign(&spickle.csb[side]->cs, spickle.pos_deployed[side]);
}
-void spickle_pack(uint8_t side)
+void spickle_mid(uint8_t side)
{
- cs_set_consign(&spickle.csb[side]->cs, spickle.pos_packed[side]);
+ cs_set_consign(&spickle.csb[side]->cs, spickle.pos_mid[side]);
}
-uint16_t spickle_get_deploy_delay(uint8_t side)
-{
- return spickle.delay_deployed[side];
-}
-
-uint16_t spickle_get_pack_delay(uint8_t side)
+void spickle_pack(uint8_t side)
{
- return spickle.delay_packed[side];
+ cs_set_consign(&spickle.csb[side]->cs, spickle.pos_packed[side]);
}
void spickle_init(void)
{
spickle_autopos();
cobboard.left_spickle.on = 1;
- //cobboard.right_spickle.on = 1;
+ cobboard.right_spickle.on = 1;
}
void spickle_set(void *dummy, int32_t cmd);
void spickle_set_coefs(uint32_t k1, uint32_t k2);
-void spickle_set_pos(uint8_t side, uint32_t pos_deploy, uint32_t pos_pack);
-void spickle_set_delay(uint8_t side, uint32_t delay_deployed, uint32_t delay_packed);
+void spickle_set_pos(uint8_t side, int32_t pos_pack,
+ int32_t pos_mid, int32_t pos_deployed);
void spickle_dump_params(void);
void spickle_deploy(uint8_t side);
+void spickle_mid(uint8_t side);
void spickle_pack(uint8_t side);
-uint16_t spickle_get_deploy_delay(uint8_t side);
-uint16_t spickle_get_pack_delay(uint8_t side);
-
void spickle_init(void);
#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
if (side == I2C_LEFT_SIDE)
return sensor_get(S_LCOB);
else
- /* XXX */
- // return sensor_get(S_LCOB);
- return 0;
+ return sensor_get(S_RCOB);
}
/* return the detected color of the cob (only valid if present) */
static uint8_t state_cob_color(uint8_t side)
{
+ /* XXX no color sensor for now */
if (side == I2C_LEFT_SIDE)
return I2C_COB_WHITE;
else
- /* XXX */
- // return sensor_get(S_LCOB);
- return 0;
+ return I2C_COB_WHITE;
}
/* return true if the cob is correctly inside */
sensor_get(S_COB_INSIDE_R);
}
+/* return true if there is no cob correctly inside */
+static uint8_t state_no_cob_inside(void)
+{
+ return !sensor_get(S_COB_INSIDE_L) &&
+ !sensor_get(S_COB_INSIDE_R);
+}
+
/* set a new state, return 0 on success */
int8_t state_set_mode(uint8_t mode)
{
/* harvest cobs from area */
static void state_do_harvest(uint8_t side)
{
- uint16_t delay;
-
/* if there is no cob, return */
if (state_cob_present(side))
return;
if (state_cob_color(side) == I2C_COB_BLACK)
return;
+ STMCH_DEBUG("start");
+
/* eat the cob */
spickle_pack(side);
- /* xxx */
+
time_wait_ms(250);
- left_cobroller_on();
- delay = spickle_get_pack_delay(side);
+ cobroller_on(side);
- WAIT_COND_OR_TIMEOUT(state_cob_inside(), delay);
+ if (WAIT_COND_OR_TIMEOUT(state_cob_inside(), 750) == 0) {
+ if (state_no_cob_inside()) {
+ printf_P(PSTR("NO COB\r\n"));
+ return;
+ }
+ printf_P(PSTR("BAD COB - press a key\r\n"));
+ while(!cmdline_keypressed());
+ }
+
+ /* cob is inside, switch off roller */
+ cobroller_off(side);
+ cob_count ++;
+
+ /* last cob, nothing to do */
+ if (cob_count == 5)
+ return;
/* redeploy the spickle */
spickle_deploy(side);
state_debug_wait_key_pressed();
- /* let the cob go */
+ /* let the loaded cobs go */
+ servo_door_block();
servo_carry_open();
- wait_ms(300); /* XXX */
+ time_wait_ms(100);
state_debug_wait_key_pressed();
- cob_count ++;
-
/* store it */
shovel_up();
- wait_ms(200);
+
+ if (WAIT_COND_OR_TIMEOUT(shovel_is_up(), 400) == 0) {
+ BRAKE_ON();
+ printf_P(PSTR("BLOCKED\r\n"));
+ while(!cmdline_keypressed());
+ }
+
state_debug_wait_key_pressed();
/* close the carry servos */
servo_carry_close();
- wait_ms(300); /* XXX */
+ servo_door_close();
+ time_wait_ms(200);
state_debug_wait_key_pressed();
shovel_down();
- left_cobroller_off();
- state_debug_wait_key_pressed();
- time_wait_ms(500);
+
+ if (WAIT_COND_OR_TIMEOUT(shovel_is_down(), 400) == 0) {
+ BRAKE_ON();
+ printf_P(PSTR("BLOCKED\r\n"));
+ while(!cmdline_keypressed());
+ }
+
+ STMCH_DEBUG("end");
}
/* eject cobs */
state_init();
}
+ cobroller_off(I2C_LEFT_SIDE);
+ cobroller_off(I2C_RIGHT_SIDE);
+
/* pack/deply spickles, enable/disable roller */
- if (L_DEPLOY(state_mode)) {
+ 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);
- //left_cobroller_on();
- left_cobroller_off();
- }
- else {
+ else
spickle_pack(I2C_LEFT_SIDE);
- left_cobroller_off();
- }
- if (R_DEPLOY(state_mode)) {
+
+ 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);
- right_cobroller_on();
- }
- else {
+ else
spickle_pack(I2C_RIGHT_SIDE);
- right_cobroller_off();
- }
/* harvest */
- if (L_DEPLOY(state_mode) && L_HARVEST(state_mode))
- state_do_harvest(I2C_LEFT_SIDE);
- if (R_DEPLOY(state_mode) && R_HARVEST(state_mode))
- state_do_harvest(I2C_RIGHT_SIDE);
+ if (cob_count < 5) {
+ if (L_DEPLOY(state_mode) && L_HARVEST(state_mode))
+ state_do_harvest(I2C_LEFT_SIDE);
+ if (R_DEPLOY(state_mode) && R_HARVEST(state_mode))
+ state_do_harvest(I2C_RIGHT_SIDE);
+ }
/* eject */
if (EJECT(state_mode)) {
extern parse_pgm_inst_t cmd_cobboard_setmode3;
extern parse_pgm_inst_t cmd_beacon_start;
extern parse_pgm_inst_t cmd_servo_balls;
+extern parse_pgm_inst_t cmd_clitoid;
extern parse_pgm_inst_t cmd_test;
/* commands_traj.c */
(parse_pgm_inst_t *)&cmd_cobboard_setmode2,
(parse_pgm_inst_t *)&cmd_cobboard_setmode3,
(parse_pgm_inst_t *)&cmd_servo_balls,
+ (parse_pgm_inst_t *)&cmd_clitoid,
(parse_pgm_inst_t *)&cmd_test,
/* commands_traj.c */
/*
* Copyright Droids Corporation (2008)
- *
+ *
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
*
* Revision : $Id: commands_cs.c,v 1.4 2009-05-02 10:08:09 zer0 Exp $
*
- * Olivier MATZ <zer0@droids-corp.org>
+ * Olivier MATZ <zer0@droids-corp.org>
*/
#include <stdio.h>
}
return NULL;
}
-
+
/**********************************************************/
/* Gains for control system */
return;
}
- if (!show)
+ if (!show)
pid_set_gains(&csb->pid, res->p, res->i, res->d);
printf_P(PSTR("%s %s %d %d %d\r\n"),
.data = NULL, /* 2nd arg of func */
.help_str = help_gain,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_gain_arg0,
- (prog_void *)&cmd_csb_name_tok,
- (prog_void *)&cmd_gain_p,
- (prog_void *)&cmd_gain_i,
- (prog_void *)&cmd_gain_d,
+ (prog_void *)&cmd_gain_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_gain_p,
+ (prog_void *)&cmd_gain_i,
+ (prog_void *)&cmd_gain_d,
NULL,
},
};
.data = (void *)1, /* 2nd arg of func */
.help_str = help_gain_show,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_gain_arg0,
- (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_gain_arg0,
+ (prog_void *)&cmd_csb_name_tok,
(prog_void *)&cmd_gain_show_arg,
NULL,
},
static void cmd_speed_parsed(void *parsed_result, void *show)
{
struct cmd_speed_result * res = parsed_result;
-
+
struct cs_block *csb;
csb = cs_from_name(res->cs.csname);
}
#if notyet
- if (!show)
+ if (!show)
ramp_set_vars(&csb->ramp, res->s, res->s); /* set speed */
- printf_P(PSTR("%s %"PRIu32"\r\n"),
+ printf_P(PSTR("%s %"PRIu32"\r\n"),
res->cs.csname,
ext.r_b.var_pos);
#else
.data = NULL, /* 2nd arg of func */
.help_str = help_speed,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_speed_arg0,
- (prog_void *)&cmd_csb_name_tok,
- (prog_void *)&cmd_speed_s,
+ (prog_void *)&cmd_speed_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_speed_s,
NULL,
},
};
.data = (void *)1, /* 2nd arg of func */
.help_str = help_speed_show,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_speed_arg0,
- (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_speed_arg0,
+ (prog_void *)&cmd_csb_name_tok,
(prog_void *)&cmd_speed_show_arg,
NULL,
},
return;
}
- if (!show)
+ if (!show)
pid_set_derivate_filter(&csb->pid, res->size);
- printf_P(PSTR("%s %s %u\r\n"),
+ printf_P(PSTR("%s %s %u\r\n"),
res->cs.cmdname,
res->cs.csname,
pid_get_derivate_filter(&csb->pid));
.data = (void *)1, /* 2nd arg of func */
.help_str = help_derivate_filter,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_derivate_filter_arg0,
- (prog_void *)&cmd_csb_name_tok,
- (prog_void *)&cmd_derivate_filter_size,
+ (prog_void *)&cmd_derivate_filter_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_derivate_filter_size,
NULL,
},
};
.data = NULL, /* 2nd arg of func */
.help_str = help_derivate_filter_show,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_derivate_filter_arg0,
- (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_derivate_filter_arg0,
+ (prog_void *)&cmd_csb_name_tok,
(prog_void *)&cmd_derivate_filter_show_arg,
NULL,
},
.data = NULL, /* 2nd arg of func */
.help_str = help_consign,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_consign_arg0,
- (prog_void *)&cmd_csb_name_tok,
- (prog_void *)&cmd_consign_p,
+ (prog_void *)&cmd_consign_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_consign_p,
NULL,
},
};
static void cmd_maximum_parsed(void *parsed_result, void *show)
{
struct cmd_maximum_result * res = parsed_result;
-
+
struct cs_block *csb;
csb = cs_from_name(res->cs.csname);
if (!show)
pid_set_maximums(&csb->pid, res->in, res->i, res->out);
- printf_P(PSTR("maximum %s %"PRIu32" %"PRIu32" %"PRIu32"\r\n"),
+ printf_P(PSTR("maximum %s %"PRIu32" %"PRIu32" %"PRIu32"\r\n"),
res->cs.csname,
pid_get_max_in(&csb->pid),
pid_get_max_I(&csb->pid),
.data = NULL, /* 2nd arg of func */
.help_str = help_maximum,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_maximum_arg0,
- (prog_void *)&cmd_csb_name_tok,
- (prog_void *)&cmd_maximum_in,
- (prog_void *)&cmd_maximum_i,
- (prog_void *)&cmd_maximum_out,
+ (prog_void *)&cmd_maximum_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_maximum_in,
+ (prog_void *)&cmd_maximum_i,
+ (prog_void *)&cmd_maximum_out,
NULL,
},
};
.data = (void *)1, /* 2nd arg of func */
.help_str = help_maximum_show,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_maximum_arg0,
- (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_maximum_arg0,
+ (prog_void *)&cmd_csb_name_tok,
(prog_void *)&cmd_maximum_show_arg,
NULL,
},
/* this structure is filled when cmd_quadramp is parsed successfully */
struct cmd_quadramp_result {
struct cmd_cs_result cs;
- uint32_t ap;
- uint32_t an;
- uint32_t sp;
- uint32_t sn;
+ double ap;
+ double an;
+ double sp;
+ double sn;
};
/* function called when cmd_quadramp is parsed successfully */
static void cmd_quadramp_parsed(void *parsed_result, void *show)
{
struct cmd_quadramp_result * res = parsed_result;
-
+
struct cs_block *csb;
csb = cs_from_name(res->cs.csname);
quadramp_set_2nd_order_vars(&csb->qr, res->ap, res->an);
}
- printf_P(PSTR("quadramp %s %"PRIi32" %"PRIi32" %"PRIi32" %"PRIi32"\r\n"),
+ printf_P(PSTR("quadramp %s %2.2f %2.2f %2.2f %2.2f\r\n"),
res->cs.csname,
csb->qr.var_2nd_ord_pos,
csb->qr.var_2nd_ord_neg,
prog_char str_quadramp_arg0[] = "quadramp";
parse_pgm_token_string_t cmd_quadramp_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_quadramp_result, cs.cmdname, str_quadramp_arg0);
-parse_pgm_token_num_t cmd_quadramp_ap = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, ap, UINT32);
-parse_pgm_token_num_t cmd_quadramp_an = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, an, UINT32);
-parse_pgm_token_num_t cmd_quadramp_sp = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sp, UINT32);
-parse_pgm_token_num_t cmd_quadramp_sn = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sn, UINT32);
+parse_pgm_token_num_t cmd_quadramp_ap = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, ap, FLOAT);
+parse_pgm_token_num_t cmd_quadramp_an = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, an, FLOAT);
+parse_pgm_token_num_t cmd_quadramp_sp = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sp, FLOAT);
+parse_pgm_token_num_t cmd_quadramp_sn = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sn, FLOAT);
prog_char help_quadramp[] = "Set quadramp values (acc+, acc-, speed+, speed-)";
parse_pgm_inst_t cmd_quadramp = {
.data = NULL, /* 2nd arg of func */
.help_str = help_quadramp,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_quadramp_arg0,
- (prog_void *)&cmd_csb_name_tok,
- (prog_void *)&cmd_quadramp_ap,
- (prog_void *)&cmd_quadramp_an,
- (prog_void *)&cmd_quadramp_sp,
- (prog_void *)&cmd_quadramp_sn,
-
+ (prog_void *)&cmd_quadramp_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_quadramp_ap,
+ (prog_void *)&cmd_quadramp_an,
+ (prog_void *)&cmd_quadramp_sp,
+ (prog_void *)&cmd_quadramp_sn,
+
NULL,
},
};
.data = (void *)1, /* 2nd arg of func */
.help_str = help_quadramp_show,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_quadramp_arg0,
- (prog_void *)&cmd_csb_name_tok,
- (prog_void *)&cmd_quadramp_show_arg,
+ (prog_void *)&cmd_quadramp_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_quadramp_show_arg,
NULL,
},
};
struct cs_block *csb;
uint8_t loop = 0;
uint8_t print_pid = 0, print_cs = 0;
-
+
csb = cs_from_name(res->cs.csname);
if (csb == NULL) {
printf_P(PSTR("null csb\r\n"));
.data = NULL, /* 2nd arg of func */
.help_str = help_cs_status,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_cs_status_arg0,
- (prog_void *)&cmd_csb_name_tok,
- (prog_void *)&cmd_cs_status_arg,
+ (prog_void *)&cmd_cs_status_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_cs_status_arg,
NULL,
},
};
static void cmd_blocking_i_parsed(void *parsed_result, void *show)
{
struct cmd_blocking_i_result * res = parsed_result;
-
+
struct cs_block *csb;
csb = cs_from_name(res->cs.csname);
bd_set_current_thresholds(&csb->bd, res->k1, res->k2,
res->i, res->cpt);
- printf_P(PSTR("%s %s %"PRIi32" %"PRIi32" %"PRIi32" %d\r\n"),
+ printf_P(PSTR("%s %s %"PRIi32" %"PRIi32" %"PRIi32" %d\r\n"),
res->cs.cmdname,
res->cs.csname,
csb->bd.k1,
.data = NULL, /* 2nd arg of func */
.help_str = help_blocking_i,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_blocking_i_arg0,
- (prog_void *)&cmd_csb_name_tok,
- (prog_void *)&cmd_blocking_i_k1,
- (prog_void *)&cmd_blocking_i_k2,
- (prog_void *)&cmd_blocking_i_i,
+ (prog_void *)&cmd_blocking_i_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_blocking_i_k1,
+ (prog_void *)&cmd_blocking_i_k2,
+ (prog_void *)&cmd_blocking_i_i,
(prog_void *)&cmd_blocking_i_cpt,
NULL,
},
.data = (void *)1, /* 2nd arg of func */
.help_str = help_blocking_i_show,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_blocking_i_arg0,
- (prog_void *)&cmd_csb_name_tok,
- (prog_void *)&cmd_blocking_i_show_arg,
+ (prog_void *)&cmd_blocking_i_arg0,
+ (prog_void *)&cmd_csb_name_tok,
+ (prog_void *)&cmd_blocking_i_show_arg,
NULL,
},
};
#include <stdio.h>
#include <string.h>
+#include <math.h>
#include <hostsim.h>
#include <aversive/pgmspace.h>
#include <quadramp.h>
#include <control_system_manager.h>
#include <trajectory_manager.h>
+#include <trajectory_manager_utils.h>
#include <vect_base.h>
#include <lines.h>
#include <polygon.h>
};
/**********************************************************/
-/* Test */
+/* Clitoid */
-/* this structure is filled when cmd_test is parsed successfully */
-struct cmd_test_result {
+/* this structure is filled when cmd_clitoid is parsed successfully */
+struct cmd_clitoid_result {
fixed_string_t arg0;
- int32_t radius;
- int32_t dist;
+ float alpha_deg;
+ float beta_deg;
+ float R_mm;
+ float Vd;
+ float Amax;
+ float d_inter_mm;
};
+/**
+ * do a superb curve joining line1 to line2 which is composed of:
+ * - a clothoid starting from line1
+ * - a circle
+ * - another clothoid up to line2
+ *
+ * the function assumes that the initial linear speed is Vd and
+ * angular speed is 0.
+ *
+ * - alpha: total angle
+ * - beta: circular part of angle (lower than alpha)
+ * - R: the radius of the circle (must be != 0)
+ * - Vd: linear speed to use (in imp per cs period)
+ * - Amax: maximum angular acceleration
+ * - d_inter: distance in mm until the intersection of the
+ * 2 lines
+ *
+ * return 0 on success: in this case these parameters are filled:
+ * - Aa_out: the angular acceleration to configure in quadramp
+ * - remain_d_mm_out: remaining distance before start to turn
+ */
+uint8_t clitoid(double alpha_deg, double beta_deg, double R_mm,
+ double Vd, double Amax, double d_inter_mm)
+{
+ double Vd_mm_s;
+ double Va, Va_rd_s;
+ double t, d_mm, alpha_rad, beta_rad;
+ double remain_d_mm;
+ double Aa, Aa_rd_s2;
+ line_t line1, line2;
+ double x, y, a_rad;
+ point_t robot, intersect, pt2, center, proj;
+ vect_t v;
+
+ /* param check */
+ if (fabs(alpha_deg) <= fabs(beta_deg)) {
+ DEBUG(E_USER_STRAT, "alpha is smaller than beta");
+ return END_ERROR;
+ }
+
+ /* get angular speed Va */
+ Vd_mm_s = Vd * (CS_HZ/DIST_IMP_MM);
+ DEBUG(E_USER_STRAT, "Vd_mm_s=%2.2f", Vd_mm_s);
+ Va_rd_s = Vd_mm_s / R_mm;
+ Va = Va_rd_s * (DIST_IMP_MM * EXT_TRACK_MM / (2 * CS_HZ));
+ DEBUG(E_USER_STRAT, "Va_rd_s=%2.2f Va=%2.2f", Va_rd_s, Va);
+
+ /* process 't', the time in seconds that we will take to do
+ * the first clothoid */
+ alpha_rad = RAD(alpha_deg);
+ beta_rad = RAD(beta_deg);
+ t = fabs(((alpha_rad - beta_rad) * R_mm) / Vd_mm_s);
+ DEBUG(E_USER_STRAT, "R_mm=%2.2f alpha_rad=%2.2f beta_rad=%2.2f t=%2.2f",
+ R_mm, alpha_rad, beta_rad, t);
+
+ /* process the angular acceleration */
+ Aa_rd_s2 = Va_rd_s / t;
+ Aa = Aa_rd_s2 * (DIST_IMP_MM * EXT_TRACK_MM /
+ (2 * CS_HZ * CS_HZ));
+ DEBUG(E_USER_STRAT, "Aa_rd_s2=%2.2f Aa=%2.2f", Aa_rd_s2, Aa);
+
+ /* exit if the robot cannot physically do it */
+ if (Aa > Amax) {
+ DEBUG(E_USER_STRAT, "greater than max acceleration");
+ return END_ERROR;
+ }
+
+ /* the robot position */
+ x = position_get_x_double(&mainboard.pos);
+ y = position_get_y_double(&mainboard.pos);
+ a_rad = position_get_a_rad_double(&mainboard.pos);
+
+ /* define line1 and line2 */
+ robot.x = x;
+ robot.y = y;
+ intersect.x = x + cos(a_rad) * d_inter_mm;
+ intersect.y = y + sin(a_rad) * d_inter_mm;
+ pts2line(&robot, &intersect, &line1);
+ pt2.x = intersect.x + cos(a_rad + alpha_rad);
+ pt2.y = intersect.y + sin(a_rad + alpha_rad);
+ pts2line(&intersect, &pt2, &line2);
+ DEBUG(E_USER_STRAT, "intersect=(%2.2f, %2.2f)",
+ intersect.x, intersect.y);
+
+ /* the center of the circle is at (d_mm, d_mm) when we have to
+ * start the clothoid */
+ d_mm = R_mm * sqrt(fabs(alpha_rad - beta_rad)) *
+ sqrt(M_PI) / 2.;
+ DEBUG(E_USER_STRAT, "d_mm=%2.2f", d_mm);
+
+ /* translate line1 */
+ v.x = intersect.x - robot.x;
+ v.y = intersect.y - robot.y;
+ if (a_rad > 0)
+ vect_rot_trigo(&v);
+ else
+ vect_rot_retro(&v);
+ vect_resize(&v, d_mm);
+ line_translate(&line1, &v);
+
+ /* translate line2 */
+ v.x = intersect.x - pt2.x;
+ v.y = intersect.y - pt2.y;
+ if (a_rad > 0)
+ vect_rot_trigo(&v);
+ else
+ vect_rot_retro(&v);
+ vect_resize(&v, d_mm);
+ line_translate(&line2, &v);
+
+ /* find the center of the circle, at the intersection of the
+ * new translated lines */
+ if (intersect_line(&line1, &line2, ¢er) != 1) {
+ DEBUG(E_USER_STRAT, "cannot find circle center");
+ return END_ERROR;
+ }
+ DEBUG(E_USER_STRAT, "center=(%2.2f,%2.2f)", center.x, center.y);
+
+ /* project center of circle on line1 */
+ proj_pt_line(¢er, &line1, &proj);
+ DEBUG(E_USER_STRAT, "proj=(%2.2f,%2.2f)", proj.x, proj.y);
+
+ /* process remaining distance before start turning */
+ remain_d_mm = d_inter_mm - (pt_norm(&proj, &intersect) + d_mm);
+ DEBUG(E_USER_STRAT, "remain_d=%2.2f", remain_d_mm);
+ if (remain_d_mm < 0) {
+ DEBUG(E_USER_STRAT, "too late, cannot turn");
+ return END_ERROR;
+ }
+
+ return END_TRAJ;
+}
+
/* function called when cmd_test is parsed successfully */
+static void cmd_clitoid_parsed(void *parsed_result, void *data)
+{
+ struct cmd_clitoid_result *res = parsed_result;
+ clitoid(res->alpha_deg, res->beta_deg, res->R_mm,
+ res->Vd, res->Amax, res->d_inter_mm);
+}
+
+prog_char str_clitoid_arg0[] = "clitoid";
+parse_pgm_token_string_t cmd_clitoid_arg0 =
+ TOKEN_STRING_INITIALIZER(struct cmd_clitoid_result,
+ arg0, str_clitoid_arg0);
+parse_pgm_token_num_t cmd_clitoid_alpha_deg =
+ TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
+ alpha_deg, FLOAT);
+parse_pgm_token_num_t cmd_clitoid_beta_deg =
+ TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
+ beta_deg, FLOAT);
+parse_pgm_token_num_t cmd_clitoid_R_mm =
+ TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
+ R_mm, FLOAT);
+parse_pgm_token_num_t cmd_clitoid_Vd =
+ TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
+ Vd, FLOAT);
+parse_pgm_token_num_t cmd_clitoid_Amax =
+ TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
+ Amax, FLOAT);
+parse_pgm_token_num_t cmd_clitoid_d_inter_mm =
+ TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
+ d_inter_mm, FLOAT);
+
+prog_char help_clitoid[] = "do a clitoid (alpha, beta, R, Vd, Amax, d_inter)";
+parse_pgm_inst_t cmd_clitoid = {
+ .f = cmd_clitoid_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_clitoid,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_clitoid_arg0,
+ (prog_void *)&cmd_clitoid_alpha_deg,
+ (prog_void *)&cmd_clitoid_beta_deg,
+ (prog_void *)&cmd_clitoid_R_mm,
+ (prog_void *)&cmd_clitoid_Vd,
+ (prog_void *)&cmd_clitoid_Amax,
+ (prog_void *)&cmd_clitoid_d_inter_mm,
+ NULL,
+ },
+};
+
+//////////////////////
+
+// 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 radius, double dist)
{
uint8_t err;
- int32_t dist_imp_target;
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("%s()\n", __FUNCTION__);
- strat_set_speed(500, 500);
+ 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,
distance = angle * radius;
if (distance < 0)
distance = -distance;
- dist_imp_target = rs_get_distance(&mainboard.rs) +
- distance * mainboard.pos.phys.distance_imp_per_mm;
+ 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);
- distance += 100; /* take some margin to avoid deceleration */
- trajectory_d_a_rel(&mainboard.traj, distance, angle);
- err = WAIT_COND_OR_TRAJ_END(rs_get_distance(&mainboard.rs) > dist_imp_target,
+ /* 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 */
+
+/* this structure is filled when cmd_test is parsed successfully */
+struct cmd_test_result {
+ fixed_string_t arg0;
+ int32_t radius;
+ int32_t dist;
+};
+
/* function called when cmd_test is parsed successfully */
static void cmd_test_parsed(void *parsed_result, void *data)
{
mainboard.angle.on = 1;
mainboard.distance.on = 1;
#endif
- line2line(375, 347, 375, 1847,
- 375, 1847, 1050, 1472,
- 100, 200);
- line2line(825, 1596, 1050, 1472,
- 1050, 1472, 1500, 1722,
- 180, 120);
- line2line(1050, 1472, 1500, 1722,
- 1500, 1722, 2175, 1347,
- 180, 120);
- line2line(1500, 1722, 2175, 1347,
- 2175, 1347, 2175, 847,
- 150, 120);
- line2line(2175, 1347, 2175, 847,
- 2175, 847, 2400, 722,
- 150, 120);
- line2line(2175, 847, 2400, 722,
- 2400, 722, 2625, 847,
- 150, 100);
- line2line(2400, 722, 2625, 847,
- 2625, 847, 2625, 1847,
- 150, 100);
- line2line(2625, 847, 2625, 1847,
- 2625, 1847, 375, 597,
- 100, 200);
+ 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);
}
prog_char str_test_arg0[] = "test";
#include <quadramp.h>
#include <control_system_manager.h>
#include <trajectory_manager.h>
+#include <trajectory_manager_utils.h>
#include <vect_base.h>
#include <lines.h>
#include <polygon.h>
strat_get_speed(&old_spdd, &old_spda);
strat_set_speed(AUTOPOS_SPEED_FAST, AUTOPOS_SPEED_FAST);
- trajectory_d_rel(&mainboard.traj, -300);
+ trajectory_d_rel(&mainboard.traj, 300);
err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING);
if (err == END_INTR)
goto intr;
wait_ms(100);
- strat_reset_pos(ROBOT_LENGTH/2, 0, 0);
+ strat_reset_pos(ROBOT_WIDTH/2,
+ COLOR_Y(ROBOT_HALF_LENGTH_FRONT),
+ COLOR_A(-90));
- trajectory_d_rel(&mainboard.traj, 120);
+ trajectory_d_rel(&mainboard.traj, -180);
err = wait_traj_end(END_INTR|END_TRAJ);
if (err == END_INTR)
goto intr;
- trajectory_a_rel(&mainboard.traj, COLOR_A(90));
+ trajectory_a_rel(&mainboard.traj, COLOR_A(-90));
err = wait_traj_end(END_INTR|END_TRAJ);
if (err == END_INTR)
goto intr;
- trajectory_d_rel(&mainboard.traj, -300);
+ trajectory_d_rel(&mainboard.traj, 300);
err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING);
if (err == END_INTR)
goto intr;
wait_ms(100);
- strat_reset_pos(DO_NOT_SET_POS, COLOR_Y(ROBOT_LENGTH/2),
- COLOR_A(90));
+ strat_reset_pos(ROBOT_HALF_LENGTH_FRONT,
+ DO_NOT_SET_POS,
+ 180);
- trajectory_d_rel(&mainboard.traj, 120);
+ trajectory_d_rel(&mainboard.traj, -170);
err = wait_traj_end(END_INTR|END_TRAJ);
if (err == END_INTR)
goto intr;
wait_ms(100);
-
- trajectory_a_rel(&mainboard.traj, COLOR_A(-40));
+
+ trajectory_a_rel(&mainboard.traj, COLOR_A(-110));
err = wait_traj_end(END_INTR|END_TRAJ);
if (err == END_INTR)
goto intr;
wait_ms(100);
-
+
strat_set_speed(old_spdd, old_spda);
return;
static void cmd_position_parsed(void * parsed_result, void * data)
{
struct cmd_position_result * res = parsed_result;
-
+
/* display raw position values */
if (!strcmp_P(res->arg1, PSTR("reset"))) {
position_set(&mainboard.pos, 0, 0, 0);
#endif
auto_position();
}
- else if (!strcmp_P(res->arg1, PSTR("autoset_red"))) {
+ else if (!strcmp_P(res->arg1, PSTR("autoset_yellow"))) {
mainboard.our_color = I2C_COLOR_YELLOW;
#ifndef HOST_VERSION
i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW);
prog_char str_position_arg0[] = "position";
parse_pgm_token_string_t cmd_position_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg0, str_position_arg0);
-prog_char str_position_arg1[] = "show#reset#autoset_blue#autoset_red";
+prog_char str_position_arg1[] = "show#reset#autoset_blue#autoset_yellow";
parse_pgm_token_string_t cmd_position_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1);
prog_char help_position[] = "Show/reset (x,y,a) position";
#include "strat.h"
#include "actuator.h"
+void dump_cs(const char *name, struct cs *cs);
+
#ifndef HOST_VERSION
int32_t encoders_left_cobroller_speed(void *number)
{
cpt++;
#ifdef HOST_VERSION
- if ((cpt & 7) == 0)
+ if ((cpt & 7) == 0) {
+ // dump_cs("dist", &mainboard.distance.cs);
robotsim_dump();
+ }
#endif
}
/* increase gain to decrease dist, increase left and it will turn more left */
#ifdef HOST_VERSION
rs_set_left_ext_encoder(&mainboard.rs, robotsim_encoder_get,
- LEFT_ENCODER, IMP_COEF);
+ LEFT_ENCODER, IMP_COEF * 1.);
rs_set_right_ext_encoder(&mainboard.rs, robotsim_encoder_get,
- RIGHT_ENCODER, IMP_COEF);
+ RIGHT_ENCODER, IMP_COEF * 1.);
#else
rs_set_left_ext_encoder(&mainboard.rs, encoders_spi_get_value,
- LEFT_ENCODER, IMP_COEF * -1.00);
+ LEFT_ENCODER, IMP_COEF * -1.036);
rs_set_right_ext_encoder(&mainboard.rs, encoders_spi_get_value,
- RIGHT_ENCODER, IMP_COEF * 1.00);
+ RIGHT_ENCODER, IMP_COEF * 1.037);
#endif
/* rs will use external encoders */
rs_set_flags(&mainboard.rs, RS_USE_EXT);
position_use_ext(&mainboard.pos);
/* TRAJECTORY MANAGER */
- trajectory_init(&mainboard.traj);
+ trajectory_init(&mainboard.traj, CS_HZ);
trajectory_set_cs(&mainboard.traj, &mainboard.distance.cs,
&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 */
/* distance window, angle window, angle start */
trajectory_set_windows(&mainboard.traj, 200., 5.0, 30.);
/* QUADRAMP */
quadramp_init(&mainboard.angle.qr);
- quadramp_set_1st_order_vars(&mainboard.angle.qr, 2000, 2000); /* set speed */
- quadramp_set_2nd_order_vars(&mainboard.angle.qr, 13, 13); /* set accel */
+ quadramp_set_1st_order_vars(&mainboard.angle.qr, 500, 500); /* set speed */
+ quadramp_set_2nd_order_vars(&mainboard.angle.qr, 5, 5); /* set accel */
/* CS */
cs_init(&mainboard.angle.cs);
/* ---- CS distance */
/* PID */
pid_init(&mainboard.distance.pid);
- pid_set_gains(&mainboard.distance.pid, 500, 100, 7000);
+ pid_set_gains(&mainboard.distance.pid, 500, 10, 7000);
pid_set_maximums(&mainboard.distance.pid, 0, 2000, 4095);
pid_set_out_shift(&mainboard.distance.pid, 10);
pid_set_derivate_filter(&mainboard.distance.pid, 6);
/* QUADRAMP */
quadramp_init(&mainboard.distance.qr);
- quadramp_set_1st_order_vars(&mainboard.distance.qr, 2000, 2000); /* set speed */
- quadramp_set_2nd_order_vars(&mainboard.distance.qr, 17, 17); /* set accel */
+ quadramp_set_1st_order_vars(&mainboard.distance.qr, 500, 500); /* set speed */
+ quadramp_set_2nd_order_vars(&mainboard.distance.qr, 5., 5.); /* set accel */
/* CS */
cs_init(&mainboard.distance.cs);
cs_init(&mainboard.right_cobroller.cs);
cs_set_correct_filter(&mainboard.right_cobroller.cs, pid_do_filter, &mainboard.right_cobroller.pid);
cs_set_process_in(&mainboard.right_cobroller.cs, pwm_ng_set, RIGHT_COBROLLER_PWM);
- cs_set_process_out(&mainboard.right_cobroller.cs, encoders_left_cobroller_speed, RIGHT_COBROLLER_ENCODER);
+ cs_set_process_out(&mainboard.right_cobroller.cs, encoders_right_cobroller_speed, RIGHT_COBROLLER_ENCODER);
cs_set_consign(&mainboard.right_cobroller.cs, 0);
/* Blocking detection */
mainboard.angle.on = 0;
mainboard.distance.on = 0;
mainboard.left_cobroller.on = 1;
- mainboard.right_cobroller.on = 0;
+ mainboard.right_cobroller.on = 1;
scheduler_add_periodical_event_priority(do_cs, NULL,
AREA_X = 3000.
AREA_Y = 2100.
+ROBOT_HEIGHT=5 # 350
+CORN_HEIGHT=5 # 150
+
area = [ (0.0, 0.0, -0.2), (3000.0, 2100.0, 0.2) ]
areasize = reduce(lambda x,y:tuple([abs(x[i])+abs(y[i]) for i in range(len(x))]) , area)
area_box = box(size=areasize, color=(0.0, 1.0, 0.0))
# all positions of robot every 5ms
save_pos = []
-robot = box(pos = (0, 0, 150),
- size = (250,320,350),
+robot = box(pos = (0, 0, ROBOT_HEIGHT/2),
+ size = (250,320,ROBOT_HEIGHT),
color = (0.3, 0.3, 0.3) )
last_pos = robot.pos.x, robot.pos.y, robot.pos.z
while y < 2100:
print x,y
if waypoints[i][j] == TYPE_WHITE_CORN:
- c = cylinder(axis=(0,0,1), length=150,
+ c = cylinder(axis=(0,0,1), length=CORN_HEIGHT,
radius=25, color=(0.8,0.8,0.8),
- pos=(x-AREA_X/2,y-AREA_Y/2,75))
+ pos=(x-AREA_X/2,y-AREA_Y/2,CORN_HEIGHT/2))
area_objects.append(c)
elif waypoints[i][j] == TYPE_BLACK_CORN:
- c = cylinder(axis=(0,0,1), length=150,
+ c = cylinder(axis=(0,0,1), length=CORN_HEIGHT,
radius=25, color=(0.2,0.2,0.2),
- pos=(x-AREA_X/2,y-AREA_Y/2,75))
+ pos=(x-AREA_X/2,y-AREA_Y/2,CORN_HEIGHT/2))
area_objects.append(c)
elif waypoints[i][j] == TYPE_BALL:
c = sphere(radius=50, color=(1., 0.,0.),
global robot, last_pos, robot_trail, robot_trail_list
global save_pos
- robot.pos = (x - AREA_X/2, y - AREA_Y/2, 150)
+ 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)
- robot.size = (250, 320, 350)
+ robot.size = (250, 320, ROBOT_HEIGHT)
# save position
save_pos.append((robot.pos.x, robot.pos, a))
#endif
#ifdef HOST_VERSION
- strat_reset_pos(1000, 1000, -90);
+ strat_reset_pos(400, COLOR_Y(400), COLOR_A(-90));
#endif
cmdline_interact();
#define MATCH_TIME 89
/* decrease track to decrease angle */
-#define EXT_TRACK_MM 302.0188
+#define EXT_TRACK_MM 304.61875
#define VIRTUAL_TRACK_MM EXT_TRACK_MM
-#define ROBOT_LENGTH 320
+#define ROBOT_HALF_LENGTH_FRONT 130
+#define ROBOT_HALF_LENGTH_REAR 120
#define ROBOT_WIDTH 320
-/* it is a 2048 imps -> 8192 because we see 1/4 period
- * and diameter: 55mm -> perimeter 173mm
- * 8192/173 -> 473 */
+/* it is a 1024 imps -> 4096 because we see 1/4 period
+ * and diameter: 55mm -> perimeter 134mm
+ * dist_imp_mm = 4096/134 x 10 -> 304 */
/* increase it to go further */
-#define IMP_ENCODERS 2048
-#define WHEEL_DIAMETER_MM 55.0
+#define IMP_ENCODERS 1024
+#define WHEEL_DIAMETER_MM 42.9
#define WHEEL_PERIM_MM (WHEEL_DIAMETER_MM * M_PI)
#define IMP_COEF 10.
#define DIST_IMP_MM (((IMP_ENCODERS*4) / WHEEL_PERIM_MM) * IMP_COEF)
#define I2C_POLL_PRIO 20
#define EEPROM_TIME_PRIO 10
-#define CS_PERIOD 5000L
+#define CS_PERIOD 5000L /* in microsecond */
+#define CS_HZ (1000000. / CS_PERIOD)
#define NB_LOGS 4
((local_r_pwm * 1000 * FILTER2)/1000);
/* basic collision detection */
- a2 = atan2(ROBOT_WIDTH/2, ROBOT_LENGTH/2);
- d = norm(ROBOT_WIDTH/2, ROBOT_LENGTH/2);
+ a2 = atan2(ROBOT_WIDTH/2, ROBOT_HALF_LENGTH_REAR);
+ d = norm(ROBOT_WIDTH/2, ROBOT_HALF_LENGTH_REAR);
xfl = x + cos(a+a2) * d;
yfl = y + sin(a+a2) * d;
{
// printf("%p, %d\n", arg, val);
if (arg == LEFT_PWM)
- l_pwm = val;
+ l_pwm = (val / 1.55);
else if (arg == RIGHT_PWM)
- r_pwm = val;
+ r_pwm = (val / 1.55);
}
int32_t robotsim_encoder_get(void *arg)
#define TRAJ_FLAGS_NO_NEAR_NO_TIMER (END_TRAJ|END_BLOCKING|END_OBSTACLE|END_INTR)
#define TRAJ_FLAGS_SMALL_DIST (END_TRAJ|END_BLOCKING|END_INTR)
+/* default acc */
+#define ACC_DIST 5.
+#define ACC_ANGLE 5.
+
/* default speeds */
-#define SPEED_DIST_FAST 2500
-#define SPEED_ANGLE_FAST 2000
-#define SPEED_DIST_SLOW 1000
-#define SPEED_ANGLE_SLOW 1000
-#define SPEED_DIST_VERY_SLOW 400
-#define SPEED_ANGLE_VERY_SLOW 400
+#define SPEED_DIST_FAST 2500.
+#define SPEED_ANGLE_FAST 2000.
+#define SPEED_DIST_SLOW 1000.
+#define SPEED_ANGLE_SLOW 1000.
+#define SPEED_DIST_VERY_SLOW 400.
+#define SPEED_ANGLE_VERY_SLOW 400.
/* strat infos structures */
--- /dev/null
+/*
+ * Copyright Droids Corporation, 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
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: strat_base.c,v 1.8 2009-11-08 17:24:33 zer0 Exp $
+ *
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <uart.h>
+#include <pwm_ng.h>
+#include <clock_time.h>
+#include <spi.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <vect_base.h>
+#include <lines.h>
+#include <polygon.h>
+#include <obstacle_avoidance.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+
+#include "../common/i2c_commands.h"
+
+#include "main.h"
+
#include <quadramp.h>
#include <control_system_manager.h>
#include <trajectory_manager.h>
+#include <trajectory_manager_utils.h>
#include <vect_base.h>
#include <lines.h>
#include <polygon.h>
#include <quadramp.h>
#include <control_system_manager.h>
#include <trajectory_manager.h>
+#include <trajectory_manager_utils.h>
#include <vect_base.h>
#include <lines.h>
#include <polygon.h>
*/
-#define DEG(x) (((double)(x)) * (180.0 / M_PI))
-#define RAD(x) (((double)(x)) * (M_PI / 180.0))
-#define M_2PI (2*M_PI)
-
struct xy_point {
int16_t x;
int16_t y;
x,y = build_path([(600,1972), (600+bcoef, 1972-acoef)])
ax.plot(x, y, 'r-')
+ p = PatchCollection([Circle((2400,972), 225)],
+ cmap=matplotlib.cm.jet,
+ alpha=0.5, facecolor=(1.,0.,0.))
+ ax.add_collection(p)
+
# limit
#x,y = build_poly([(250,250), (2750,250), (2750,1850), (250,1850)])
#ax.plot(x, y, 'g--')
ax.grid(color = (0.3, 0.3, 0.3))
ax.set_xlim(-100, 3100)
ax.set_ylim(-100, 2200)
+ #ax.set_xlim(0, 825)
+ #ax.set_ylim(1472, 1972)
#ax.set_title('spline paths')
#plt.show()
fig.savefig(filename)
git.droids-corp.org / aversive.git / commitdiff