]> git.droids-corp.org - aversive.git/commitdiff
work on trajectory, update cobboard and ballboard too
authorzer0 <zer0@carbon.local>
Sun, 11 Apr 2010 15:19:40 +0000 (17:19 +0200)
committerzer0 <zer0@carbon.local>
Sun, 11 Apr 2010 15:19:40 +0000 (17:19 +0200)
40 files changed:
modules/base/math/geometry/lines.c
modules/base/math/geometry/lines.h
modules/comm/uart/uart_host.c
modules/devices/control_system/filters/quadramp/quadramp.c
modules/devices/control_system/filters/quadramp/quadramp.h
modules/devices/robot/trajectory_manager/trajectory_manager.c
modules/devices/robot/trajectory_manager/trajectory_manager.h
modules/devices/robot/trajectory_manager/trajectory_manager_core.c
modules/devices/robot/trajectory_manager/trajectory_manager_utils.c
modules/devices/robot/trajectory_manager/trajectory_manager_utils.h
projects/microb2010/ballboard/commands_cs.c
projects/microb2010/ballboard/cs.c
projects/microb2010/ballboard/main.c
projects/microb2010/cobboard/actuator.c
projects/microb2010/cobboard/actuator.h
projects/microb2010/cobboard/commands_cobboard.c
projects/microb2010/cobboard/commands_cs.c
projects/microb2010/cobboard/main.c
projects/microb2010/cobboard/sensor.c
projects/microb2010/cobboard/sensor.h
projects/microb2010/cobboard/shovel.c
projects/microb2010/cobboard/shovel.h
projects/microb2010/cobboard/spickle.c
projects/microb2010/cobboard/spickle.h
projects/microb2010/cobboard/state.c
projects/microb2010/mainboard/commands.c
projects/microb2010/mainboard/commands_cs.c
projects/microb2010/mainboard/commands_mainboard.c
projects/microb2010/mainboard/commands_traj.c
projects/microb2010/mainboard/cs.c
projects/microb2010/mainboard/display.py
projects/microb2010/mainboard/main.c
projects/microb2010/mainboard/main.h
projects/microb2010/mainboard/robotsim.c
projects/microb2010/mainboard/strat.h
projects/microb2010/mainboard/strat_avoid.c [new file with mode: 0644]
projects/microb2010/mainboard/strat_base.c
projects/microb2010/mainboard/strat_utils.c
projects/microb2010/mainboard/strat_utils.h
projects/microb2010/tests/oa/graph.py

index 09ad1a1ea554545d051ff4d796bb102df031dc83..935d01204e7f6bd38273a83bbb2bf682e526bc71 100755 (executable)
  *  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);
+}
index 6763b91db60279cb9615097b66794843f2470972..62094f4ccc5e57600c3d5bdda89fbcbade827202 100755 (executable)
@@ -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_ */
index bd6aeed993e2b37d071587591ab72e8fa06f513f..04231c2fbaa35b8509f760cfd2e21979ddaa507e 100644 (file)
@@ -24,6 +24,8 @@
 #include <uart.h>
 #include <uart_private.h>
 
+#include <fcntl.h>
+
 /* 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();
 }
 
index b7f18a43ba824f942c03d682faadab05d87060d0..793df389b8b6f19229793832d9e4c3d9e01bf4c4 100644 (file)
@@ -26,9 +26,7 @@
 #include <aversive.h>
 #include <quadramp.h>
 
-#define NEXT(n, i)  (((n) + (i)/(n)) >> 1)
-
-void quadramp_init(struct quadramp_filter * q)
+void quadramp_init(struct quadramp_filter *q)
 {
        uint8_t flags;
        IRQ_LOCK(flags);
@@ -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;
 }
index 5e8100c1f822c6bfae5864300e4ed5a3c67a62df..7073df03517d880becd3203a3f8f08299d1781cd 100644 (file)
 
 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
index 773388ee5d6fb0be370ce301ba4a93f01e2b7583..731153226a695ce8b369004551ea261f918dce3e 100644 (file)
 /************ 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)
index 6e7e82e06d01ed88bde8fcd2a74ef3b6081b2d75..594b7b0d564af493bea7214646422ba5e4957fe0 100644 (file)
@@ -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 */
index 0177419f44c9a45667684cda2228ea96f7ef9da2..524628559f49f914fe79f422d71d434b4a88e1b5 100644 (file)
@@ -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, &center) != 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(&center, &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;
+}
index 0cf01acb2e452eee3ec3fbe05afc658643d25860..c48b5925fa1397f583c5ed823c9899e7fefecf29 100644 (file)
@@ -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));
+}
+
index 9ceaf48ccbf78c636516d5239cb88099794ca8d6..f90475e03b2d097f6a0adbb06b656ef9851c15e1 100644 (file)
 #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);
index 098ffb34f529eba54dedd78582082903b1ecc922..c1dca26ae1b5d9633a65756fcbf0c36032519702 100644 (file)
@@ -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,
        },
 };
index a82f3f491c7f7bb2ececd1e3a3fe321ac8f82ed1..bab700db0c1cc0a4bcefe78fab4d4d23cd2c75c2 100644 (file)
@@ -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);
 
index 963336865661db443eb9305d98778e0a6e72ef75..082b6586abe70dbcbc56c4164842730e9cc89c7b 100755 (executable)
@@ -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);
index 79f91cd8cd091b304a2f315298afbc05d7dadf23..6811854a3e97a2c9cea034b6e58c191db7133335 100644 (file)
@@ -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
 //#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)
index 90ff487a29c14f2075183c1d1aba3d77350313dc..5b59a736ac6c982f8df6f94e64c5999c0d2209ff 100644 (file)
@@ -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
 
index 888d15a366761139e90a92339bc1074822db6d69..4ea3b806b07cf1e78326c43c3e016a99f54c2cc6 100644 (file)
@@ -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,
        },
 };
index 27be7839b87cd28ec6c6d655e38f9665c7e559bc..dfb7785da1d3f16900f265dd065c0e146125d4bd 100644 (file)
@@ -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 = {
index 62103c813fa2faefaff7534219b5883a411f07db..24f14bcb93930695efa9a3d3833dd43aab02fa13 100755 (executable)
@@ -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"));
index b820d9873674bdf2b64ad2567cb12c4667a8bf48..5bbf43f5ead6b9cefabc01e715bd16644ce53e1e 100644 (file)
@@ -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 */
index 4e634fcbb8fb5aefda7e51bc1e33d517dd139e8f..0ba6b0cbcec843c1e09f9a8bcda9b5473e6d4c26 100644 (file)
@@ -1,7 +1,7 @@
-/*  
+/*
  *  Copyright Droids Corporation (2009)
  *  Olivier MATZ <zer0@droids-corp.org>
- * 
+ *
  *  This program is free software; you can redistribute it and/or modify
  *  it under the terms of the GNU General Public License as published by
  *  the Free Software Foundation; either version 2 of the License, or
 #define ADC_MAX       4
 
 /* synchronize with sensor.c */
-#define S_CAP1          0
-#define S_COB_INSIDE_L  1
+#define S_COB_INSIDE_L  0
+#define S_CAP2          1
 #define S_COB_INSIDE_R  2
 #define S_CAP4          3
 #define S_LCOB          4
 #define S_LEFT          5
 #define S_RIGHT         6
-#define S_COL_RIGHT     7
+#define S_RCOB          7
 #define S_RESERVED1     8
 #define S_RESERVED2     9
 #define S_RESERVED3    10
index 39ce2bc33de99dc4e185c7e17844196d44861ee6..6d71e5876834f256d7a16fcc3c2cbbc1472874bf 100644 (file)
 #include <rdline.h>
 
 #include "main.h"
+#include "shovel.h"
 
 /* init spickle position at beginning */
 static void shovel_autopos(void)
 {
+       printf_P(PSTR("shovel autopos..."));
        pwm_ng_set(SHOVEL_PWM, -500);
        wait_ms(1000);
        pwm_ng_set(LEFT_SPICKLE_PWM, 0);
        encoders_spi_set_value(SHOVEL_ENCODER, 0);
+       printf_P(PSTR("ok\r\n"));
+}
+
+static uint8_t shovel_is_at_pos(int32_t pos)
+{
+       int32_t diff;
+       diff = pos - encoders_spi_get_value(SHOVEL_ENCODER);
+       if (diff < 0)
+               diff = -diff;
+       if (diff < 500)
+               return 1;
+       return 0;
+}
+
+uint8_t shovel_is_up(void)
+{
+       return shovel_is_at_pos(SHOVEL_UP);
+}
+
+uint8_t shovel_is_down(void)
+{
+       return shovel_is_at_pos(SHOVEL_DOWN);
 }
 
 void shovel_init(void)
index 7ba8445ec1301c9e9fc28f7093bcb242c3ae9d46..241b06503c1db0f9bfbfc13ddd308bde2b20752e 100644 (file)
@@ -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_ */
index 62732c208cdfa24589dcba374bb5144396a3ae1c..e1d9a7b3b7e8ea33703c39f5134096bbd489780f 100644 (file)
@@ -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;
 }
index d36f80f382a610f25f1090f609c2ce60109a0f72..81c74d8ccb48eb72e7444477c38d0535005ab487 100644 (file)
 
 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
index 33084050186c3e0e9406288604d5c0dd3e98812a..9d082bacf7aabb9b606b654d00fd635bac4e0e12 100644 (file)
@@ -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)) {
index b1f09d0a1b99516dba742176a2453d201c63a276..cdbeb6e1b57f3f4252462504582f16b6d55cc958 100644 (file)
@@ -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 */
index 272f1e933f5f782f08f3ce542583967ed59ec023..c749c612cfb2b8267e95c07e81fd9d38ac15058a 100644 (file)
@@ -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 <zer0@droids-corp.org> 
+ *  Olivier MATZ <zer0@droids-corp.org>
  */
 
 #include <stdio.h>
@@ -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,
        },
 };
index ced8e580001a90a2ecdd33dd1a76f946b6b9f214..fe4fabbc0aa9f8e78f5dc1559e2364d8dcb86e28 100644 (file)
@@ -22,6 +22,7 @@
 
 #include <stdio.h>
 #include <string.h>
+#include <math.h>
 
 #include <hostsim.h>
 #include <aversive/pgmspace.h>
@@ -40,6 +41,7 @@
 #include <quadramp.h>
 #include <control_system_manager.h>
 #include <trajectory_manager.h>
+#include <trajectory_manager_utils.h>
 #include <vect_base.h>
 #include <lines.h>
 #include <polygon.h>
@@ -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, &center) != 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(&center, &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";
index 81e2e0447aef1395a7945f4e2d7c0cc777f60e42..7bf6057b52a83be8ab6cfa1b92430a9141ac5ba1 100644 (file)
@@ -39,6 +39,7 @@
 #include <quadramp.h>
 #include <control_system_manager.h>
 #include <trajectory_manager.h>
+#include <trajectory_manager_utils.h>
 #include <vect_base.h>
 #include <lines.h>
 #include <polygon.h>
@@ -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";
index 81e52457f6af1b27a61a2778f3657241c9e43e53..905d533ed4e3369b27ea417c4b705871a5841d46 100644 (file)
@@ -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,
index 3ca1a473daa9ab0ce2403b8f5bf386e63b59d737..ad8cff204a1272c8aeb6e61a8cd26398d4ec2b1e 100644 (file)
@@ -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))
index 4061d195df164e12184fc7d4139099b20877bc78..88d4f97bacc796eb0ae9219259a50c5db4a2d9ca 100755 (executable)
@@ -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();
index 53cfe69acbabc90b9181ec058f6a09f356e8c7d7..48655eb0024f9263e9caa0741c9ac10cb270682c 100755 (executable)
 #define MATCH_TIME 89
 
 /* decrease track to decrease angle */
-#define EXT_TRACK_MM 302.0188
+#define EXT_TRACK_MM 304.61875
 #define VIRTUAL_TRACK_MM EXT_TRACK_MM
 
-#define ROBOT_LENGTH 320
+#define ROBOT_HALF_LENGTH_FRONT 130
+#define ROBOT_HALF_LENGTH_REAR 120
 #define ROBOT_WIDTH 320
 
-/* it is a 2048 imps -> 8192 because we see 1/4 period
- * and diameter: 55mm -> perimeter 173mm 
- * 8192/173 -> 473 */
+/* it is a 1024 imps -> 4096 because we see 1/4 period
+ * and diameter: 55mm -> perimeter 134mm
+ * dist_imp_mm = 4096/134 x 10 -> 304 */
 /* increase it to go further */
-#define IMP_ENCODERS 2048
-#define WHEEL_DIAMETER_MM 55.0
+#define IMP_ENCODERS 1024
+#define WHEEL_DIAMETER_MM 42.9
 #define WHEEL_PERIM_MM (WHEEL_DIAMETER_MM * M_PI)
 #define IMP_COEF 10.
 #define DIST_IMP_MM (((IMP_ENCODERS*4) / WHEEL_PERIM_MM) * IMP_COEF)
 #define I2C_POLL_PRIO       20
 #define EEPROM_TIME_PRIO    10
 
-#define CS_PERIOD 5000L
+#define CS_PERIOD 5000L /* in microsecond */ 
+#define CS_HZ (1000000. / CS_PERIOD)
 
 #define NB_LOGS 4
 
index 15b107563e38e59085e6b8dbc40ab6f00d11e8b5..d0aa3ae7ea00b7f2a7c14af014089db38f101667 100644 (file)
@@ -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)
index e92ff4f755afd7c07a2d65f99a8afc3a48f17d6b..53d9192d31caeecccad3d8a0722ae5948cd22406 100644 (file)
 #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 (file)
index 0000000..60dead3
--- /dev/null
@@ -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 <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <uart.h>
+#include <pwm_ng.h>
+#include <clock_time.h>
+#include <spi.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <vect_base.h>
+#include <lines.h>
+#include <polygon.h>
+#include <obstacle_avoidance.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+
+#include "../common/i2c_commands.h"
+
+#include "main.h"
+
index 40c6f8660b2036bf8c05d4df13fe4b73ad26f8b7..2c527780c7bf9ecafe8d67709144f6ed8ec82c7b 100644 (file)
@@ -38,6 +38,7 @@
 #include <quadramp.h>
 #include <control_system_manager.h>
 #include <trajectory_manager.h>
+#include <trajectory_manager_utils.h>
 #include <vect_base.h>
 #include <lines.h>
 #include <polygon.h>
index 06df8c7bc29a599875f084e996a5d634796f5086..708f08a27310fd9c18ff01b4436fdd8e0110417a 100644 (file)
@@ -37,6 +37,7 @@
 #include <quadramp.h>
 #include <control_system_manager.h>
 #include <trajectory_manager.h>
+#include <trajectory_manager_utils.h>
 #include <vect_base.h>
 #include <lines.h>
 #include <polygon.h>
index 85997c2c0a8373ff02438cafcf7d7df06cda7baf..7502a4355e8d70fbba17668cbe52e114ca3b697f 100644 (file)
  */
 
 
-#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;
index b7e5853bb663f4b4731b9536f85b476b5c4a4054..49d3649f1487667223028cac42a5a86734ae913a 100644 (file)
@@ -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)