From: zer0 Date: Sun, 11 Apr 2010 15:19:40 +0000 (+0200) Subject: work on trajectory, update cobboard and ballboard too X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=commitdiff_plain;h=fa8546ea39c7442ad3bf5a822a72a2b50a41045d work on trajectory, update cobboard and ballboard too --- diff --git a/modules/base/math/geometry/lines.c b/modules/base/math/geometry/lines.c index 09ad1a1..935d012 100755 --- a/modules/base/math/geometry/lines.c +++ b/modules/base/math/geometry/lines.c @@ -46,24 +46,24 @@ * 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 */ /* @@ -74,7 +74,7 @@ intersect_line(const line_t *l1, const line_t *l2, point_t *p) 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) @@ -118,12 +118,12 @@ void pts2line(const point_t *p1, const point_t *p2, line_t *l) 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); } @@ -145,16 +145,16 @@ void proj_pt_line(const point_t * p, const line_t * l, point_t * p_out) /* 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; @@ -210,7 +210,7 @@ intersect_segment(const point_t *s1, const point_t *s2, *p = *s2; return 2; } - + debug_printf("px=%" PRIi32 " py=%" PRIi32 "\n", p->x, p->y); /* Consider as parallel if intersection is too far */ @@ -242,3 +242,8 @@ intersect_segment(const point_t *s1, const point_t *s2, return 1; } + +void line_translate(line_t *l, vect_t *v) +{ + l->c -= (l->a * v->x + l->b * v->y); +} diff --git a/modules/base/math/geometry/lines.h b/modules/base/math/geometry/lines.h index 6763b91..62094f4 100755 --- a/modules/base/math/geometry/lines.h +++ b/modules/base/math/geometry/lines.h @@ -52,4 +52,7 @@ intersect_segment(const point_t *s1, const point_t *s2, 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_ */ diff --git a/modules/comm/uart/uart_host.c b/modules/comm/uart/uart_host.c index bd6aeed..04231c2 100644 --- a/modules/comm/uart/uart_host.c +++ b/modules/comm/uart/uart_host.c @@ -24,6 +24,8 @@ #include #include +#include + /* this file os a stub for host */ void uart_init(void) @@ -61,12 +63,13 @@ void uart_getconf(uint8_t num, struct uart_config *u) 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(); } diff --git a/modules/devices/control_system/filters/quadramp/quadramp.c b/modules/devices/control_system/filters/quadramp/quadramp.c index b7f18a4..793df38 100644 --- a/modules/devices/control_system/filters/quadramp/quadramp.c +++ b/modules/devices/control_system/filters/quadramp/quadramp.c @@ -26,9 +26,7 @@ #include #include -#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); @@ -37,16 +35,16 @@ void quadramp_init(struct quadramp_filter * q) } -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); @@ -55,9 +53,9 @@ void quadramp_set_2nd_order_vars(struct quadramp_filter * q, 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); @@ -83,104 +81,100 @@ uint8_t quadramp_is_finished(struct quadramp_filter *q) 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 @@ -188,5 +182,5 @@ int32_t quadramp_do_filter(void * data, int32_t in) q->previous_out = pos_target; q->previous_in = in; - return pos_target ; + return pos_target; } diff --git a/modules/devices/control_system/filters/quadramp/quadramp.h b/modules/devices/control_system/filters/quadramp/quadramp.h index 5e8100c..7073df0 100644 --- a/modules/devices/control_system/filters/quadramp/quadramp.h +++ b/modules/devices/control_system/filters/quadramp/quadramp.h @@ -26,12 +26,12 @@ 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; }; @@ -42,12 +42,12 @@ void quadramp_init(struct quadramp_filter *q); 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 diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager.c b/modules/devices/robot/trajectory_manager/trajectory_manager.c index 773388e..7311532 100644 --- a/modules/devices/robot/trajectory_manager/trajectory_manager.c +++ b/modules/devices/robot/trajectory_manager/trajectory_manager.c @@ -41,12 +41,13 @@ /************ 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); @@ -77,7 +78,7 @@ void trajectory_set_robot_params(struct trajectory *traj, } /** 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); @@ -86,6 +87,16 @@ void trajectory_set_speed(struct trajectory *traj, int16_t d_speed, int16_t a_sp 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) diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager.h b/modules/devices/robot/trajectory_manager/trajectory_manager.h index 6e7e82e..594b7b0 100644 --- a/modules/devices/robot/trajectory_manager/trajectory_manager.h +++ b/modules/devices/robot/trajectory_manager/trajectory_manager.h @@ -52,6 +52,10 @@ enum trajectory_state { /* line */ RUNNING_LINE, + + /* clitoid */ + RUNNING_CLITOID_LINE, + RUNNING_CLITOID_CURVE, }; struct circle_target { @@ -64,10 +68,18 @@ 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 { @@ -86,19 +98,24 @@ 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, @@ -110,7 +127,10 @@ void trajectory_set_robot_params(struct trajectory *traj, 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. @@ -133,6 +153,8 @@ void trajectory_set_circle_coef(struct trajectory *traj, double coef); * 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 */ diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c index 0177419..5246285 100644 --- a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c +++ b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c @@ -55,6 +55,7 @@ static uint8_t evt_debug_cpt = 0; } \ } while (0) +static void start_clitoid(struct trajectory *traj); /** * update angle and/or distance @@ -294,13 +295,28 @@ void trajectory_goto_xy_rel(struct trajectory *traj, double x_rel_mm, double y_r /************ 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 */ @@ -642,6 +658,19 @@ static void trajectory_manager_line_event(struct trajectory *traj) 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); + } } @@ -668,6 +697,7 @@ void trajectory_manager_event(void * param) break; case RUNNING_LINE: + case RUNNING_CLITOID_LINE: trajectory_manager_line_event(traj); break; @@ -762,15 +792,13 @@ void trajectory_circle_abs_dist_deg(struct trajectory *traj, /*********** *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; @@ -788,8 +816,223 @@ void trajectory_line_abs(struct trajectory *traj, 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; +} diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager_utils.c b/modules/devices/robot/trajectory_manager/trajectory_manager_utils.c index 0cf01ac..c48b592 100644 --- a/modules/devices/robot/trajectory_manager/trajectory_manager_utils.c +++ b/modules/devices/robot/trajectory_manager/trajectory_manager_utils.c @@ -41,7 +41,7 @@ #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; @@ -51,7 +51,7 @@ void set_quadramp_speed(struct trajectory *traj, int16_t d_speed, int16_t a_spee } /** 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; @@ -59,17 +59,28 @@ uint32_t 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) { 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); @@ -143,3 +154,89 @@ uint8_t is_robot_in_angle_window(struct trajectory *traj, double a_win_rad) 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)); +} + diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager_utils.h b/modules/devices/robot/trajectory_manager/trajectory_manager_utils.h index 9ceaf48..f90475e 100644 --- a/modules/devices/robot/trajectory_manager/trajectory_manager_utils.h +++ b/modules/devices/robot/trajectory_manager/trajectory_manager_utils.h @@ -30,13 +30,16 @@ #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); @@ -60,3 +63,16 @@ uint8_t is_robot_in_xy_window(struct trajectory *traj, double d_win); * 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); diff --git a/projects/microb2010/ballboard/commands_cs.c b/projects/microb2010/ballboard/commands_cs.c index 098ffb3..c1dca26 100644 --- a/projects/microb2010/ballboard/commands_cs.c +++ b/projects/microb2010/ballboard/commands_cs.c @@ -428,17 +428,17 @@ parse_pgm_inst_t cmd_maximum_show = { /* 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); @@ -452,7 +452,7 @@ static void cmd_quadramp_parsed(void *parsed_result, void *show) 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, @@ -462,10 +462,10 @@ static void cmd_quadramp_parsed(void *parsed_result, void *show) 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 = { @@ -473,13 +473,13 @@ 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, }, }; diff --git a/projects/microb2010/ballboard/cs.c b/projects/microb2010/ballboard/cs.c index a82f3f4..bab700d 100644 --- a/projects/microb2010/ballboard/cs.c +++ b/projects/microb2010/ballboard/cs.c @@ -108,7 +108,7 @@ void microb_cs_init(void) /* 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); diff --git a/projects/microb2010/ballboard/main.c b/projects/microb2010/ballboard/main.c index 9633368..082b658 100755 --- a/projects/microb2010/ballboard/main.c +++ b/projects/microb2010/ballboard/main.c @@ -163,7 +163,7 @@ int main(void) # 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) { @@ -197,8 +197,7 @@ int main(void) 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); diff --git a/projects/microb2010/cobboard/actuator.c b/projects/microb2010/cobboard/actuator.c index 79f91cd..6811854 100644 --- a/projects/microb2010/cobboard/actuator.c +++ b/projects/microb2010/cobboard/actuator.c @@ -1,6 +1,6 @@ -/* +/* * 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 @@ -49,13 +49,14 @@ //#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); @@ -81,24 +82,25 @@ void servo_door_close(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) diff --git a/projects/microb2010/cobboard/actuator.h b/projects/microb2010/cobboard/actuator.h index 90ff487..5b59a73 100644 --- a/projects/microb2010/cobboard/actuator.h +++ b/projects/microb2010/cobboard/actuator.h @@ -26,13 +26,13 @@ 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 diff --git a/projects/microb2010/cobboard/commands_cobboard.c b/projects/microb2010/cobboard/commands_cobboard.c index 888d15a..4ea3b80 100644 --- a/projects/microb2010/cobboard/commands_cobboard.c +++ b/projects/microb2010/cobboard/commands_cobboard.c @@ -402,11 +402,13 @@ static void cmd_servo_door_parsed(void *parsed_result, 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"; @@ -436,19 +438,17 @@ static void cmd_cobroller_parsed(void *parsed_result, __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"; @@ -575,6 +575,9 @@ static void cmd_spickle_parsed(void * parsed_result, 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")); } @@ -584,7 +587,7 @@ parse_pgm_token_string_t cmd_spickle_arg0 = 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); @@ -611,6 +614,7 @@ struct cmd_spickle_params_result { fixed_string_t arg2; int32_t arg3; int32_t arg4; + int32_t arg5; }; /* function called when cmd_spickle_params is parsed successfully */ @@ -631,9 +635,7 @@ static void cmd_spickle_params_parsed(void *parsed_result, 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"; @@ -642,25 +644,28 @@ parse_pgm_token_string_t cmd_spickle_params_arg0 = 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, }, }; diff --git a/projects/microb2010/cobboard/commands_cs.c b/projects/microb2010/cobboard/commands_cs.c index 27be783..dfb7785 100644 --- a/projects/microb2010/cobboard/commands_cs.c +++ b/projects/microb2010/cobboard/commands_cs.c @@ -428,10 +428,10 @@ parse_pgm_inst_t cmd_maximum_show = { /* 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 */ @@ -452,7 +452,7 @@ static void cmd_quadramp_parsed(void *parsed_result, void *show) 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, @@ -462,10 +462,10 @@ static void cmd_quadramp_parsed(void *parsed_result, void *show) 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 = { diff --git a/projects/microb2010/cobboard/main.c b/projects/microb2010/cobboard/main.c index 62103c8..24f14bc 100755 --- a/projects/microb2010/cobboard/main.c +++ b/projects/microb2010/cobboard/main.c @@ -183,8 +183,6 @@ int main(void) error_register_notice(mylog); error_register_debug(mylog); - wait_ms(3000); - /* SPI + ENCODERS */ encoders_spi_init(); /* this will also init spi hardware */ @@ -205,8 +203,8 @@ int main(void) 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 | @@ -248,13 +246,15 @@ int main(void) 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")); diff --git a/projects/microb2010/cobboard/sensor.c b/projects/microb2010/cobboard/sensor.c index b820d98..5bbf43f 100644 --- a/projects/microb2010/cobboard/sensor.c +++ b/projects/microb2010/cobboard/sensor.c @@ -142,14 +142,14 @@ struct sensor_filter { * 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 */ diff --git a/projects/microb2010/cobboard/sensor.h b/projects/microb2010/cobboard/sensor.h index 4e634fc..0ba6b0c 100644 --- a/projects/microb2010/cobboard/sensor.h +++ b/projects/microb2010/cobboard/sensor.h @@ -1,7 +1,7 @@ -/* +/* * Copyright Droids Corporation (2009) * Olivier MATZ - * + * * 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 @@ -28,14 +28,14 @@ #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 diff --git a/projects/microb2010/cobboard/shovel.c b/projects/microb2010/cobboard/shovel.c index 39ce2bc..6d71e58 100644 --- a/projects/microb2010/cobboard/shovel.c +++ b/projects/microb2010/cobboard/shovel.c @@ -40,14 +40,38 @@ #include #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) diff --git a/projects/microb2010/cobboard/shovel.h b/projects/microb2010/cobboard/shovel.h index 7ba8445..241b065 100644 --- a/projects/microb2010/cobboard/shovel.h +++ b/projects/microb2010/cobboard/shovel.h @@ -23,7 +23,7 @@ #define _SHOVEL_H_ #define SHOVEL_DOWN 100 -#define SHOVEL_MID 6000 +#define SHOVEL_MID 4900 #define SHOVEL_UP 10000 void shovel_init(void); @@ -43,4 +43,7 @@ static inline void shovel_up(void) cs_set_consign(&cobboard.shovel.cs, SHOVEL_UP); } +uint8_t shovel_is_up(void); +uint8_t shovel_is_down(void); + #endif /* _SHOVEL_H_ */ diff --git a/projects/microb2010/cobboard/spickle.c b/projects/microb2010/cobboard/spickle.c index 62732c2..e1d9a7b 100644 --- a/projects/microb2010/cobboard/spickle.c +++ b/projects/microb2010/cobboard/spickle.c @@ -54,9 +54,8 @@ struct spickle_params { 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]; }; @@ -67,17 +66,13 @@ static struct spickle_params spickle = { &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 */ @@ -88,13 +83,15 @@ static struct spickle_params spickle = { /* 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. */ @@ -140,33 +137,26 @@ void spickle_set_coefs(uint32_t k1, uint32_t k2) 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) @@ -174,24 +164,19 @@ 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; } diff --git a/projects/microb2010/cobboard/spickle.h b/projects/microb2010/cobboard/spickle.h index d36f80f..81c74d8 100644 --- a/projects/microb2010/cobboard/spickle.h +++ b/projects/microb2010/cobboard/spickle.h @@ -24,17 +24,15 @@ 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 diff --git a/projects/microb2010/cobboard/state.c b/projects/microb2010/cobboard/state.c index 3308405..9d082ba 100644 --- a/projects/microb2010/cobboard/state.c +++ b/projects/microb2010/cobboard/state.c @@ -1,6 +1,6 @@ -/* +/* * 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 @@ -98,20 +98,17 @@ static uint8_t state_cob_present(uint8_t side) 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 */ @@ -121,6 +118,13 @@ static uint8_t state_cob_inside(void) 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) { @@ -158,8 +162,6 @@ uint8_t state_get_mode(void) /* 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; @@ -168,40 +170,67 @@ static void state_do_harvest(uint8_t side) 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 */ @@ -228,30 +257,35 @@ void state_machine(void) 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)) { diff --git a/projects/microb2010/mainboard/commands.c b/projects/microb2010/mainboard/commands.c index b1f09d0..cdbeb6e 100644 --- a/projects/microb2010/mainboard/commands.c +++ b/projects/microb2010/mainboard/commands.c @@ -76,6 +76,7 @@ extern parse_pgm_inst_t cmd_cobboard_setmode2; 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 */ @@ -159,6 +160,7 @@ parse_pgm_ctx_t main_ctx[] = { (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 */ diff --git a/projects/microb2010/mainboard/commands_cs.c b/projects/microb2010/mainboard/commands_cs.c index 272f1e9..c749c61 100644 --- a/projects/microb2010/mainboard/commands_cs.c +++ b/projects/microb2010/mainboard/commands_cs.c @@ -1,6 +1,6 @@ /* * 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 @@ -17,7 +17,7 @@ * * Revision : $Id: commands_cs.c,v 1.4 2009-05-02 10:08:09 zer0 Exp $ * - * Olivier MATZ + * Olivier MATZ */ #include @@ -89,7 +89,7 @@ struct cs_block *cs_from_name(const char *name) } return NULL; } - + /**********************************************************/ /* Gains for control system */ @@ -113,7 +113,7 @@ static void cmd_gain_parsed(void * parsed_result, void *show) 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"), @@ -136,11 +136,11 @@ parse_pgm_inst_t cmd_gain = { .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, }, }; @@ -161,8 +161,8 @@ parse_pgm_inst_t cmd_gain_show = { .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, }, @@ -181,7 +181,7 @@ struct cmd_speed_result { 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); @@ -191,10 +191,10 @@ static void cmd_speed_parsed(void *parsed_result, void *show) } #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 @@ -212,9 +212,9 @@ parse_pgm_inst_t cmd_speed = { .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, }, }; @@ -234,8 +234,8 @@ parse_pgm_inst_t cmd_speed_show = { .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, }, @@ -262,10 +262,10 @@ static void cmd_derivate_filter_parsed(void *parsed_result, void *show) 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)); @@ -281,9 +281,9 @@ parse_pgm_inst_t cmd_derivate_filter = { .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, }, }; @@ -304,8 +304,8 @@ parse_pgm_inst_t cmd_derivate_filter_show = { .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, }, @@ -346,9 +346,9 @@ parse_pgm_inst_t cmd_consign = { .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, }, }; @@ -369,7 +369,7 @@ struct cmd_maximum_result { 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); @@ -381,7 +381,7 @@ static void cmd_maximum_parsed(void *parsed_result, void *show) 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), @@ -400,11 +400,11 @@ parse_pgm_inst_t cmd_maximum = { .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, }, }; @@ -425,8 +425,8 @@ parse_pgm_inst_t cmd_maximum_show = { .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, }, @@ -438,17 +438,17 @@ parse_pgm_inst_t cmd_maximum_show = { /* 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); @@ -462,7 +462,7 @@ static void cmd_quadramp_parsed(void *parsed_result, void *show) 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, @@ -472,10 +472,10 @@ static void cmd_quadramp_parsed(void *parsed_result, void *show) 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 = { @@ -483,13 +483,13 @@ 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, }, }; @@ -510,9 +510,9 @@ parse_pgm_inst_t cmd_quadramp_show = { .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, }, }; @@ -535,7 +535,7 @@ static void cmd_cs_status_parsed(void *parsed_result, void *data) 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")); @@ -587,9 +587,9 @@ parse_pgm_inst_t cmd_cs_status = { .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, }, }; @@ -611,7 +611,7 @@ struct cmd_blocking_i_result { 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); @@ -624,7 +624,7 @@ static void cmd_blocking_i_parsed(void *parsed_result, void *show) 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, @@ -646,11 +646,11 @@ parse_pgm_inst_t cmd_blocking_i = { .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, }, @@ -672,9 +672,9 @@ parse_pgm_inst_t cmd_blocking_i_show = { .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, }, }; diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c index ced8e58..fe4fabb 100644 --- a/projects/microb2010/mainboard/commands_mainboard.c +++ b/projects/microb2010/mainboard/commands_mainboard.c @@ -22,6 +22,7 @@ #include #include +#include #include #include @@ -40,6 +41,7 @@ #include #include #include +#include #include #include #include @@ -1068,16 +1070,207 @@ parse_pgm_inst_t cmd_servo_balls = { }; /**********************************************************/ -/* 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, @@ -1085,14 +1278,16 @@ static void line2line(double line1x1, double line1y1, 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, @@ -1106,21 +1301,100 @@ static void line2line(double line1x1, double line1y1, 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) { @@ -1130,30 +1404,91 @@ 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"; diff --git a/projects/microb2010/mainboard/commands_traj.c b/projects/microb2010/mainboard/commands_traj.c index 81e2e04..7bf6057 100644 --- a/projects/microb2010/mainboard/commands_traj.c +++ b/projects/microb2010/mainboard/commands_traj.c @@ -39,6 +39,7 @@ #include #include #include +#include #include #include #include @@ -701,43 +702,46 @@ static void auto_position(void) 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; @@ -750,7 +754,7 @@ intr: 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); @@ -766,7 +770,7 @@ static void cmd_position_parsed(void * parsed_result, void * data) #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); @@ -784,7 +788,7 @@ static void cmd_position_parsed(void * parsed_result, void * data) 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"; diff --git a/projects/microb2010/mainboard/cs.c b/projects/microb2010/mainboard/cs.c index 81e5245..905d533 100644 --- a/projects/microb2010/mainboard/cs.c +++ b/projects/microb2010/mainboard/cs.c @@ -56,6 +56,8 @@ #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) { @@ -155,8 +157,10 @@ static void do_cs(void *dummy) cpt++; #ifdef HOST_VERSION - if ((cpt & 7) == 0) + if ((cpt & 7) == 0) { + // dump_cs("dist", &mainboard.distance.cs); robotsim_dump(); + } #endif } @@ -197,14 +201,14 @@ void microb_cs_init(void) /* 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); @@ -217,11 +221,12 @@ void microb_cs_init(void) 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.); @@ -235,8 +240,8 @@ void microb_cs_init(void) /* 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); @@ -254,15 +259,15 @@ void microb_cs_init(void) /* ---- 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); @@ -310,7 +315,7 @@ void microb_cs_init(void) 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 */ @@ -323,7 +328,7 @@ void microb_cs_init(void) 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, diff --git a/projects/microb2010/mainboard/display.py b/projects/microb2010/mainboard/display.py index 3ca1a47..ad8cff2 100644 --- a/projects/microb2010/mainboard/display.py +++ b/projects/microb2010/mainboard/display.py @@ -4,6 +4,9 @@ from visual import * 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)) @@ -13,8 +16,8 @@ scene.autoscale=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 @@ -191,14 +194,14 @@ def toggle_obj_disp(): 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.), @@ -224,11 +227,11 @@ def set_robot(x, y, a): 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)) diff --git a/projects/microb2010/mainboard/main.c b/projects/microb2010/mainboard/main.c index 4061d19..88d4f97 100755 --- a/projects/microb2010/mainboard/main.c +++ b/projects/microb2010/mainboard/main.c @@ -304,7 +304,7 @@ int main(void) #endif #ifdef HOST_VERSION - strat_reset_pos(1000, 1000, -90); + strat_reset_pos(400, COLOR_Y(400), COLOR_A(-90)); #endif cmdline_interact(); diff --git a/projects/microb2010/mainboard/main.h b/projects/microb2010/mainboard/main.h index 53cfe69..48655eb 100755 --- a/projects/microb2010/mainboard/main.h +++ b/projects/microb2010/mainboard/main.h @@ -74,18 +74,19 @@ #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) @@ -118,7 +119,8 @@ #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 diff --git a/projects/microb2010/mainboard/robotsim.c b/projects/microb2010/mainboard/robotsim.c index 15b1075..d0aa3ae 100644 --- a/projects/microb2010/mainboard/robotsim.c +++ b/projects/microb2010/mainboard/robotsim.c @@ -118,8 +118,8 @@ void robotsim_update(void) ((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; @@ -156,9 +156,9 @@ void robotsim_pwm(void *arg, int32_t val) { // 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) diff --git a/projects/microb2010/mainboard/strat.h b/projects/microb2010/mainboard/strat.h index e92ff4f..53d9192 100644 --- a/projects/microb2010/mainboard/strat.h +++ b/projects/microb2010/mainboard/strat.h @@ -69,13 +69,17 @@ #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 */ diff --git a/projects/microb2010/mainboard/strat_avoid.c b/projects/microb2010/mainboard/strat_avoid.c new file mode 100644 index 0000000..60dead3 --- /dev/null +++ b/projects/microb2010/mainboard/strat_avoid.c @@ -0,0 +1,55 @@ +/* + * 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 +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "../common/i2c_commands.h" + +#include "main.h" + diff --git a/projects/microb2010/mainboard/strat_base.c b/projects/microb2010/mainboard/strat_base.c index 40c6f86..2c52778 100644 --- a/projects/microb2010/mainboard/strat_base.c +++ b/projects/microb2010/mainboard/strat_base.c @@ -38,6 +38,7 @@ #include #include #include +#include #include #include #include diff --git a/projects/microb2010/mainboard/strat_utils.c b/projects/microb2010/mainboard/strat_utils.c index 06df8c7..708f08a 100644 --- a/projects/microb2010/mainboard/strat_utils.c +++ b/projects/microb2010/mainboard/strat_utils.c @@ -37,6 +37,7 @@ #include #include #include +#include #include #include #include diff --git a/projects/microb2010/mainboard/strat_utils.h b/projects/microb2010/mainboard/strat_utils.h index 85997c2..7502a43 100644 --- a/projects/microb2010/mainboard/strat_utils.h +++ b/projects/microb2010/mainboard/strat_utils.h @@ -20,10 +20,6 @@ */ -#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; diff --git a/projects/microb2010/tests/oa/graph.py b/projects/microb2010/tests/oa/graph.py index b7e5853..49d3649 100644 --- a/projects/microb2010/tests/oa/graph.py +++ b/projects/microb2010/tests/oa/graph.py @@ -197,6 +197,11 @@ def build_area(ax): 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--') @@ -275,6 +280,8 @@ def graph(filename): 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)