save
authorzer0 <zer0@carbon.local>
Tue, 13 Apr 2010 18:16:32 +0000 (20:16 +0200)
committerzer0 <zer0@carbon.local>
Tue, 13 Apr 2010 18:16:32 +0000 (20:16 +0200)
34 files changed:
modules/devices/robot/blocking_detection_manager/blocking_detection_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_core.h
projects/microb2010/ballboard/actuator.c
projects/microb2010/ballboard/cmdline.h
projects/microb2010/ballboard/commands_ballboard.c
projects/microb2010/ballboard/commands_gen.c
projects/microb2010/ballboard/cs.c
projects/microb2010/ballboard/main.c
projects/microb2010/ballboard/main.h
projects/microb2010/ballboard/state.c
projects/microb2010/cobboard/actuator.c
projects/microb2010/cobboard/actuator.h
projects/microb2010/cobboard/commands_cobboard.c
projects/microb2010/cobboard/cs.c
projects/microb2010/cobboard/i2c_protocol.c
projects/microb2010/cobboard/main.c
projects/microb2010/cobboard/main.h
projects/microb2010/cobboard/state.c
projects/microb2010/cobboard/state.h
projects/microb2010/common/i2c_commands.h
projects/microb2010/mainboard/commands.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/i2c_protocol.c
projects/microb2010/mainboard/main.c
projects/microb2010/mainboard/main.h
projects/microb2010/mainboard/robotsim.c
projects/microb2010/mainboard/robotsim.h
projects/microb2010/mainboard/strat.c
projects/microb2010/mainboard/strat.h

index cdb2b58..8b5f547 100644 (file)
@@ -1,6 +1,6 @@
-/*  
+/*
  *  Copyright Droids Corporation (2007)
- * 
+ *
  *  This program is free software; you can redistribute it and/or modify
  *  it under the terms of the GNU General Public License as published by
  *  the Free Software Foundation; either version 2 of the License, or
@@ -38,8 +38,8 @@ void bd_init(struct blocking_detection * bd)
 }
 
 /* thresholds */
-void bd_set_current_thresholds(struct blocking_detection * bd, 
-                              int32_t k1, int32_t k2, 
+void bd_set_current_thresholds(struct blocking_detection * bd,
+                              int32_t k1, int32_t k2,
                               uint32_t i_thres, uint16_t cpt_thres)
 {
        uint8_t flags;
@@ -53,7 +53,7 @@ void bd_set_current_thresholds(struct blocking_detection * bd,
 }
 
 /* speed threshold */
-void bd_set_speed_threshold(struct blocking_detection * bd, 
+void bd_set_speed_threshold(struct blocking_detection * bd,
                            uint16_t speed)
 {
        uint8_t flags;
@@ -71,45 +71,48 @@ void bd_reset(struct blocking_detection * bd)
        IRQ_UNLOCK(flags);
 }
 
-
-
 /** function to be called periodically */
-void bd_manage_from_speed_cmd(struct blocking_detection * bd, 
-                             int32_t speed, int32_t cmd)          
+void bd_manage_from_speed_cmd(struct blocking_detection * bd,
+                             int32_t speed, int32_t cmd)
 {
-       int32_t i=0;
+       int32_t i = 0;
+
+       /* disabled */
+       if (bd->cpt_thres == 0)
+               return;
 
-       /* if current-based blocking_detection enabled */
-       if ( bd->cpt_thres ) {
-               i = bd->k1 * cmd - bd->k2 * speed;
-               if ((uint32_t)ABS(i) > bd->i_thres && 
-                   (bd->speed_thres == 0 || ABS(speed) < bd->speed_thres)) {
-                       if (bd->cpt == bd->cpt_thres - 1)
-                               WARNING(E_BLOCKING_DETECTION_MANAGER, 
-                                     "BLOCKING cmd=%ld, speed=%ld i=%ld",
-                                     cmd, speed, i);
-                       if (bd->cpt < bd->cpt_thres)
-                               bd->cpt++;
-               }
-               else {
-                       bd->cpt=0;
-               }
+       i = bd->k1 * cmd - bd->k2 * speed;
+
+       /* if i is above threshold, speed is below threshold, and cmd
+        * has the same sign than i */
+       if ((uint32_t)ABS(i) > bd->i_thres &&
+           (bd->speed_thres == 0 || ABS(speed) < bd->speed_thres) &&
+           (i * cmd > 0)) {
+               if (bd->cpt == bd->cpt_thres - 1)
+                       WARNING(E_BLOCKING_DETECTION_MANAGER,
+                               "BLOCKING cmd=%ld, speed=%ld i=%ld",
+                               cmd, speed, i);
+               if (bd->cpt < bd->cpt_thres)
+                       bd->cpt++;
+       }
+       else {
+               bd->cpt=0;
+       }
 #if BD_DEBUG
-               if (bd->debug_cpt++ == BD_DEBUG) {
-                       DEBUG(E_BLOCKING_DETECTION_MANAGER, "cmd=%ld, speed=%ld i=%ld",
-                             cmd, speed, i);
-                       bd->debug_cpt = 0;
-               }
+       if (bd->debug_cpt++ == BD_DEBUG) {
+               DEBUG(E_BLOCKING_DETECTION_MANAGER, "cmd=%ld, speed=%ld i=%ld",
+                     cmd, speed, i);
+               bd->debug_cpt = 0;
        }
 #endif
 }
 
 /** function to be called periodically */
-void bd_manage_from_pos_cmd(struct blocking_detection * bd, 
-                           int32_t pos, int32_t cmd)   
+void bd_manage_from_pos_cmd(struct blocking_detection * bd,
+                           int32_t pos, int32_t cmd)
 {
        int32_t speed = (pos - bd->prev_pos);
-       bd_manage_from_speed_cmd(bd, speed, cmd);       
+       bd_manage_from_speed_cmd(bd, speed, cmd);
        bd->prev_pos = pos;
 }
 
index 594b7b0..3e22502 100644 (file)
@@ -130,7 +130,7 @@ void trajectory_set_robot_params(struct trajectory *traj,
 void trajectory_set_speed(struct trajectory *traj, double d_speed, double a_speed);
 
 /** set speed consign */
-void trajectory_set_speed(struct trajectory *traj, double d_acc, double a_acc);
+void trajectory_set_acc(struct trajectory *traj, double d_acc, double a_acc);
 
 /**
  * set windows for trajectory.
index 5246285..5197300 100644 (file)
@@ -643,13 +643,11 @@ static void trajectory_manager_line_event(struct trajectory *traj)
        }
 
        /* position consign is infinite */
-       d_consign = (int32_t)(v2pol_target.r * (traj->position->phys.distance_imp_per_mm));
+       d_consign = pos_mm2imp(traj, v2pol_target.r);
        d_consign += rs_get_distance(traj->robot);
 
-       /* angle consign */
-       a_consign = (int32_t)(v2pol_target.theta *
-                             (traj->position->phys.distance_imp_per_mm) *
-                             (traj->position->phys.track_mm) / 2.2);
+       /* angle consign (1.1 to avoid oscillations) */
+       a_consign = pos_rd2imp(traj, v2pol_target.theta) / 1.1;
        a_consign += rs_get_angle(traj->robot);
 
        EVT_DEBUG(E_TRAJECTORY, "target.x=%2.2f target.y=%2.2f "
@@ -849,7 +847,7 @@ void trajectory_line_abs(struct trajectory *traj,
  * - Va_out: the angular speed to configure in quadramp
  * - remain_d_mm_out: remaining distance before start to turn
  */
-static uint8_t calc_clitoid(struct trajectory *traj,
+static int8_t calc_clitoid(struct trajectory *traj,
                            double x, double y, double a_rad,
                            double alpha_deg, double beta_deg, double R_mm,
                            double Vd, double Amax, double d_inter_mm,
@@ -857,12 +855,14 @@ static uint8_t calc_clitoid(struct trajectory *traj,
 {
        double Vd_mm_s;
        double Va, Va_rd_s;
-       double t, d_mm, alpha_rad, beta_rad;
+       double t, tau, d_mm, alpha_rad, beta_rad;
        double remain_d_mm;
        double Aa, Aa_rd_s2;
        line_t line1, line2;
-       point_t robot, intersect, pt2, center, proj;
+       line_t line1_int, line2_int;
+       point_t robot, intersect, pt2, center, proj, M;
        vect_t v;
+       double xm, ym, L, A;
 
        /* param check */
        if (fabs(alpha_deg) <= fabs(beta_deg)) {
@@ -896,11 +896,6 @@ static uint8_t calc_clitoid(struct trajectory *traj,
                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;
@@ -913,46 +908,74 @@ static uint8_t calc_clitoid(struct trajectory *traj,
        DEBUG(E_TRAJECTORY, "intersect=(%2.2f, %2.2f)",
              intersect.x, intersect.y);
 
-       /* the center of the circle is at (d_mm, d_mm) when we have to
-        * start the clothoid */
-       d_mm = R_mm * sqrt(fabs(alpha_rad - beta_rad)) *
-               sqrt(M_PI) / 2.;
+       /* L and A are the parameters of the clothoid, xm and ym are
+        * the relative coords (starting from the beginning of
+        * clothoid) of the crossing point between the clothoid and
+        * the circle. */
+       L = Vd_mm_s * t;
+       A = R_mm * sqrt(fabs(alpha_rad - beta_rad));
+       xm =
+               L
+               - (pow(L, 5) / (40. * pow(A, 4)))
+               + (pow(L, 9) / (3456. * pow(A, 8)))
+               - (pow(L, 13) / (599040. * pow(A, 12)));
+       ym =
+               (pow(L, 3) / (6. * pow(A, 2)))
+               - (pow(L, 7) / (336. * pow(A, 6)))
+               + (pow(L, 11) / (42240. * pow(A, 10)))
+               - (pow(L, 15) / (9676800. * pow(A, 14)));
+       DEBUG(E_TRAJECTORY, "relative xm,ym = (%2.2f, %2.2f)",
+             xm, ym);
+
+       /* the center of the circle is at d_mm when we have to start
+        * the clothoid */
+       tau = (alpha_rad - beta_rad) / 2.;
+       d_mm = ym + (R_mm * cos(tau));
        DEBUG(E_TRAJECTORY, "d_mm=%2.2f", d_mm);
 
        /* translate line1 */
+       memcpy(&line1_int, &line1, sizeof(line1_int));
+       memcpy(&line2_int, &line2, sizeof(line2_int));
        v.x = intersect.x - robot.x;
        v.y = intersect.y - robot.y;
-       if (a_rad > 0)
+       if (alpha_rad > 0)
                vect_rot_trigo(&v);
        else
                vect_rot_retro(&v);
        vect_resize(&v, d_mm);
-       line_translate(&line1, &v);
+       line_translate(&line1_int, &v);
+       DEBUG(E_TRAJECTORY, "translate line1 by %2.2f,%2.2f", v.x, v.y);
 
-       /* translate line2 */
+       /* translate line2_int */
        v.x = intersect.x - pt2.x;
        v.y = intersect.y - pt2.y;
-       if (a_rad > 0)
+       if (alpha_rad < 0)
                vect_rot_trigo(&v);
        else
                vect_rot_retro(&v);
        vect_resize(&v, d_mm);
-       line_translate(&line2, &v);
+       line_translate(&line2_int, &v);
+       DEBUG(E_TRAJECTORY, "translate line2 by %2.2f,%2.2f", v.x, v.y);
 
        /* find the center of the circle, at the intersection of the
         * new translated lines */
-       if (intersect_line(&line1, &line2, &center) != 1) {
+       if (intersect_line(&line1_int, &line2_int, &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);
+       /* M is the same point than xm, ym but in absolute coords */
+       M.x = center.x + cos(a_rad - M_PI/2 + tau) * R_mm;
+       M.y = center.y + sin(a_rad - M_PI/2 + tau) * R_mm;
+       DEBUG(E_TRAJECTORY, "absolute M = (%2.2f, %2.2f)", M.x, M.y);
+
+       /* project M on line 1 */
+       proj_pt_line(&M, &line1, &proj);
+       DEBUG(E_TRAJECTORY, "proj M = (%2.2f, %2.2f)", proj.x, proj.y);
 
        /* process remaining distance before start turning */
-       remain_d_mm = d_inter_mm - (pt_norm(&proj, &intersect) + d_mm);
+       remain_d_mm = d_inter_mm - (pt_norm(&proj, &intersect) + xm);
        DEBUG(E_TRAJECTORY, "remain_d=%2.2f", remain_d_mm);
        if (remain_d_mm < 0) {
                DEBUG(E_TRAJECTORY, "too late, cannot turn");
@@ -966,6 +989,7 @@ static uint8_t calc_clitoid(struct trajectory *traj,
        return 0;
 }
 
+/* after the line, start the clothoid */
 static void start_clitoid(struct trajectory *traj)
 {
        double Aa = traj->target.line.Aa;
@@ -975,12 +999,14 @@ static void start_clitoid(struct trajectory *traj)
        double d;
 
        delete_event(traj);
+       DEBUG(E_TRAJECTORY, "%s() Va=%2.2f Aa=%2.2f",
+             __FUNCTION__, Va, Aa);
        traj->state = RUNNING_CLITOID_CURVE;
-       set_quadramp_acc(traj, Aa, Aa);
-       set_quadramp_speed(traj, Va, Va);
-       d = R_mm * a_rad;
-       d *= 2.; /* margin to avoid deceleration */
+       d = fabs(R_mm * a_rad);
+       d *= 3.; /* margin to avoid deceleration */
        trajectory_d_a_rel(traj, d, DEG(a_rad));
+       set_quadramp_acc(traj, traj->d_acc, Aa);
+       set_quadramp_speed(traj, traj->d_speed, Va);
 }
 
 
@@ -1010,11 +1036,12 @@ static void start_clitoid(struct trajectory *traj)
 int8_t trajectory_clitoid(struct trajectory *traj,
                          double x, double y, double a, double advance,
                          double alpha_deg, double beta_deg, double R_mm,
-                         double Vd, double d_inter_mm)
+                         double d_inter_mm)
 {
-       double remain = 0, Aa = 0, Va = 0;
+       double remain = 0, Aa = 0, Va = 0, Vd;
        double turnx, turny;
 
+       Vd = traj->d_speed;
        if (calc_clitoid(traj, x, y, a, alpha_deg, beta_deg, R_mm,
                         Vd, traj->a_acc, d_inter_mm,
                         &Aa, &Va, &remain) < 0)
@@ -1029,6 +1056,9 @@ int8_t trajectory_clitoid(struct trajectory *traj,
        traj->target.line.R = R_mm;
        traj->target.line.turn_pt.x = turnx;
        traj->target.line.turn_pt.y = turny;
+       DEBUG(E_TRAJECTORY, "%s() turn_pt=%2.2f,%2.2f",
+             __FUNCTION__, turnx, turny);
+
        __trajectory_line_abs(traj, x, y, turnx, turny,
                              advance);
        traj->state = RUNNING_CLITOID_LINE;
index a724f69..b5a1395 100644 (file)
@@ -111,3 +111,33 @@ void trajectory_manager_event(void * param);
 /*********** *CIRCLE */
 
 /* XXX TODO */
+
+/*********** CLITOID */
+
+/**
+ * do a superb curve joining line1 to line2 which is composed of:
+ *   - a clothoid starting from line1
+ *   - a circle
+ *   - another clothoid up to line2
+ * this curve is called a clitoid (hehe)
+ *
+ * the function assumes that the initial linear speed is Vd and
+ * angular speed is 0.
+ *
+ * - x,y,a: starting position
+ * - advance: parameter for line following
+ * - alpha: total angle
+ * - beta: circular part of angle (lower than alpha)
+ * - R: the radius of the circle (must be != 0)
+ * - Vd: linear speed to use (in imp per cs period)
+ * - Amax: maximum angular acceleration
+ * - d_inter: distance in mm until the intersection of the
+ *            2 lines
+ *
+ * return 0 if trajectory can be loaded, then it is processed in
+ * background.
+ */
+int8_t trajectory_clitoid(struct trajectory *traj,
+                         double x, double y, double a, double advance,
+                         double alpha_deg, double beta_deg, double R_mm,
+                         double d_inter_mm);
index 2509d7c..7365427 100644 (file)
 
 #include "main.h"
 
-#define ROLLER_ON      800
+#define ROLLER_ON      -1200
 #define ROLLER_OFF     0
-#define ROLLER_REVERSE -800
+#define ROLLER_REVERSE 1200
 
-#define FORKROT_DEPLOYED 1000
+#define FORKROT_DEPLOYED -50000
 #define FORKROT_PACKED   0
 
 #define FORKTRANS_LEFT   0
index 57dfa49..04ce1f3 100644 (file)
@@ -35,6 +35,6 @@ static inline uint8_t cmdline_keypressed(void) {
        return (uart_recv_nowait(CMDLINE_UART) != -1);
 }
 
-static inline uint8_t cmdline_getchar(void) {
+static inline int16_t cmdline_getchar(void) {
        return uart_recv_nowait(CMDLINE_UART);
 }
index fd45914..ba570a4 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
@@ -17,7 +17,7 @@
  *
  *  Revision : $Id: commands_ballboard.c,v 1.2 2009-04-24 19:30:42 zer0 Exp $
  *
- *  Olivier MATZ <zer0@droids-corp.org> 
+ *  Olivier MATZ <zer0@droids-corp.org>
  */
 
 #include <stdio.h>
@@ -63,22 +63,24 @@ static void cmd_event_parsed(void *parsed_result, void *data)
        u08 bit=0;
 
        struct cmd_event_result * res = parsed_result;
-       
+
        if (!strcmp_P(res->arg1, PSTR("all"))) {
-               bit = DO_ENCODERS | DO_CS | DO_BD | DO_POWER;
+               bit = 0xFF;
                if (!strcmp_P(res->arg2, PSTR("on")))
                        ballboard.flags |= bit;
                else if (!strcmp_P(res->arg2, PSTR("off")))
                        ballboard.flags &= bit;
                else { /* show */
-                       printf_P(PSTR("encoders is %s\r\n"), 
+                       printf_P(PSTR("encoders is %s\r\n"),
                                 (DO_ENCODERS & ballboard.flags) ? "on":"off");
-                       printf_P(PSTR("cs is %s\r\n"), 
+                       printf_P(PSTR("cs is %s\r\n"),
                                 (DO_CS & ballboard.flags) ? "on":"off");
-                       printf_P(PSTR("bd is %s\r\n"), 
+                       printf_P(PSTR("bd is %s\r\n"),
                                 (DO_BD & ballboard.flags) ? "on":"off");
-                       printf_P(PSTR("power is %s\r\n"), 
+                       printf_P(PSTR("power is %s\r\n"),
                                 (DO_POWER & ballboard.flags) ? "on":"off");
+                       printf_P(PSTR("errblock is %s\r\n"),
+                                (DO_ERRBLOCKING & ballboard.flags) ? "on":"off");
                }
                return;
        }
@@ -92,6 +94,8 @@ static void cmd_event_parsed(void *parsed_result, void *data)
                bit = DO_BD;
        else if (!strcmp_P(res->arg1, PSTR("power")))
                bit = DO_POWER;
+       else if (!strcmp_P(res->arg1, PSTR("errblock")))
+               bit = DO_ERRBLOCKING;
 
 
        if (!strcmp_P(res->arg2, PSTR("on")))
@@ -104,13 +108,13 @@ static void cmd_event_parsed(void *parsed_result, void *data)
                }
                ballboard.flags &= (~bit);
        }
-       printf_P(PSTR("%s is %s\r\n"), res->arg1, 
+       printf_P(PSTR("%s is %s\r\n"), res->arg1,
                      (bit & ballboard.flags) ? "on":"off");
 }
 
 prog_char str_event_arg0[] = "event";
 parse_pgm_token_string_t cmd_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg0, str_event_arg0);
-prog_char str_event_arg1[] = "all#encoders#cs#bd#power";
+prog_char str_event_arg1[] = "all#encoders#cs#bd#power#errblock";
 parse_pgm_token_string_t cmd_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg1, str_event_arg1);
 prog_char str_event_arg2[] = "on#off#show";
 parse_pgm_token_string_t cmd_event_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg2, str_event_arg2);
@@ -121,9 +125,9 @@ parse_pgm_inst_t cmd_event = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_event,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_event_arg0, 
-               (prog_void *)&cmd_event_arg1, 
-               (prog_void *)&cmd_event_arg2, 
+               (prog_void *)&cmd_event_arg0,
+               (prog_void *)&cmd_event_arg1,
+               (prog_void *)&cmd_event_arg2,
                NULL,
        },
 };
@@ -161,8 +165,8 @@ parse_pgm_inst_t cmd_color = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_color,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_color_arg0, 
-               (prog_void *)&cmd_color_color, 
+               (prog_void *)&cmd_color_arg0,
+               (prog_void *)&cmd_color_color,
                NULL,
        },
 };
@@ -275,7 +279,7 @@ struct cmd_state3_result {
 };
 
 /* function called when cmd_state3 is parsed successfully */
-static void cmd_state3_parsed(void *parsed_result, 
+static void cmd_state3_parsed(void *parsed_result,
                              __attribute__((unused)) void *data)
 {
        struct cmd_state3_result *res = parsed_result;
@@ -300,9 +304,9 @@ parse_pgm_inst_t cmd_state3 = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_state3,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_state3_arg0, 
-               (prog_void *)&cmd_state3_arg1, 
-               (prog_void *)&cmd_state3_arg2, 
+               (prog_void *)&cmd_state3_arg0,
+               (prog_void *)&cmd_state3_arg1,
+               (prog_void *)&cmd_state3_arg2,
                NULL,
        },
 };
@@ -331,7 +335,7 @@ parse_pgm_inst_t cmd_state_machine = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_state_machine,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_state_machine_arg0, 
+               (prog_void *)&cmd_state_machine_arg0,
                NULL,
        },
 };
@@ -363,8 +367,8 @@ parse_pgm_inst_t cmd_state_debug = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_state_debug,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_state_debug_arg0, 
-               (prog_void *)&cmd_state_debug_on, 
+               (prog_void *)&cmd_state_debug_arg0,
+               (prog_void *)&cmd_state_debug_on,
                NULL,
        },
 };
@@ -406,8 +410,8 @@ parse_pgm_inst_t cmd_fork = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_fork,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_fork_arg0, 
-               (prog_void *)&cmd_fork_arg1, 
+               (prog_void *)&cmd_fork_arg0,
+               (prog_void *)&cmd_fork_arg1,
                NULL,
        },
 };
@@ -445,8 +449,8 @@ parse_pgm_inst_t cmd_roller = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_roller,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_roller_arg0, 
-               (prog_void *)&cmd_roller_arg1, 
+               (prog_void *)&cmd_roller_arg0,
+               (prog_void *)&cmd_roller_arg1,
                NULL,
        },
 };
@@ -474,7 +478,7 @@ parse_pgm_inst_t cmd_test = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_test,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_test_arg0, 
+               (prog_void *)&cmd_test_arg0,
                NULL,
        },
 };
index b2a1e5d..66dccc1 100644 (file)
@@ -357,9 +357,7 @@ static const prog_char i2c_log[] = "i2c";
 static const prog_char i2cproto_log[] = "i2cproto";
 static const prog_char sensor_log[] = "sensor";
 static const prog_char block_log[] = "bd";
-static const prog_char beacon_log[] = "beacon";
-static const prog_char scanner_log[] = "scanner";
-static const prog_char imgprocess_log[] = "imgprocess";
+static const prog_char state_log[] = "state";
 
 struct log_name_and_num {
        const prog_char * name;
@@ -372,9 +370,7 @@ static const struct log_name_and_num log_name_and_num[] = {
        { i2cproto_log, E_USER_I2C_PROTO },
        { sensor_log, E_USER_SENSOR },
        { block_log, E_BLOCKING_DETECTION_MANAGER },
-       { beacon_log, E_USER_BEACON },
-       { scanner_log, E_USER_SCANNER },
-       { imgprocess_log, E_USER_IMGPROCESS },
+       { state_log, E_USER_ST_MACH },
 };
 
 static uint8_t
@@ -523,7 +519,7 @@ static void cmd_log_type_parsed(void * parsed_result, void * data)
 prog_char str_log_arg1_type[] = "type";
 parse_pgm_token_string_t cmd_log_arg1_type = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg1, str_log_arg1_type);
 /* keep it sync with log_name_and_num above */
-prog_char str_log_arg2_type[] = "uart#i2c#i2cproto#sensor#bd#beacon#scanner#imgprocess";
+prog_char str_log_arg2_type[] = "uart#i2c#i2cproto#sensor#bd#state";
 parse_pgm_token_string_t cmd_log_arg2_type = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg2, str_log_arg2_type);
 prog_char str_log_arg3[] = "on#off";
 parse_pgm_token_string_t cmd_log_arg3 = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg3, str_log_arg3);
index bab700d..cf122e6 100644 (file)
@@ -1,7 +1,7 @@
-/*  
+/*
  *  Copyright Droids Corporation
  *  Olivier Matz <zer0@droids-corp.org>
- * 
+ *
  *  This program is free software; you can redistribute it and/or modify
  *  it under the terms of the GNU General Public License as published by
  *  the Free Software Foundation; either version 2 of the License, or
@@ -72,10 +72,20 @@ static void do_cs(void *dummy)
                if (ballboard.forkrot.on)
                        cs_manage(&ballboard.forkrot.cs);
        }
-       if (ballboard.flags & DO_BD) {
+       if ((ballboard.flags & DO_BD) && (ballboard.flags & DO_POWER)) {
                bd_manage_from_cs(&ballboard.roller.bd, &ballboard.roller.cs);
                bd_manage_from_cs(&ballboard.forktrans.bd, &ballboard.forktrans.cs);
                bd_manage_from_cs(&ballboard.forkrot.bd, &ballboard.forkrot.cs);
+
+               /* urgent case: stop power on blocking */
+               if (ballboard.flags & DO_ERRBLOCKING) {
+                       if (bd_get(&ballboard.roller.bd) ||
+                           bd_get(&ballboard.forktrans.bd) ||
+                           bd_get(&ballboard.forkrot.bd)) {
+                               printf_P(PSTR("MOTOR BLOCKED STOP ALL\r\n"));
+                               ballboard.flags &= ~(DO_POWER | DO_ERRBLOCKING);
+                       }
+               }
        }
        if (ballboard.flags & DO_POWER)
                BRAKE_OFF();
@@ -86,7 +96,7 @@ static void do_cs(void *dummy)
 void dump_cs(const char *name, struct cs *cs)
 {
        printf_P(PSTR("%s cons=% .5ld fcons=% .5ld err=% .5ld "
-                     "in=% .5ld out=% .5ld\r\n"), 
+                     "in=% .5ld out=% .5ld\r\n"),
                 name, cs_get_consign(cs), cs_get_filtered_consign(cs),
                 cs_get_error(cs), cs_get_filtered_feedback(cs),
                 cs_get_out(cs));
@@ -119,10 +129,15 @@ void microb_cs_init(void)
        cs_set_process_out(&ballboard.roller.cs, encoders_spi_update_roller_speed, ROLLER_ENCODER);
        cs_set_consign(&ballboard.roller.cs, 0);
 
+       /* Blocking detection */
+       bd_init(&ballboard.roller.bd);
+       bd_set_speed_threshold(&ballboard.roller.bd, 150);
+       bd_set_current_thresholds(&ballboard.roller.bd, 500, 8000, 1000000, 200);
+
        /* ---- CS forktrans */
        /* PID */
        pid_init(&ballboard.forktrans.pid);
-       pid_set_gains(&ballboard.forktrans.pid, 200, 5, 250);
+       pid_set_gains(&ballboard.forktrans.pid, 30, 5, 0);
        pid_set_maximums(&ballboard.forktrans.pid, 0, 10000, 2047);
        pid_set_out_shift(&ballboard.forktrans.pid, 6);
        pid_set_derivate_filter(&ballboard.forktrans.pid, 6);
@@ -143,19 +158,19 @@ void microb_cs_init(void)
        /* Blocking detection */
        bd_init(&ballboard.forktrans.bd);
        bd_set_speed_threshold(&ballboard.forktrans.bd, 150);
-       bd_set_current_thresholds(&ballboard.forktrans.bd, 500, 8000, 1000000, 40);
+       bd_set_current_thresholds(&ballboard.forktrans.bd, 500, 8000, 1000000, 200);
 
        /* ---- CS forkrot */
        /* PID */
        pid_init(&ballboard.forkrot.pid);
-       pid_set_gains(&ballboard.forkrot.pid, 200, 5, 250);
+       pid_set_gains(&ballboard.forkrot.pid, 30, 5, 0);
        pid_set_maximums(&ballboard.forkrot.pid, 0, 10000, 2047);
        pid_set_out_shift(&ballboard.forkrot.pid, 6);
        pid_set_derivate_filter(&ballboard.forkrot.pid, 6);
 
        /* QUADRAMP */
        quadramp_init(&ballboard.forkrot.qr);
-       quadramp_set_1st_order_vars(&ballboard.forkrot.qr, 200, 200); /* set speed */
+       quadramp_set_1st_order_vars(&ballboard.forkrot.qr, 800, 800); /* set speed */
        quadramp_set_2nd_order_vars(&ballboard.forkrot.qr, 20, 20); /* set accel */
 
        /* CS */
@@ -169,15 +184,15 @@ void microb_cs_init(void)
        /* Blocking detection */
        bd_init(&ballboard.forkrot.bd);
        bd_set_speed_threshold(&ballboard.forkrot.bd, 150);
-       bd_set_current_thresholds(&ballboard.forkrot.bd, 500, 8000, 1000000, 40);
+       bd_set_current_thresholds(&ballboard.forkrot.bd, 500, 8000, 1000000, 200);
 
        /* set them on !! */
-       ballboard.roller.on = 0;
+       ballboard.roller.on = 1;
        ballboard.forktrans.on = 1;
        ballboard.forkrot.on = 1;
 
 
        scheduler_add_periodical_event_priority(do_cs, NULL,
-                                               5000L / SCHEDULER_UNIT, 
+                                               5000L / SCHEDULER_UNIT,
                                                CS_PRIO);
 }
index 082b658..ed8716b 100755 (executable)
@@ -112,16 +112,21 @@ void bootloader(void)
 
 void do_led_blink(__attribute__((unused)) void *dummy)
 {
-#if 1 /* simple blink */
-       static uint8_t a=0;
+       static uint8_t a = 0;
 
-       if(a)
-               LED1_ON();
-       else
-               LED1_OFF();
-
-       a = !a;
-#endif
+       if (ballboard.flags & DO_ERRBLOCKING) {
+               if (a & 1)
+                       LED1_ON();
+               else
+                       LED1_OFF();
+       }
+       else {
+               if (a & 4)
+                       LED1_ON();
+               else
+                       LED1_OFF();
+       }
+       a++;
 }
 
 static void main_timer_interrupt(void)
@@ -149,7 +154,8 @@ int main(void)
        LED1_OFF();
        memset(&gen, 0, sizeof(gen));
        memset(&ballboard, 0, sizeof(ballboard));
-       ballboard.flags = DO_ENCODERS | DO_CS | DO_POWER; // DO_BD
+       ballboard.flags = DO_ENCODERS | DO_CS | DO_POWER |
+               DO_ERRBLOCKING | DO_BD;
 
        /* UART */
        uart_init();
@@ -239,6 +245,9 @@ int main(void)
        /* ax12 */
        ax12_user_init();
 
+       gen.logs[0] = E_USER_ST_MACH;
+       gen.log_level = 5;
+
        sei();
 
        printf_P(PSTR("\r\n"));
index 3ebb1e7..cde4608 100755 (executable)
@@ -60,9 +60,7 @@
 /** ERROR NUMS */
 #define E_USER_I2C_PROTO       195
 #define E_USER_SENSOR          196
-#define E_USER_BEACON          197
-#define E_USER_SCANNER         198
-#define E_USER_IMGPROCESS      199
+#define E_USER_ST_MACH         197
 
 #define LED_PRIO           170
 #define TIME_PRIO          160
@@ -115,6 +113,7 @@ struct ballboard {
 #define DO_CS        2
 #define DO_BD        4
 #define DO_POWER     8
+#define DO_ERRBLOCKING 16
        uint8_t flags;                /* misc flags */
 
        /* control systems */
index 9568be8..0492e18 100644 (file)
@@ -94,6 +94,7 @@ static void state_debug_wait_key_pressed(void)
 int8_t state_set_mode(uint8_t mode)
 {
        state_mode = mode;
+       STMCH_DEBUG("%s(): mode=%x ", __FUNCTION__, mode);
 
 /*     STMCH_DEBUG("%s(): l_deploy=%d l_harvest=%d " */
 /*                 "r_deploy=%d r_harvest=%d eject=%d", */
@@ -135,6 +136,7 @@ static void state_do_harvest(void)
 static void state_do_eject(void)
 {
        roller_reverse();
+       time_wait_ms(2000);
 }
 
 /* main state machine */
index 6811854..1b6e91f 100644 (file)
@@ -103,6 +103,14 @@ void cobroller_off(uint8_t side)
                cobboard.right_cobroller_speed = 0;
 }
 
+void cobroller_reverse(uint8_t side)
+{
+       if (side == I2C_LEFT_SIDE)
+               cobboard.left_cobroller_speed = -COBROLLER_SPEED;
+       else
+               cobboard.right_cobroller_speed = COBROLLER_SPEED;
+}
+
 void actuator_init(void)
 {
 
index 5b59a73..4896447 100644 (file)
@@ -33,6 +33,7 @@ void servo_door_block(void);
 
 void cobroller_on(uint8_t side);
 void cobroller_off(uint8_t side);
+void cobroller_reverse(uint8_t side);
 
 #endif
 
index 4ea3b80..bea27e9 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
@@ -17,7 +17,7 @@
  *
  *  Revision : $Id: commands_cobboard.c,v 1.6 2009-11-08 17:25:00 zer0 Exp $
  *
- *  Olivier MATZ <zer0@droids-corp.org> 
+ *  Olivier MATZ <zer0@droids-corp.org>
  */
 
 #include <stdio.h>
@@ -68,20 +68,22 @@ static void cmd_event_parsed(void *parsed_result, __attribute__((unused)) void *
        struct cmd_event_result * res = parsed_result;
 
        if (!strcmp_P(res->arg1, PSTR("all"))) {
-               bit = DO_ENCODERS | DO_CS | DO_BD | DO_POWER;
+               bit = 0xFF;
                if (!strcmp_P(res->arg2, PSTR("on")))
                        cobboard.flags |= bit;
                else if (!strcmp_P(res->arg2, PSTR("off")))
                        cobboard.flags &= bit;
                else { /* show */
-                       printf_P(PSTR("encoders is %s\r\n"), 
+                       printf_P(PSTR("encoders is %s\r\n"),
                                 (DO_ENCODERS & cobboard.flags) ? "on":"off");
-                       printf_P(PSTR("cs is %s\r\n"), 
+                       printf_P(PSTR("cs is %s\r\n"),
                                 (DO_CS & cobboard.flags) ? "on":"off");
-                       printf_P(PSTR("bd is %s\r\n"), 
+                       printf_P(PSTR("bd is %s\r\n"),
                                 (DO_BD & cobboard.flags) ? "on":"off");
-                       printf_P(PSTR("power is %s\r\n"), 
+                       printf_P(PSTR("power is %s\r\n"),
                                 (DO_POWER & cobboard.flags) ? "on":"off");
+                       printf_P(PSTR("errblock is %s\r\n"),
+                                (DO_ERRBLOCKING & cobboard.flags) ? "on":"off");
                }
                return;
        }
@@ -95,6 +97,8 @@ static void cmd_event_parsed(void *parsed_result, __attribute__((unused)) void *
                bit = DO_BD;
        else if (!strcmp_P(res->arg1, PSTR("power")))
                bit = DO_POWER;
+       else if (!strcmp_P(res->arg1, PSTR("errblock")))
+               bit = DO_ERRBLOCKING;
 
 
        if (!strcmp_P(res->arg2, PSTR("on")))
@@ -107,13 +111,13 @@ static void cmd_event_parsed(void *parsed_result, __attribute__((unused)) void *
                }
                cobboard.flags &= (~bit);
        }
-       printf_P(PSTR("%s is %s\r\n"), res->arg1, 
+       printf_P(PSTR("%s is %s\r\n"), res->arg1,
                      (bit & cobboard.flags) ? "on":"off");
 }
 
 prog_char str_event_arg0[] = "event";
 parse_pgm_token_string_t cmd_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg0, str_event_arg0);
-prog_char str_event_arg1[] = "all#encoders#cs#bd#power";
+prog_char str_event_arg1[] = "all#encoders#cs#bd#power#errblock";
 parse_pgm_token_string_t cmd_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg1, str_event_arg1);
 prog_char str_event_arg2[] = "on#off#show";
 parse_pgm_token_string_t cmd_event_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg2, str_event_arg2);
@@ -124,9 +128,9 @@ parse_pgm_inst_t cmd_event = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_event,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_event_arg0, 
-               (prog_void *)&cmd_event_arg1, 
-               (prog_void *)&cmd_event_arg2, 
+               (prog_void *)&cmd_event_arg0,
+               (prog_void *)&cmd_event_arg1,
+               (prog_void *)&cmd_event_arg2,
                NULL,
        },
 };
@@ -164,8 +168,8 @@ parse_pgm_inst_t cmd_color = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_color,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_color_arg0, 
-               (prog_void *)&cmd_color_color, 
+               (prog_void *)&cmd_color_arg0,
+               (prog_void *)&cmd_color_color,
                NULL,
        },
 };
@@ -204,8 +208,8 @@ parse_pgm_inst_t cmd_state1 = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_state1,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_state1_arg0, 
-               (prog_void *)&cmd_state1_arg1, 
+               (prog_void *)&cmd_state1_arg0,
+               (prog_void *)&cmd_state1_arg1,
                NULL,
        },
 };
@@ -272,9 +276,9 @@ parse_pgm_inst_t cmd_state2 = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_state2,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_state2_arg0, 
-               (prog_void *)&cmd_state2_arg1, 
-               (prog_void *)&cmd_state2_arg2, 
+               (prog_void *)&cmd_state2_arg0,
+               (prog_void *)&cmd_state2_arg1,
+               (prog_void *)&cmd_state2_arg2,
                NULL,
        },
 };
@@ -290,7 +294,7 @@ struct cmd_state3_result {
 };
 
 /* function called when cmd_state3 is parsed successfully */
-static void cmd_state3_parsed(void *parsed_result, 
+static void cmd_state3_parsed(void *parsed_result,
                              __attribute__((unused)) void *data)
 {
        struct cmd_state3_result *res = parsed_result;
@@ -315,9 +319,9 @@ parse_pgm_inst_t cmd_state3 = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_state3,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_state3_arg0, 
-               (prog_void *)&cmd_state3_arg1, 
-               (prog_void *)&cmd_state3_arg2, 
+               (prog_void *)&cmd_state3_arg0,
+               (prog_void *)&cmd_state3_arg1,
+               (prog_void *)&cmd_state3_arg2,
                NULL,
        },
 };
@@ -346,7 +350,7 @@ parse_pgm_inst_t cmd_state_machine = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_state_machine,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_state_machine_arg0, 
+               (prog_void *)&cmd_state_machine_arg0,
                NULL,
        },
 };
@@ -378,8 +382,8 @@ parse_pgm_inst_t cmd_state_debug = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_state_debug,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_state_debug_arg0, 
-               (prog_void *)&cmd_state_debug_on, 
+               (prog_void *)&cmd_state_debug_arg0,
+               (prog_void *)&cmd_state_debug_on,
                NULL,
        },
 };
@@ -417,8 +421,8 @@ parse_pgm_inst_t cmd_servo_door = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_servo_door,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_servo_door_arg0, 
-               (prog_void *)&cmd_servo_door_arg1, 
+               (prog_void *)&cmd_servo_door_arg0,
+               (prog_void *)&cmd_servo_door_arg1,
                NULL,
        },
 };
@@ -464,9 +468,9 @@ parse_pgm_inst_t cmd_cobroller = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_cobroller,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_cobroller_arg0, 
-               (prog_void *)&cmd_cobroller_arg1, 
-               (prog_void *)&cmd_cobroller_arg2, 
+               (prog_void *)&cmd_cobroller_arg0,
+               (prog_void *)&cmd_cobroller_arg1,
+               (prog_void *)&cmd_cobroller_arg2,
                NULL,
        },
 };
@@ -504,8 +508,8 @@ parse_pgm_inst_t cmd_shovel = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_shovel,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_shovel_arg0, 
-               (prog_void *)&cmd_shovel_arg1, 
+               (prog_void *)&cmd_shovel_arg0,
+               (prog_void *)&cmd_shovel_arg1,
                NULL,
        },
 };
@@ -541,8 +545,8 @@ parse_pgm_inst_t cmd_servo_carry = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_servo_carry,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_servo_carry_arg0, 
-               (prog_void *)&cmd_servo_carry_arg1, 
+               (prog_void *)&cmd_servo_carry_arg0,
+               (prog_void *)&cmd_servo_carry_arg1,
                NULL,
        },
 };
@@ -597,9 +601,9 @@ parse_pgm_inst_t cmd_spickle = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_spickle,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_spickle_arg0, 
-               (prog_void *)&cmd_spickle_arg1, 
-               (prog_void *)&cmd_spickle_arg2, 
+               (prog_void *)&cmd_spickle_arg0,
+               (prog_void *)&cmd_spickle_arg1,
+               (prog_void *)&cmd_spickle_arg2,
                NULL,
        },
 };
@@ -680,8 +684,8 @@ parse_pgm_inst_t cmd_spickle_params_show = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_spickle_params_show,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_spickle_params_arg0, 
-               (prog_void *)&cmd_spickle_params_arg1_show, 
+               (prog_void *)&cmd_spickle_params_arg0,
+               (prog_void *)&cmd_spickle_params_arg1_show,
                NULL,
        },
 };
@@ -702,7 +706,7 @@ static void cmd_spickle_params2_parsed(void *parsed_result,
                                      __attribute__((unused)) void *data)
 {
        struct cmd_spickle_params2_result * res = parsed_result;
-       
+
        if (!strcmp_P(res->arg1, PSTR("coef"))) {
                spickle_set_coefs(res->arg2, res->arg3);
        }
@@ -728,10 +732,10 @@ parse_pgm_inst_t cmd_spickle_params2 = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_spickle_params2,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_spickle_params2_arg0, 
-               (prog_void *)&cmd_spickle_params2_arg1, 
-               (prog_void *)&cmd_spickle_params2_arg2, 
-               (prog_void *)&cmd_spickle_params2_arg3, 
+               (prog_void *)&cmd_spickle_params2_arg0,
+               (prog_void *)&cmd_spickle_params2_arg1,
+               (prog_void *)&cmd_spickle_params2_arg2,
+               (prog_void *)&cmd_spickle_params2_arg3,
                NULL,
        },
 };
@@ -746,8 +750,8 @@ parse_pgm_inst_t cmd_spickle_params2_show = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_spickle_params2_show,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_spickle_params2_arg0, 
-               (prog_void *)&cmd_spickle_params2_arg1_show, 
+               (prog_void *)&cmd_spickle_params2_arg0,
+               (prog_void *)&cmd_spickle_params2_arg1_show,
                NULL,
        },
 };
@@ -776,7 +780,7 @@ parse_pgm_inst_t cmd_test = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_test,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_test_arg0, 
+               (prog_void *)&cmd_test_arg0,
                NULL,
        },
 };
index f9c0b7d..a718970 100644 (file)
@@ -95,10 +95,20 @@ static void do_cs(__attribute__((unused)) void *dummy)
        }
 #endif
 
-       if (cobboard.flags & DO_BD) {
+       if ((cobboard.flags & DO_BD) && (cobboard.flags & DO_POWER)) {
                bd_manage_from_cs(&cobboard.left_spickle.bd, &cobboard.left_spickle.cs);
                bd_manage_from_cs(&cobboard.right_spickle.bd, &cobboard.right_spickle.cs);
                bd_manage_from_cs(&cobboard.shovel.bd, &cobboard.shovel.cs);
+
+               /* urgent case: stop power on blocking */
+               if (cobboard.flags & DO_ERRBLOCKING) {
+                       if (bd_get(&cobboard.left_spickle.bd) ||
+                           bd_get(&cobboard.right_spickle.bd) ||
+                           bd_get(&cobboard.shovel.bd)) {
+                               printf_P(PSTR("MOTOR BLOCKED STOP ALL\r\n"));
+                               cobboard.flags &= ~(DO_POWER | DO_ERRBLOCKING);
+                       }
+               }
        }
        if (cobboard.flags & DO_POWER)
                BRAKE_OFF();
@@ -154,7 +164,7 @@ void microb_cs_init(void)
        /* Blocking detection */
        bd_init(&cobboard.left_spickle.bd);
        bd_set_speed_threshold(&cobboard.left_spickle.bd, 150);
-       bd_set_current_thresholds(&cobboard.left_spickle.bd, 500, 8000, 1000000, 40);
+       bd_set_current_thresholds(&cobboard.left_spickle.bd, 500, 8000, 1000000, 200);
 
        /* ---- CS right_spickle */
        /* PID */
@@ -174,7 +184,7 @@ void microb_cs_init(void)
        /* Blocking detection */
        bd_init(&cobboard.right_spickle.bd);
        bd_set_speed_threshold(&cobboard.right_spickle.bd, 150);
-       bd_set_current_thresholds(&cobboard.right_spickle.bd, 500, 8000, 1000000, 40);
+       bd_set_current_thresholds(&cobboard.right_spickle.bd, 500, 8000, 1000000, 200);
 
        /* ---- CS shovel */
        /* PID */
@@ -200,7 +210,7 @@ void microb_cs_init(void)
        /* Blocking detection */
        bd_init(&cobboard.shovel.bd);
        bd_set_speed_threshold(&cobboard.shovel.bd, 150);
-       bd_set_current_thresholds(&cobboard.shovel.bd, 500, 8000, 1000000, 40);
+       bd_set_current_thresholds(&cobboard.shovel.bd, 500, 8000, 1000000, 200);
 
        /* set them on (or not) !! */
        cobboard.left_spickle.on = 0;
index 9de2d4c..8d86b49 100644 (file)
@@ -93,7 +93,7 @@ static void i2c_send_status(void)
 
        /* status */
        ans.mode = state_get_mode();
-       ans.status = 0x55; /* TODO */
+       ans.status = state_get_status();
 
        ans.left_cobroller_speed = cobboard.left_cobroller_speed;
        ans.right_cobroller_speed = cobboard.right_cobroller_speed;
index 24f14bc..1f4065b 100755 (executable)
@@ -115,16 +115,21 @@ void bootloader(void)
 
 void do_led_blink(__attribute__((unused)) void *dummy)
 {
-#if 1 /* simple blink */
-       static uint8_t a=0;
+       static uint8_t a = 0;
 
-       if(a)
-               LED1_ON();
-       else
-               LED1_OFF();
-
-       a = !a;
-#endif
+       if (cobboard.flags & DO_ERRBLOCKING) {
+               if (a & 1)
+                       LED1_ON();
+               else
+                       LED1_OFF();
+       }
+       else {
+               if (a & 4)
+                       LED1_ON();
+               else
+                       LED1_OFF();
+       }
+       a++;
 }
 
 static void main_timer_interrupt(void)
@@ -153,7 +158,8 @@ int main(void)
        memset(&gen, 0, sizeof(gen));
        memset(&cobboard, 0, sizeof(cobboard));
        /* cs is enabled after arm_calibrate() */
-       cobboard.flags = DO_ENCODERS | DO_POWER; // DO_BD
+       cobboard.flags = DO_ENCODERS | DO_POWER | DO_BD |
+               DO_ERRBLOCKING;
 
        /* UART */
        uart_init();
index 0d07a9c..699a5c1 100755 (executable)
@@ -119,6 +119,7 @@ struct cobboard {
 #define DO_CS        2
 #define DO_BD        4
 #define DO_POWER     8
+#define DO_ERRBLOCKING 16
        uint8_t flags;                /* misc flags */
 
        /* control systems */
index 9d082ba..a16675d 100644 (file)
@@ -58,6 +58,7 @@
 
 static struct vt100 local_vt100;
 static volatile uint8_t state_mode;
+static volatile uint8_t state_status;
 static uint8_t cob_count;
 
 /* short aliases */
@@ -72,13 +73,6 @@ static uint8_t cob_count;
 
 uint8_t state_debug = 0;
 
-#if 0
-static void state_dump_sensors(void)
-{
-       STMCH_DEBUG("TODO\n");
-}
-#endif
-
 uint8_t state_get_cob_count(void)
 {
        return cob_count;
@@ -125,11 +119,30 @@ static uint8_t state_no_cob_inside(void)
                !sensor_get(S_COB_INSIDE_R);
 }
 
+/* pack/deploy spickles depending on mode */
+static void spickle_prepare(uint8_t side)
+{
+       if (cob_count >= 5)
+               spickle_pack(side);
+       else if (DEPLOY(side, state_mode) && !HARVEST(side, state_mode))
+               spickle_mid(side);
+       else if (DEPLOY(side, state_mode) && HARVEST(side, state_mode))
+               spickle_deploy(side);
+       else
+               spickle_pack(side);
+}
+
 /* set a new state, return 0 on success */
 int8_t state_set_mode(uint8_t mode)
 {
        state_mode = mode;
 
+       /* preempt current action if not busy */
+       if (state_status != I2C_COBBOARD_STATUS_LBUSY)
+               spickle_prepare(I2C_LEFT_SIDE);
+       if (state_status != I2C_COBBOARD_STATUS_RBUSY)
+               spickle_prepare(I2C_RIGHT_SIDE);
+
 /*     STMCH_DEBUG("%s(): l_deploy=%d l_harvest=%d " */
 /*                 "r_deploy=%d r_harvest=%d eject=%d", */
 /*                 __FUNCTION__, L_DEPLOY(mode), L_HARVEST(mode), */
@@ -159,9 +172,19 @@ uint8_t state_get_mode(void)
        return state_mode;
 }
 
+uint8_t state_get_status(void)
+{
+       return state_status;
+}
+
 /* harvest cobs from area */
 static void state_do_harvest(uint8_t side)
 {
+       if (side == I2C_LEFT_SIDE)
+               state_status = I2C_COBBOARD_STATUS_LBUSY;
+       else
+               state_status = I2C_COBBOARD_STATUS_RBUSY;
+
        /* if there is no cob, return */
        if (state_cob_present(side))
                return;
@@ -178,13 +201,38 @@ static void state_do_harvest(uint8_t side)
        time_wait_ms(250);
        cobroller_on(side);
 
+       /* check that cob is correctly in place */
        if (WAIT_COND_OR_TIMEOUT(state_cob_inside(), 750) == 0) {
                if (state_no_cob_inside()) {
-                       printf_P(PSTR("NO COB\r\n"));
+                       STMCH_DEBUG("no cob");
+                       return;
+               }
+               STMCH_DEBUG("bad cob state");
+
+               /* while cob is not correctly placed try to extract
+                * it as much as we can */
+               while (state_cob_inside() == 0 &&
+                      state_no_cob_inside() == 0) {
+                       uint8_t cpt = 0;
+                       if (cpt == 0)
+                               cobroller_reverse(side);
+                       else if (cpt == 1)
+                               cobroller_off(side);
+                       else if (cpt == 2)
+                               cobroller_on(side);
+                       cpt ++;
+                       cpt %= 3;
+                       shovel_mid();
+                       time_wait_ms(250);
+                       shovel_down();
+                       time_wait_ms(250);
+               }
+
+               STMCH_DEBUG("cob removed");
+               if (state_no_cob_inside()) {
+                       STMCH_DEBUG("no cob");
                        return;
                }
-               printf_P(PSTR("BAD COB - press a key\r\n"));
-               while(!cmdline_keypressed());
        }
 
        /* cob is inside, switch off roller */
@@ -208,10 +256,11 @@ static void state_do_harvest(uint8_t side)
        /* store it */
        shovel_up();
 
-       if (WAIT_COND_OR_TIMEOUT(shovel_is_up(), 400) == 0) {
-               BRAKE_ON();
-               printf_P(PSTR("BLOCKED\r\n"));
-               while(!cmdline_keypressed());
+       while (WAIT_COND_OR_TIMEOUT(shovel_is_up(), 400) == 0) {
+               STMCH_DEBUG("shovel blocked");
+               shovel_down();
+               time_wait_ms(250);
+               shovel_up();
        }
 
        state_debug_wait_key_pressed();
@@ -224,10 +273,11 @@ static void state_do_harvest(uint8_t side)
 
        shovel_down();
 
-       if (WAIT_COND_OR_TIMEOUT(shovel_is_down(), 400) == 0) {
-               BRAKE_ON();
-               printf_P(PSTR("BLOCKED\r\n"));
-               while(!cmdline_keypressed());
+       while (WAIT_COND_OR_TIMEOUT(shovel_is_down(), 400) == 0) {
+               STMCH_DEBUG("shovel blocked");
+               shovel_up();
+               time_wait_ms(250);
+               shovel_down();
        }
 
        STMCH_DEBUG("end");
@@ -236,6 +286,7 @@ static void state_do_harvest(uint8_t side)
 /* eject cobs */
 static void state_do_eject(void)
 {
+       state_status = I2C_COBBOARD_STATUS_EJECT;
        cob_count = 0;
        shovel_mid();
        servo_carry_open();
@@ -251,33 +302,19 @@ void state_machine(void)
 {
        while (state_want_exit() == 0) {
 
+               state_status = I2C_COBBOARD_STATUS_READY;
+
                /* init */
                if (INIT(state_mode)) {
                        state_mode &= (~I2C_COBBOARD_MODE_INIT);
                        state_init();
                }
 
+               /* pack/deply spickles, enable/disable roller */
                cobroller_off(I2C_LEFT_SIDE);
                cobroller_off(I2C_RIGHT_SIDE);
-
-               /* pack/deply spickles, enable/disable roller */
-               if (cob_count >= 5)
-                       spickle_pack(I2C_LEFT_SIDE);
-               else if (L_DEPLOY(state_mode) && !L_HARVEST(state_mode))
-                       spickle_mid(I2C_LEFT_SIDE);
-               else if (L_DEPLOY(state_mode) && L_HARVEST(state_mode))
-                       spickle_deploy(I2C_LEFT_SIDE);
-               else
-                       spickle_pack(I2C_LEFT_SIDE);
-
-               if (cob_count >= 5)
-                       spickle_pack(I2C_RIGHT_SIDE);
-               else if (R_DEPLOY(state_mode) && !R_HARVEST(state_mode))
-                       spickle_mid(I2C_RIGHT_SIDE);
-               else if (R_DEPLOY(state_mode) && R_HARVEST(state_mode))
-                       spickle_deploy(I2C_RIGHT_SIDE);
-               else
-                       spickle_pack(I2C_RIGHT_SIDE);
+               spickle_prepare(I2C_LEFT_SIDE);
+               spickle_prepare(I2C_RIGHT_SIDE);
 
                /* harvest */
                if (cob_count < 5) {
@@ -305,4 +342,5 @@ void state_init(void)
        spickle_pack(I2C_RIGHT_SIDE);
        state_mode = 0;
        cob_count = 0;
+       state_status = I2C_COBBOARD_STATUS_READY;
 }
index 8dcb6c1..d9f95fc 100644 (file)
@@ -27,6 +27,7 @@ int8_t state_set_mode(uint8_t mode);
 
 /* get current state */
 uint8_t state_get_mode(void);
+uint8_t state_get_status(void);
 
 uint8_t state_get_cob_count(void);
 
index 87f98c7..d6794af 100644 (file)
@@ -52,7 +52,7 @@ struct i2c_cmd_hdr {
 struct i2c_cmd_led_control {
        struct i2c_cmd_hdr hdr;
        uint8_t led_num:7;
-       uint8_t state:1;        
+       uint8_t state:1;
 };
 
 /****/
@@ -115,9 +115,10 @@ struct i2c_ans_cobboard_status {
        /* mode type are defined above: I2C_COBBOARD_MODE_xxx */
        uint8_t mode;
 
-#define I2C_COBBOARD_STATUS_F_READY         0x00
-#define I2C_COBBOARD_STATUS_F_BUSY          0x01
-#define I2C_COBBOARD_STATUS_F_EXCPT         0x02
+#define I2C_COBBOARD_STATUS_READY         0x00
+#define I2C_COBBOARD_STATUS_LBUSY         0x01
+#define I2C_COBBOARD_STATUS_RBUSY         0x02
+#define I2C_COBBOARD_STATUS_EJECT         0x03
        uint8_t status;
 
        uint8_t cob_count;
index cdbeb6e..e5bb456 100644 (file)
@@ -74,6 +74,10 @@ extern parse_pgm_inst_t cmd_cobboard_show;
 extern parse_pgm_inst_t cmd_cobboard_setmode1;
 extern parse_pgm_inst_t cmd_cobboard_setmode2;
 extern parse_pgm_inst_t cmd_cobboard_setmode3;
+extern parse_pgm_inst_t cmd_ballboard_show;
+extern parse_pgm_inst_t cmd_ballboard_setmode1;
+extern parse_pgm_inst_t cmd_ballboard_setmode2;
+extern parse_pgm_inst_t cmd_ballboard_setmode3;
 extern parse_pgm_inst_t cmd_beacon_start;
 extern parse_pgm_inst_t cmd_servo_balls;
 extern parse_pgm_inst_t cmd_clitoid;
@@ -82,6 +86,8 @@ extern parse_pgm_inst_t cmd_test;
 /* commands_traj.c */
 extern parse_pgm_inst_t cmd_traj_speed;
 extern parse_pgm_inst_t cmd_traj_speed_show;
+extern parse_pgm_inst_t cmd_traj_acc;
+extern parse_pgm_inst_t cmd_traj_acc_show;
 extern parse_pgm_inst_t cmd_trajectory;
 extern parse_pgm_inst_t cmd_trajectory_show;
 extern parse_pgm_inst_t cmd_circle_coef;
@@ -159,6 +165,10 @@ parse_pgm_ctx_t main_ctx[] = {
        (parse_pgm_inst_t *)&cmd_cobboard_setmode1,
        (parse_pgm_inst_t *)&cmd_cobboard_setmode2,
        (parse_pgm_inst_t *)&cmd_cobboard_setmode3,
+       (parse_pgm_inst_t *)&cmd_ballboard_show,
+       (parse_pgm_inst_t *)&cmd_ballboard_setmode1,
+       (parse_pgm_inst_t *)&cmd_ballboard_setmode2,
+       (parse_pgm_inst_t *)&cmd_ballboard_setmode3,
        (parse_pgm_inst_t *)&cmd_servo_balls,
        (parse_pgm_inst_t *)&cmd_clitoid,
        (parse_pgm_inst_t *)&cmd_test,
@@ -166,6 +176,8 @@ parse_pgm_ctx_t main_ctx[] = {
        /* commands_traj.c */
        (parse_pgm_inst_t *)&cmd_traj_speed,
        (parse_pgm_inst_t *)&cmd_traj_speed_show,
+       (parse_pgm_inst_t *)&cmd_traj_acc,
+       (parse_pgm_inst_t *)&cmd_traj_acc_show,
        (parse_pgm_inst_t *)&cmd_trajectory,
        (parse_pgm_inst_t *)&cmd_trajectory_show,
        (parse_pgm_inst_t *)&cmd_circle_coef,
index fe4fabb..a69208b 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
@@ -17,7 +17,7 @@
  *
  *  Revision : $Id: commands_mainboard.c,v 1.9 2009-11-08 17:24:33 zer0 Exp $
  *
- *  Olivier MATZ <zer0@droids-corp.org> 
+ *  Olivier MATZ <zer0@droids-corp.org>
  */
 
 #include <stdio.h>
@@ -41,6 +41,7 @@
 #include <quadramp.h>
 #include <control_system_manager.h>
 #include <trajectory_manager.h>
+#include <trajectory_manager_core.h>
 #include <trajectory_manager_utils.h>
 #include <vect_base.h>
 #include <lines.h>
@@ -81,29 +82,30 @@ static void cmd_event_parsed(void *parsed_result, void *data)
        u08 bit=0;
 
        struct cmd_event_result * res = parsed_result;
-       
+
        if (!strcmp_P(res->arg1, PSTR("all"))) {
-               bit = DO_ENCODERS | DO_CS | DO_RS | DO_POS |
-                       DO_BD | DO_TIMER | DO_POWER;
+               bit = 0xFF;
                if (!strcmp_P(res->arg2, PSTR("on")))
                        mainboard.flags |= bit;
                else if (!strcmp_P(res->arg2, PSTR("off")))
                        mainboard.flags &= bit;
                else { /* show */
-                       printf_P(PSTR("encoders is %s\r\n"), 
+                       printf_P(PSTR("encoders is %s\r\n"),
                                 (DO_ENCODERS & mainboard.flags) ? "on":"off");
-                       printf_P(PSTR("cs is %s\r\n"), 
+                       printf_P(PSTR("cs is %s\r\n"),
                                 (DO_CS & mainboard.flags) ? "on":"off");
-                       printf_P(PSTR("rs is %s\r\n"), 
+                       printf_P(PSTR("rs is %s\r\n"),
                                 (DO_RS & mainboard.flags) ? "on":"off");
-                       printf_P(PSTR("pos is %s\r\n"), 
+                       printf_P(PSTR("pos is %s\r\n"),
                                 (DO_POS & mainboard.flags) ? "on":"off");
-                       printf_P(PSTR("bd is %s\r\n"), 
+                       printf_P(PSTR("bd is %s\r\n"),
                                 (DO_BD & mainboard.flags) ? "on":"off");
-                       printf_P(PSTR("timer is %s\r\n"), 
+                       printf_P(PSTR("timer is %s\r\n"),
                                 (DO_TIMER & mainboard.flags) ? "on":"off");
-                       printf_P(PSTR("power is %s\r\n"), 
+                       printf_P(PSTR("power is %s\r\n"),
                                 (DO_POWER & mainboard.flags) ? "on":"off");
+                       printf_P(PSTR("errblock is %s\r\n"),
+                                (DO_ERRBLOCKING & mainboard.flags) ? "on":"off");
                }
                return;
        }
@@ -126,6 +128,8 @@ static void cmd_event_parsed(void *parsed_result, void *data)
        }
        else if (!strcmp_P(res->arg1, PSTR("power")))
                bit = DO_POWER;
+       else if (!strcmp_P(res->arg1, PSTR("errblock")))
+               bit = DO_ERRBLOCKING;
 
        if (!strcmp_P(res->arg2, PSTR("on")))
                mainboard.flags |= bit;
@@ -141,13 +145,13 @@ static void cmd_event_parsed(void *parsed_result, void *data)
                }
                mainboard.flags &= (~bit);
        }
-       printf_P(PSTR("%s is %s\r\n"), res->arg1, 
+       printf_P(PSTR("%s is %s\r\n"), res->arg1,
                      (bit & mainboard.flags) ? "on":"off");
 }
 
 prog_char str_event_arg0[] = "event";
 parse_pgm_token_string_t cmd_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg0, str_event_arg0);
-prog_char str_event_arg1[] = "all#encoders#cs#rs#pos#bd#timer#power";
+prog_char str_event_arg1[] = "all#encoders#cs#rs#pos#bd#timer#power#errblock";
 parse_pgm_token_string_t cmd_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg1, str_event_arg1);
 prog_char str_event_arg2[] = "on#off#show";
 parse_pgm_token_string_t cmd_event_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg2, str_event_arg2);
@@ -158,9 +162,9 @@ parse_pgm_inst_t cmd_event = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_event,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_event_arg0, 
-               (prog_void *)&cmd_event_arg1, 
-               (prog_void *)&cmd_event_arg2, 
+               (prog_void *)&cmd_event_arg0,
+               (prog_void *)&cmd_event_arg1,
+               (prog_void *)&cmd_event_arg2,
                NULL,
        },
 };
@@ -181,7 +185,7 @@ static void cmd_spi_test_parsed(void * parsed_result, void * data)
        printf("not implemented\n");
 #else
        uint16_t i = 0, ret = 0, ret2 = 0;
-       
+
        if (mainboard.flags & DO_ENCODERS) {
                printf_P(PSTR("Disable encoder event first\r\n"));
                return;
@@ -211,7 +215,7 @@ parse_pgm_inst_t cmd_spi_test = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_spi_test,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_spi_test_arg0, 
+               (prog_void *)&cmd_spi_test_arg0,
                NULL,
        },
 };
@@ -251,8 +255,8 @@ parse_pgm_inst_t cmd_opponent = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_opponent,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_opponent_arg0, 
-               (prog_void *)&cmd_opponent_arg1, 
+               (prog_void *)&cmd_opponent_arg0,
+               (prog_void *)&cmd_opponent_arg1,
                NULL,
        },
 };
@@ -269,10 +273,10 @@ parse_pgm_inst_t cmd_opponent_set = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_opponent_set,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_opponent_arg0, 
+               (prog_void *)&cmd_opponent_arg0,
                (prog_void *)&cmd_opponent_arg1_set,
-               (prog_void *)&cmd_opponent_arg2, 
-               (prog_void *)&cmd_opponent_arg3, 
+               (prog_void *)&cmd_opponent_arg2,
+               (prog_void *)&cmd_opponent_arg3,
                NULL,
        },
 };
@@ -338,9 +342,9 @@ parse_pgm_inst_t cmd_start = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_start,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_start_arg0, 
-               (prog_void *)&cmd_start_color, 
-               (prog_void *)&cmd_start_debug, 
+               (prog_void *)&cmd_start_arg0,
+               (prog_void *)&cmd_start_color,
+               (prog_void *)&cmd_start_debug,
                NULL,
        },
 };
@@ -358,7 +362,7 @@ struct cmd_interact_result {
 static void print_cs(void)
 {
        printf_P(PSTR("cons_d=% .8"PRIi32" cons_a=% .8"PRIi32" fil_d=% .8"PRIi32" fil_a=% .8"PRIi32" "
-                     "err_d=% .8"PRIi32" err_a=% .8"PRIi32" out_d=% .8"PRIi32" out_a=% .8"PRIi32"\r\n"), 
+                     "err_d=% .8"PRIi32" err_a=% .8"PRIi32" out_d=% .8"PRIi32" out_a=% .8"PRIi32"\r\n"),
                 cs_get_consign(&mainboard.distance.cs),
                 cs_get_consign(&mainboard.angle.cs),
                 cs_get_filtered_consign(&mainboard.distance.cs),
@@ -371,7 +375,7 @@ static void print_cs(void)
 
 static void print_pos(void)
 {
-       printf_P(PSTR("x=% .8d y=% .8d a=% .8d\r\n"), 
+       printf_P(PSTR("x=% .8d y=% .8d a=% .8d\r\n"),
                 position_get_x_s16(&mainboard.pos),
                 position_get_y_s16(&mainboard.pos),
                 position_get_a_deg_s16(&mainboard.pos));
@@ -484,7 +488,7 @@ static void cmd_interact_parsed(void * parsed_result, void * data)
                        wait_ms(10);
                        continue;
                }
-               
+
                if (cmd == -1) {
                        switch(c) {
                        case '1': print ^= PRINT_POS; break;
@@ -494,7 +498,7 @@ static void cmd_interact_parsed(void * parsed_result, void * data)
                        case '5': print ^= PRINT_TIME; break;
                        case '6': print ^= PRINT_BLOCKING; break;
 
-                       case 'q': 
+                       case 'q':
                                if (mainboard.flags & DO_CS)
                                        strat_hardstop();
                                pwm_set_and_save(LEFT_PWM, 0);
@@ -504,21 +508,21 @@ static void cmd_interact_parsed(void * parsed_result, void * data)
                                pwm_set_and_save(LEFT_PWM, 0);
                                pwm_set_and_save(RIGHT_PWM, 0);
                                break;
-                       default: 
+                       default:
                                break;
                        }
                }
                else {
                        switch(cmd) {
-                       case KEY_UP_ARR: 
+                       case KEY_UP_ARR:
                                pwm_set_and_save(LEFT_PWM, 1200);
                                pwm_set_and_save(RIGHT_PWM, 1200);
                                break;
-                       case KEY_LEFT_ARR: 
+                       case KEY_LEFT_ARR:
                                pwm_set_and_save(LEFT_PWM, -1200);
                                pwm_set_and_save(RIGHT_PWM, 1200);
                                break;
-                       case KEY_DOWN_ARR: 
+                       case KEY_DOWN_ARR:
                                pwm_set_and_save(LEFT_PWM, -1200);
                                pwm_set_and_save(RIGHT_PWM, -1200);
                                break;
@@ -541,7 +545,7 @@ parse_pgm_inst_t cmd_interact = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_interact,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_interact_arg0, 
+               (prog_void *)&cmd_interact_arg0,
                NULL,
        },
 };
@@ -588,8 +592,8 @@ parse_pgm_inst_t cmd_color = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_color,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_color_arg0, 
-               (prog_void *)&cmd_color_color, 
+               (prog_void *)&cmd_color_arg0,
+               (prog_void *)&cmd_color_color,
                NULL,
        },
 };
@@ -609,11 +613,11 @@ static void cmd_rs_parsed(void *parsed_result, void *data)
 {
        //      struct cmd_rs_result *res = parsed_result;
        do {
-               printf_P(PSTR("angle cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "), 
+               printf_P(PSTR("angle cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "),
                         cs_get_consign(&mainboard.angle.cs),
                         cs_get_filtered_feedback(&mainboard.angle.cs),
                         cs_get_out(&mainboard.angle.cs));
-               printf_P(PSTR("distance cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "), 
+               printf_P(PSTR("distance cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "),
                         cs_get_consign(&mainboard.distance.cs),
                         cs_get_filtered_feedback(&mainboard.distance.cs),
                         cs_get_out(&mainboard.distance.cs));
@@ -634,8 +638,8 @@ parse_pgm_inst_t cmd_rs = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_rs,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_rs_arg0, 
-               (prog_void *)&cmd_rs_arg1, 
+               (prog_void *)&cmd_rs_arg0,
+               (prog_void *)&cmd_rs_arg1,
                NULL,
        },
 };
@@ -668,7 +672,7 @@ parse_pgm_inst_t cmd_i2cdebug = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_i2cdebug,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_i2cdebug_arg0, 
+               (prog_void *)&cmd_i2cdebug_arg0,
                NULL,
        },
 };
@@ -707,8 +711,8 @@ parse_pgm_inst_t cmd_cobboard_show = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_cobboard_show,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_cobboard_show_arg0, 
-               (prog_void *)&cmd_cobboard_show_arg1, 
+               (prog_void *)&cmd_cobboard_show_arg0,
+               (prog_void *)&cmd_cobboard_show_arg1,
                NULL,
        },
 };
@@ -725,16 +729,12 @@ struct cmd_cobboard_setmode1_result {
 /* function called when cmd_cobboard_setmode1 is parsed successfully */
 static void cmd_cobboard_setmode1_parsed(void *parsed_result, void *data)
 {
-#ifdef HOST_VERSION
-       printf("not implemented\n");
-#else
        struct cmd_cobboard_setmode1_result *res = parsed_result;
 
        if (!strcmp_P(res->arg1, PSTR("init")))
                i2c_cobboard_mode_init();
        else if (!strcmp_P(res->arg1, PSTR("eject")))
                i2c_cobboard_mode_eject();
-#endif
 }
 
 prog_char str_cobboard_setmode1_arg0[] = "cobboard";
@@ -748,8 +748,8 @@ parse_pgm_inst_t cmd_cobboard_setmode1 = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_cobboard_setmode1,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_cobboard_setmode1_arg0, 
-               (prog_void *)&cmd_cobboard_setmode1_arg1, 
+               (prog_void *)&cmd_cobboard_setmode1_arg0,
+               (prog_void *)&cmd_cobboard_setmode1_arg1,
                NULL,
        },
 };
@@ -767,9 +767,6 @@ struct cmd_cobboard_setmode2_result {
 /* function called when cmd_cobboard_setmode2 is parsed successfully */
 static void cmd_cobboard_setmode2_parsed(void * parsed_result, void * data)
 {
-#ifdef HOST_VERSION
-       printf("not implemented\n");
-#else
        struct cmd_cobboard_setmode2_result *res = parsed_result;
        uint8_t side = I2C_LEFT_SIDE;
 
@@ -784,7 +781,6 @@ static void cmd_cobboard_setmode2_parsed(void * parsed_result, void * data)
                i2c_cobboard_mode_harvest(side);
        else if (!strcmp_P(res->arg1, PSTR("pack")))
                i2c_cobboard_mode_pack(side);
-#endif
 }
 
 prog_char str_cobboard_setmode2_arg0[] = "cobboard";
@@ -800,9 +796,9 @@ parse_pgm_inst_t cmd_cobboard_setmode2 = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_cobboard_setmode2,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_cobboard_setmode2_arg0, 
-               (prog_void *)&cmd_cobboard_setmode2_arg1, 
-               (prog_void *)&cmd_cobboard_setmode2_arg2, 
+               (prog_void *)&cmd_cobboard_setmode2_arg0,
+               (prog_void *)&cmd_cobboard_setmode2_arg1,
+               (prog_void *)&cmd_cobboard_setmode2_arg2,
                NULL,
        },
 };
@@ -820,13 +816,9 @@ struct cmd_cobboard_setmode3_result {
 /* function called when cmd_cobboard_setmode3 is parsed successfully */
 static void cmd_cobboard_setmode3_parsed(void *parsed_result, void *data)
 {
-#ifdef HOST_VERSION
-       printf("not implemented\n");
-#else
        struct cmd_cobboard_setmode3_result *res = parsed_result;
        if (!strcmp_P(res->arg1, PSTR("xxx")))
                printf("faux\r\n");
-#endif
 }
 
 prog_char str_cobboard_setmode3_arg0[] = "cobboard";
@@ -841,9 +833,9 @@ parse_pgm_inst_t cmd_cobboard_setmode3 = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_cobboard_setmode3,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_cobboard_setmode3_arg0, 
-               (prog_void *)&cmd_cobboard_setmode3_arg1, 
-               (prog_void *)&cmd_cobboard_setmode3_arg2, 
+               (prog_void *)&cmd_cobboard_setmode3_arg0,
+               (prog_void *)&cmd_cobboard_setmode3_arg1,
+               (prog_void *)&cmd_cobboard_setmode3_arg2,
                NULL,
        },
 };
@@ -880,8 +872,8 @@ parse_pgm_inst_t cmd_ballboard_show = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_ballboard_show,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_ballboard_show_arg0, 
-               (prog_void *)&cmd_ballboard_show_arg1, 
+               (prog_void *)&cmd_ballboard_show_arg0,
+               (prog_void *)&cmd_ballboard_show_arg1,
                NULL,
        },
 };
@@ -898,9 +890,6 @@ struct cmd_ballboard_setmode1_result {
 /* function called when cmd_ballboard_setmode1 is parsed successfully */
 static void cmd_ballboard_setmode1_parsed(void *parsed_result, void *data)
 {
-#ifdef HOST_VERSION
-       printf("not implemented\n");
-#else
        struct cmd_ballboard_setmode1_result *res = parsed_result;
 
        if (!strcmp_P(res->arg1, PSTR("init")))
@@ -913,7 +902,6 @@ static void cmd_ballboard_setmode1_parsed(void *parsed_result, void *data)
                i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_HARVEST);
 
        /* other commands */
-#endif
 }
 
 prog_char str_ballboard_setmode1_arg0[] = "ballboard";
@@ -927,8 +915,8 @@ parse_pgm_inst_t cmd_ballboard_setmode1 = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_ballboard_setmode1,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_ballboard_setmode1_arg0, 
-               (prog_void *)&cmd_ballboard_setmode1_arg1, 
+               (prog_void *)&cmd_ballboard_setmode1_arg0,
+               (prog_void *)&cmd_ballboard_setmode1_arg1,
                NULL,
        },
 };
@@ -946,9 +934,6 @@ struct cmd_ballboard_setmode2_result {
 /* function called when cmd_ballboard_setmode2 is parsed successfully */
 static void cmd_ballboard_setmode2_parsed(void * parsed_result, void * data)
 {
-#ifdef HOST_VERSION
-       printf("not implemented\n");
-#else
        struct cmd_ballboard_setmode2_result *res = parsed_result;
        uint8_t mode = I2C_BALLBOARD_MODE_INIT;
 
@@ -965,7 +950,6 @@ static void cmd_ballboard_setmode2_parsed(void * parsed_result, void * data)
                        mode = I2C_BALLBOARD_MODE_TAKE_R_FORK;
        }
        i2c_ballboard_set_mode(mode);
-#endif
 }
 
 prog_char str_ballboard_setmode2_arg0[] = "ballboard";
@@ -981,9 +965,9 @@ parse_pgm_inst_t cmd_ballboard_setmode2 = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_ballboard_setmode2,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_ballboard_setmode2_arg0, 
-               (prog_void *)&cmd_ballboard_setmode2_arg1, 
-               (prog_void *)&cmd_ballboard_setmode2_arg2, 
+               (prog_void *)&cmd_ballboard_setmode2_arg0,
+               (prog_void *)&cmd_ballboard_setmode2_arg1,
+               (prog_void *)&cmd_ballboard_setmode2_arg2,
                NULL,
        },
 };
@@ -1001,13 +985,9 @@ struct cmd_ballboard_setmode3_result {
 /* function called when cmd_ballboard_setmode3 is parsed successfully */
 static void cmd_ballboard_setmode3_parsed(void *parsed_result, void *data)
 {
-#ifdef HOST_VERSION
-       printf("not implemented\n");
-#else
        struct cmd_ballboard_setmode3_result *res = parsed_result;
        if (!strcmp_P(res->arg1, PSTR("xxx")))
                printf("faux\r\n");
-#endif
 }
 
 prog_char str_ballboard_setmode3_arg0[] = "ballboard";
@@ -1022,9 +1002,9 @@ parse_pgm_inst_t cmd_ballboard_setmode3 = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_ballboard_setmode3,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_ballboard_setmode3_arg0, 
-               (prog_void *)&cmd_ballboard_setmode3_arg1, 
-               (prog_void *)&cmd_ballboard_setmode3_arg2, 
+               (prog_void *)&cmd_ballboard_setmode3_arg0,
+               (prog_void *)&cmd_ballboard_setmode3_arg1,
+               (prog_void *)&cmd_ballboard_setmode3_arg2,
                NULL,
        },
 };
@@ -1063,8 +1043,8 @@ parse_pgm_inst_t cmd_servo_balls = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_servo_balls,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_servo_balls_arg0, 
-               (prog_void *)&cmd_servo_balls_arg1, 
+               (prog_void *)&cmd_servo_balls_arg0,
+               (prog_void *)&cmd_servo_balls_arg1,
                NULL,
        },
 };
@@ -1220,8 +1200,16 @@ uint8_t clitoid(double alpha_deg, double beta_deg, double R_mm,
 static void cmd_clitoid_parsed(void *parsed_result, void *data)
 {
        struct cmd_clitoid_result *res = parsed_result;
-       clitoid(res->alpha_deg, res->beta_deg, res->R_mm,
-               res->Vd, res->Amax, res->d_inter_mm);
+/*     clitoid(res->alpha_deg, res->beta_deg, res->R_mm, */
+/*             res->Vd, res->Amax, res->d_inter_mm); */
+       double x = position_get_x_double(&mainboard.pos);
+       double y = position_get_y_double(&mainboard.pos);
+       double a = position_get_a_rad_double(&mainboard.pos);
+
+       strat_set_speed(res->Vd, SPEED_ANGLE_FAST);
+       trajectory_clitoid(&mainboard.traj, x, y, a, 150.,
+                          res->alpha_deg, res->beta_deg, res->R_mm,
+                          res->d_inter_mm);
 }
 
 prog_char str_clitoid_arg0[] = "clitoid";
@@ -1264,127 +1252,6 @@ parse_pgm_inst_t cmd_clitoid = {
        },
 };
 
-//////////////////////
-
-// 500 -- 5
-// 400 -- 3
-#define TEST_SPEED 400
-#define TEST_ACC 3
-
-static void line2line(double line1x1, double line1y1,
-                     double line1x2, double line1y2,
-                     double line2x1, double line2y1,
-                     double line2x2, double line2y2,
-                     double radius, double dist)
-{
-       uint8_t err;
-       double speed_d, speed_a;
-       double distance, angle;
-       double line1_angle = atan2(line1y2-line1y1, line1x2-line1x1);
-       double line2_angle = atan2(line2y2-line2y1, line2x2-line2x1);
-
-       printf_P(PSTR("%s()\r\n"), __FUNCTION__);
-
-       strat_set_speed(TEST_SPEED, TEST_SPEED);
-       quadramp_set_2nd_order_vars(&mainboard.angle.qr, TEST_ACC, TEST_ACC);
-
-       circle_get_da_speed_from_radius(&mainboard.traj, radius,
-                                       &speed_d, &speed_a);
-       trajectory_line_abs(&mainboard.traj,
-                           line1x1, line1y1,
-                           line1x2, line1y2, 150.);
-       err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line1x2, line1y2) <
-                                   dist, TRAJ_FLAGS_NO_NEAR);
-       /* circle */
-       strat_set_speed(speed_d, speed_a);
-       angle = line2_angle - line1_angle;
-       distance = angle * radius;
-       if (distance < 0)
-               distance = -distance;
-       angle = simple_modulo_2pi(angle);
-       angle = DEG(angle);
-       printf_P(PSTR("(%d,%d,%d) "),
-                position_get_x_s16(&mainboard.pos),
-                position_get_y_s16(&mainboard.pos),
-                position_get_a_deg_s16(&mainboard.pos));
-       printf_P(PSTR("circle distance=%2.2f angle=%2.2f\r\n"),
-                distance, angle);
-
-       /* take some margin on dist to avoid deceleration */
-       trajectory_d_a_rel(&mainboard.traj, distance + 250, angle);
-
-       /* circle exit condition */
-       err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
-                                   TRAJ_FLAGS_NO_NEAR);
-
-       strat_set_speed(500, 500);
-       printf_P(PSTR("(%d,%d,%d) "),
-                position_get_x_s16(&mainboard.pos),
-                position_get_y_s16(&mainboard.pos),
-                position_get_a_deg_s16(&mainboard.pos));
-       printf_P(PSTR("line\r\n"));
-       trajectory_line_abs(&mainboard.traj,
-                           line2x1, line2y1,
-                           line2x2, line2y2, 150.);
-}
-
-static void halfturn(double line1x1, double line1y1,
-                    double line1x2, double line1y2,
-                    double line2x1, double line2y1,
-                    double line2x2, double line2y2,
-                    double radius, double dist, double dir)
-{
-       uint8_t err;
-       double speed_d, speed_a;
-       double distance, angle;
-
-       printf_P(PSTR("%s()\r\n"), __FUNCTION__);
-
-       strat_set_speed(TEST_SPEED, TEST_SPEED);
-       quadramp_set_2nd_order_vars(&mainboard.angle.qr, TEST_ACC, TEST_ACC);
-
-       circle_get_da_speed_from_radius(&mainboard.traj, radius,
-                                       &speed_d, &speed_a);
-       trajectory_line_abs(&mainboard.traj,
-                           line1x1, line1y1,
-                           line1x2, line1y2, 150.);
-       err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line1x2, line1y2) <
-                                   dist, TRAJ_FLAGS_NO_NEAR);
-       /* circle */
-       strat_set_speed(speed_d, speed_a);
-       angle = dir * M_PI/2.;
-       distance = angle * radius;
-       if (distance < 0)
-               distance = -distance;
-       angle = simple_modulo_2pi(angle);
-       angle = DEG(angle);
-
-       /* take some margin on dist to avoid deceleration */
-       DEBUG(E_USER_STRAT, "circle1 distance=%2.2f angle=%2.2f",
-             distance, angle);
-       trajectory_d_a_rel(&mainboard.traj, distance + 500, angle);
-
-       /* circle exit condition */
-       err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
-                                   TRAJ_FLAGS_NO_NEAR);
-
-       DEBUG(E_USER_STRAT, "miniline");
-       err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line2x1, line2y1) <
-                                   dist, TRAJ_FLAGS_NO_NEAR);
-       DEBUG(E_USER_STRAT, "circle2");
-       /* take some margin on dist to avoid deceleration */
-       trajectory_d_a_rel(&mainboard.traj, distance + 500, angle);
-
-       err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
-                                   TRAJ_FLAGS_NO_NEAR);
-
-       strat_set_speed(500, 500);
-       DEBUG(E_USER_STRAT, "line");
-       trajectory_line_abs(&mainboard.traj,
-                           line2x1, line2y1,
-                           line2x2, line2y2, 150.);
-}
-
 /**********************************************************/
 /* Test */
 
@@ -1398,96 +1265,6 @@ struct cmd_test_result {
 /* function called when cmd_test is parsed successfully */
 static void cmd_test_parsed(void *parsed_result, void *data)
 {
-       //      struct cmd_test_result *res = parsed_result;
-#ifdef HOST_VERSION
-       strat_reset_pos(400, 400, 90);
-       mainboard.angle.on = 1;
-       mainboard.distance.on = 1;
-#endif
-       printf_P(PSTR("%s()\r\n"), __FUNCTION__);
-       while (!cmdline_keypressed()) {
-               /****** PASS1 */
-
-#define DIST_HARD_TURN   260
-#define RADIUS_HARD_TURN 100
-#define DIST_EASY_TURN   190
-#define RADIUS_EASY_TURN 190
-#define DIST_HALF_TURN   225
-#define RADIUS_HALF_TURN 130
-
-               /* hard turn */
-               line2line(375, 597, 375, 1847,
-                         375, 1847, 1050, 1472,
-                         RADIUS_HARD_TURN, DIST_HARD_TURN);
-
-               /* easy left and easy right !*/
-               line2line(825, 1596, 1050, 1472,
-                         1050, 1472, 1500, 1722,
-                         RADIUS_EASY_TURN, DIST_EASY_TURN);
-               line2line(1050, 1472, 1500, 1722,
-                         1500, 1722, 2175, 1347,
-                         RADIUS_EASY_TURN, DIST_EASY_TURN);
-               line2line(1500, 1722, 2175, 1347,
-                         2175, 1347, 2175, 847,
-                         RADIUS_EASY_TURN, DIST_EASY_TURN);
-
-               /* half turns */
-               halfturn(2175, 1347, 2175, 722,
-                        2625, 722, 2625, 1597,
-                        RADIUS_HALF_TURN, DIST_HALF_TURN, 1.);
-               halfturn(2625, 847, 2625, 1722,
-                         2175, 1722, 2175, 1097,
-                         RADIUS_HALF_TURN, DIST_HALF_TURN, 1.);
-
-               /* easy turns */
-               line2line(2175, 1597, 2175, 1097,
-                         2175, 1097, 1500, 722,
-                         RADIUS_EASY_TURN, DIST_EASY_TURN);
-               line2line(2175, 1097, 1500, 722,
-                         1500, 722, 1050, 972,
-                         RADIUS_EASY_TURN, DIST_EASY_TURN);
-               line2line(1500, 722, 1050, 972,
-                         1050, 972, 375, 597,
-                         RADIUS_EASY_TURN, DIST_EASY_TURN);
-
-               /* hard turn */
-               line2line(1050, 972, 375, 597,
-                         375, 597, 375, 1097,
-                         RADIUS_HARD_TURN, DIST_HARD_TURN);
-
-               /****** PASS2 */
-
-               /* easy turn */
-               line2line(375, 597, 375, 1097,
-                         375, 1097, 1050, 1472,
-                         RADIUS_EASY_TURN, DIST_EASY_TURN);
-
-               /* hard turn */
-               line2line(375, 1097, 1050, 1472,
-                         1050, 1472, 375, 1847,
-                         RADIUS_HARD_TURN, DIST_HARD_TURN);
-
-               /* hard turn */
-               line2line(1050, 1472, 375, 1847,
-                         375, 1847, 375, 1347,
-                         RADIUS_HARD_TURN, DIST_HARD_TURN);
-
-               /* easy turn */
-               line2line(375, 1847, 375, 1347,
-                         375, 1347, 1050, 972,
-                         RADIUS_EASY_TURN, DIST_EASY_TURN);
-
-               /* hard turn */
-               line2line(375, 1347, 1050, 972,
-                         1050, 972, 375, 597,
-                         RADIUS_HARD_TURN, DIST_HARD_TURN);
-
-               /* hard turn */
-               line2line(1050, 972, 375, 597,
-                         375, 597, 375, 1847,
-                         RADIUS_HARD_TURN, DIST_HARD_TURN);
-
-       }
        trajectory_hardstop(&mainboard.traj);
 }
 
index 7bf6057..8c59996 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
@@ -17,7 +17,7 @@
  *
  *  Revision : $Id: commands_traj.c,v 1.8 2009-11-08 17:24:33 zer0 Exp $
  *
- *  Olivier MATZ <zer0@droids-corp.org> 
+ *  Olivier MATZ <zer0@droids-corp.org>
  */
 
 #include <stdio.h>
 struct cmd_traj_speed_result {
        fixed_string_t arg0;
        fixed_string_t arg1;
-       uint16_t s;
+       float s;
 };
 
 /* function called when cmd_traj_speed is parsed successfully */
 static void cmd_traj_speed_parsed(void *parsed_result, void *data)
 {
        struct cmd_traj_speed_result * res = parsed_result;
-       
+
        if (!strcmp_P(res->arg1, PSTR("angle"))) {
                trajectory_set_speed(&mainboard.traj, mainboard.traj.d_speed, res->s);
        }
@@ -85,7 +85,7 @@ static void cmd_traj_speed_parsed(void *parsed_result, void *data)
        }
        /* else it is a "show" */
 
-       printf_P(PSTR("angle %u, distance %u\r\n"), 
+       printf_P(PSTR("angle %2.2f, distance %2.2f\r\n"),
                 mainboard.traj.a_speed,
                 mainboard.traj.d_speed);
 }
@@ -94,7 +94,7 @@ prog_char str_traj_speed_arg0[] = "traj_speed";
 parse_pgm_token_string_t cmd_traj_speed_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg0, str_traj_speed_arg0);
 prog_char str_traj_speed_arg1[] = "angle#distance";
 parse_pgm_token_string_t cmd_traj_speed_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg1, str_traj_speed_arg1);
-parse_pgm_token_num_t cmd_traj_speed_s = TOKEN_NUM_INITIALIZER(struct cmd_traj_speed_result, s, UINT16);
+parse_pgm_token_num_t cmd_traj_speed_s = TOKEN_NUM_INITIALIZER(struct cmd_traj_speed_result, s, FLOAT);
 
 prog_char help_traj_speed[] = "Set traj_speed values for trajectory manager";
 parse_pgm_inst_t cmd_traj_speed = {
@@ -102,9 +102,9 @@ parse_pgm_inst_t cmd_traj_speed = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_traj_speed,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_traj_speed_arg0, 
-               (prog_void *)&cmd_traj_speed_arg1, 
-               (prog_void *)&cmd_traj_speed_s, 
+               (prog_void *)&cmd_traj_speed_arg0,
+               (prog_void *)&cmd_traj_speed_arg1,
+               (prog_void *)&cmd_traj_speed_s,
                NULL,
        },
 };
@@ -120,13 +120,77 @@ parse_pgm_inst_t cmd_traj_speed_show = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_traj_speed_show,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_traj_speed_arg0, 
+               (prog_void *)&cmd_traj_speed_arg0,
                (prog_void *)&cmd_traj_speed_show_arg,
                NULL,
        },
 };
 
 /**********************************************************/
+/* Traj_Accs for trajectory_manager */
+
+/* this structure is filled when cmd_traj_acc is parsed successfully */
+struct cmd_traj_acc_result {
+       fixed_string_t arg0;
+       fixed_string_t arg1;
+       float s;
+};
+
+/* function called when cmd_traj_acc is parsed successfully */
+static void cmd_traj_acc_parsed(void *parsed_result, void *data)
+{
+       struct cmd_traj_acc_result * res = parsed_result;
+
+       if (!strcmp_P(res->arg1, PSTR("angle"))) {
+               trajectory_set_acc(&mainboard.traj, mainboard.traj.d_acc, res->s);
+       }
+       else if (!strcmp_P(res->arg1, PSTR("distance"))) {
+               trajectory_set_acc(&mainboard.traj, res->s, mainboard.traj.a_acc);
+       }
+       /* else it is a "show" */
+
+       printf_P(PSTR("angle %2.2f, distance %2.2f\r\n"),
+                mainboard.traj.a_acc,
+                mainboard.traj.d_acc);
+}
+
+prog_char str_traj_acc_arg0[] = "traj_acc";
+parse_pgm_token_string_t cmd_traj_acc_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_traj_acc_result, arg0, str_traj_acc_arg0);
+prog_char str_traj_acc_arg1[] = "angle#distance";
+parse_pgm_token_string_t cmd_traj_acc_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_traj_acc_result, arg1, str_traj_acc_arg1);
+parse_pgm_token_num_t cmd_traj_acc_s = TOKEN_NUM_INITIALIZER(struct cmd_traj_acc_result, s, FLOAT);
+
+prog_char help_traj_acc[] = "Set traj_acc values for trajectory manager";
+parse_pgm_inst_t cmd_traj_acc = {
+       .f = cmd_traj_acc_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_traj_acc,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_traj_acc_arg0,
+               (prog_void *)&cmd_traj_acc_arg1,
+               (prog_void *)&cmd_traj_acc_s,
+               NULL,
+       },
+};
+
+/* show */
+
+prog_char str_traj_acc_show_arg[] = "show";
+parse_pgm_token_string_t cmd_traj_acc_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_traj_acc_result, arg1, str_traj_acc_show_arg);
+
+prog_char help_traj_acc_show[] = "Show traj_acc values for trajectory manager";
+parse_pgm_inst_t cmd_traj_acc_show = {
+       .f = cmd_traj_acc_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_traj_acc_show,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_traj_acc_arg0,
+               (prog_void *)&cmd_traj_acc_show_arg,
+               NULL,
+       },
+};
+
+/**********************************************************/
 /* circle coef configuration */
 
 /* this structure is filled when cmd_circle_coef is parsed successfully */
@@ -179,7 +243,7 @@ parse_pgm_inst_t cmd_circle_coef_show = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_circle_coef_show,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_circle_coef_arg0, 
+               (prog_void *)&cmd_circle_coef_arg0,
                (prog_void *)&cmd_circle_coef_show_arg,
                NULL,
        },
@@ -202,7 +266,7 @@ struct cmd_trajectory_result {
 static void cmd_trajectory_parsed(void * parsed_result, void * data)
 {
        struct cmd_trajectory_result * res = parsed_result;
-       
+
        if (!strcmp_P(res->arg1, PSTR("set"))) {
                trajectory_set_windows(&mainboard.traj, res->d_win,
                                       res->a_win, res->a_start);
@@ -226,11 +290,11 @@ parse_pgm_inst_t cmd_trajectory = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_trajectory,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_trajectory_arg0, 
-               (prog_void *)&cmd_trajectory_arg1, 
-               (prog_void *)&cmd_trajectory_d, 
-               (prog_void *)&cmd_trajectory_a, 
-               (prog_void *)&cmd_trajectory_as, 
+               (prog_void *)&cmd_trajectory_arg0,
+               (prog_void *)&cmd_trajectory_arg1,
+               (prog_void *)&cmd_trajectory_d,
+               (prog_void *)&cmd_trajectory_a,
+               (prog_void *)&cmd_trajectory_as,
                NULL,
        },
 };
@@ -246,7 +310,7 @@ parse_pgm_inst_t cmd_trajectory_show = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_trajectory_show,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_trajectory_arg0, 
+               (prog_void *)&cmd_trajectory_arg0,
                (prog_void *)&cmd_trajectory_show_arg,
                NULL,
        },
@@ -272,9 +336,9 @@ static void cmd_rs_gains_parsed(void * parsed_result, void * data)
        struct cmd_rs_gains_result * res = parsed_result;
 
        if (!strcmp_P(res->arg1, PSTR("set"))) {
-               rs_set_left_ext_encoder(&mainboard.rs, encoders_spi_get_value, 
+               rs_set_left_ext_encoder(&mainboard.rs, encoders_spi_get_value,
                                        LEFT_ENCODER, res->left); // en augmentant on tourne à gauche
-               rs_set_right_ext_encoder(&mainboard.rs, encoders_spi_get_value, 
+               rs_set_right_ext_encoder(&mainboard.rs, encoders_spi_get_value,
                                         RIGHT_ENCODER, res->right); //en augmentant on tourne à droite
        }
        printf_P(PSTR("rs_gains set %2.2f %2.2f\r\n"),
@@ -295,10 +359,10 @@ parse_pgm_inst_t cmd_rs_gains = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_rs_gains,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_rs_gains_arg0, 
-               (prog_void *)&cmd_rs_gains_arg1, 
-               (prog_void *)&cmd_rs_gains_l, 
-               (prog_void *)&cmd_rs_gains_r, 
+               (prog_void *)&cmd_rs_gains_arg0,
+               (prog_void *)&cmd_rs_gains_arg1,
+               (prog_void *)&cmd_rs_gains_l,
+               (prog_void *)&cmd_rs_gains_r,
                NULL,
        },
 };
@@ -314,7 +378,7 @@ parse_pgm_inst_t cmd_rs_gains_show = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_rs_gains_show,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_rs_gains_arg0, 
+               (prog_void *)&cmd_rs_gains_arg0,
                (prog_void *)&cmd_rs_gains_show_arg,
                NULL,
        },
@@ -353,9 +417,9 @@ parse_pgm_inst_t cmd_track = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_track,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_track_arg0, 
-               (prog_void *)&cmd_track_arg1, 
-               (prog_void *)&cmd_track_val, 
+               (prog_void *)&cmd_track_arg0,
+               (prog_void *)&cmd_track_arg1,
+               (prog_void *)&cmd_track_val,
                NULL,
        },
 };
@@ -371,7 +435,7 @@ parse_pgm_inst_t cmd_track_show = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_track_show,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_track_arg0, 
+               (prog_void *)&cmd_track_arg0,
                (prog_void *)&cmd_track_show_arg,
                NULL,
        },
@@ -400,7 +464,7 @@ static void cmd_pt_list_parsed(void * parsed_result, void * data)
 {
        struct cmd_pt_list_result * res = parsed_result;
        uint8_t i, why=0;
-       
+
        if (!strcmp_P(res->arg1, PSTR("append"))) {
                res->arg2 = pt_list_len;
        }
@@ -419,7 +483,7 @@ static void cmd_pt_list_parsed(void * parsed_result, void * data)
                        printf_P(PSTR("List is too large\r\n"));
                        return;
                }
-               memmove(&pt_list[res->arg2+1], &pt_list[res->arg2], 
+               memmove(&pt_list[res->arg2+1], &pt_list[res->arg2],
                       PT_LIST_SIZE-1-res->arg2);
                pt_list[res->arg2].x = res->arg3;
                pt_list[res->arg2].y = res->arg4;
@@ -434,14 +498,14 @@ static void cmd_pt_list_parsed(void * parsed_result, void * data)
                        printf_P(PSTR("Index too large\r\n"));
                        return;
                }
-               memmove(&pt_list[res->arg2], &pt_list[res->arg2+1], 
+               memmove(&pt_list[res->arg2], &pt_list[res->arg2+1],
                       (PT_LIST_SIZE-1-res->arg2)*sizeof(struct xy_point));
                pt_list_len--;
        }
        else if (!strcmp_P(res->arg1, PSTR("reset"))) {
                pt_list_len = 0;
        }
-       
+
        /* else it is a "show" or a "start" */
        if (pt_list_len == 0) {
                printf_P(PSTR("List empty\r\n"));
@@ -493,11 +557,11 @@ parse_pgm_inst_t cmd_pt_list = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_pt_list,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_pt_list_arg0, 
-               (prog_void *)&cmd_pt_list_arg1, 
-               (prog_void *)&cmd_pt_list_arg2, 
-               (prog_void *)&cmd_pt_list_arg3, 
-               (prog_void *)&cmd_pt_list_arg4, 
+               (prog_void *)&cmd_pt_list_arg0,
+               (prog_void *)&cmd_pt_list_arg1,
+               (prog_void *)&cmd_pt_list_arg2,
+               (prog_void *)&cmd_pt_list_arg3,
+               (prog_void *)&cmd_pt_list_arg4,
                NULL,
        },
 };
@@ -513,10 +577,10 @@ parse_pgm_inst_t cmd_pt_list_append = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_pt_list_append,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_pt_list_arg0, 
-               (prog_void *)&cmd_pt_list_arg1_append, 
-               (prog_void *)&cmd_pt_list_arg3, 
-               (prog_void *)&cmd_pt_list_arg4, 
+               (prog_void *)&cmd_pt_list_arg0,
+               (prog_void *)&cmd_pt_list_arg1_append,
+               (prog_void *)&cmd_pt_list_arg3,
+               (prog_void *)&cmd_pt_list_arg4,
                NULL,
        },
 };
@@ -532,8 +596,8 @@ parse_pgm_inst_t cmd_pt_list_del = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_pt_list_del,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_pt_list_arg0, 
-               (prog_void *)&cmd_pt_list_del_arg, 
+               (prog_void *)&cmd_pt_list_arg0,
+               (prog_void *)&cmd_pt_list_del_arg,
                (prog_void *)&cmd_pt_list_arg2,
                NULL,
        },
@@ -549,7 +613,7 @@ parse_pgm_inst_t cmd_pt_list_show = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_pt_list_show,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_pt_list_arg0, 
+               (prog_void *)&cmd_pt_list_arg0,
                (prog_void *)&cmd_pt_list_show_arg,
                NULL,
        },
@@ -654,9 +718,9 @@ parse_pgm_inst_t cmd_goto1 = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_goto1,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_goto_arg0, 
-               (prog_void *)&cmd_goto_arg1_a, 
-               (prog_void *)&cmd_goto_arg2, 
+               (prog_void *)&cmd_goto_arg0,
+               (prog_void *)&cmd_goto_arg1_a,
+               (prog_void *)&cmd_goto_arg2,
                NULL,
        },
 };
@@ -672,10 +736,10 @@ parse_pgm_inst_t cmd_goto2 = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_goto2,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_goto_arg0, 
-               (prog_void *)&cmd_goto_arg1_b, 
+               (prog_void *)&cmd_goto_arg0,
+               (prog_void *)&cmd_goto_arg1_b,
                (prog_void *)&cmd_goto_arg2,
-               (prog_void *)&cmd_goto_arg3, 
+               (prog_void *)&cmd_goto_arg3,
                NULL,
        },
 };
@@ -780,7 +844,7 @@ static void cmd_position_parsed(void * parsed_result, void * data)
        }
 
        /* else it's just a "show" */
-       printf_P(PSTR("x=%.2f y=%.2f a=%.2f\r\n"), 
+       printf_P(PSTR("x=%.2f y=%.2f a=%.2f\r\n"),
                 position_get_x_double(&mainboard.pos),
                 position_get_y_double(&mainboard.pos),
                 DEG(position_get_a_rad_double(&mainboard.pos)));
@@ -797,8 +861,8 @@ parse_pgm_inst_t cmd_position = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_position,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_position_arg0, 
-               (prog_void *)&cmd_position_arg1, 
+               (prog_void *)&cmd_position_arg0,
+               (prog_void *)&cmd_position_arg1,
                NULL,
        },
 };
@@ -816,11 +880,11 @@ parse_pgm_inst_t cmd_position_set = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_position_set,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_position_arg0, 
-               (prog_void *)&cmd_position_arg1_set, 
-               (prog_void *)&cmd_position_arg2, 
-               (prog_void *)&cmd_position_arg3, 
-               (prog_void *)&cmd_position_arg4, 
+               (prog_void *)&cmd_position_arg0,
+               (prog_void *)&cmd_position_arg1_set,
+               (prog_void *)&cmd_position_arg2,
+               (prog_void *)&cmd_position_arg3,
+               (prog_void *)&cmd_position_arg4,
                NULL,
        },
 };
@@ -858,8 +922,8 @@ parse_pgm_inst_t cmd_strat_infos = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_strat_infos,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_strat_infos_arg0, 
-               (prog_void *)&cmd_strat_infos_arg1, 
+               (prog_void *)&cmd_strat_infos_arg0,
+               (prog_void *)&cmd_strat_infos_arg1,
                NULL,
        },
 };
@@ -893,8 +957,8 @@ parse_pgm_inst_t cmd_strat_conf = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_strat_conf,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_strat_conf_arg0, 
-               (prog_void *)&cmd_strat_conf_arg1, 
+               (prog_void *)&cmd_strat_conf_arg0,
+               (prog_void *)&cmd_strat_conf_arg1,
                NULL,
        },
 };
@@ -919,7 +983,7 @@ static void cmd_strat_conf2_parsed(void *parsed_result, void *data)
                on = 1;
        else
                on = 0;
-       
+
 #if 0
        if (!strcmp_P(res->arg1, PSTR("one_temple_on_disc")))
                bit = STRAT_CONF_ONLY_ONE_ON_DISC;
@@ -962,9 +1026,9 @@ parse_pgm_inst_t cmd_strat_conf2 = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_strat_conf2,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_strat_conf2_arg0, 
-               (prog_void *)&cmd_strat_conf2_arg1, 
-               (prog_void *)&cmd_strat_conf2_arg2, 
+               (prog_void *)&cmd_strat_conf2_arg0,
+               (prog_void *)&cmd_strat_conf2_arg1,
+               (prog_void *)&cmd_strat_conf2_arg2,
                NULL,
        },
 };
@@ -1028,9 +1092,9 @@ parse_pgm_inst_t cmd_strat_conf3 = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_strat_conf3,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_strat_conf3_arg0, 
-               (prog_void *)&cmd_strat_conf3_arg1, 
-               (prog_void *)&cmd_strat_conf3_arg2, 
+               (prog_void *)&cmd_strat_conf3_arg0,
+               (prog_void *)&cmd_strat_conf3_arg1,
+               (prog_void *)&cmd_strat_conf3_arg2,
                NULL,
        },
 };
@@ -1039,6 +1103,224 @@ parse_pgm_inst_t cmd_strat_conf3 = {
 /**********************************************************/
 /* Subtraj */
 
+//////////////////////
+
+// 500 -- 5
+// 400 -- 3
+#define TEST_SPEED 400
+#define TEST_ACC 3
+
+static void line2line(double line1x1, double line1y1,
+                     double line1x2, double line1y2,
+                     double line2x1, double line2y1,
+                     double line2x2, double line2y2,
+                     double radius, double dist)
+{
+       uint8_t err;
+       double speed_d, speed_a;
+       double distance, angle;
+       double line1_angle = atan2(line1y2-line1y1, line1x2-line1x1);
+       double line2_angle = atan2(line2y2-line2y1, line2x2-line2x1);
+
+       printf_P(PSTR("%s()\r\n"), __FUNCTION__);
+
+       strat_set_speed(TEST_SPEED, TEST_SPEED);
+       quadramp_set_2nd_order_vars(&mainboard.angle.qr, TEST_ACC, TEST_ACC);
+
+       circle_get_da_speed_from_radius(&mainboard.traj, radius,
+                                       &speed_d, &speed_a);
+       trajectory_line_abs(&mainboard.traj,
+                           line1x1, line1y1,
+                           line1x2, line1y2, 150.);
+       err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line1x2, line1y2) <
+                                   dist, TRAJ_FLAGS_NO_NEAR);
+       /* circle */
+       strat_set_speed(speed_d, speed_a);
+       angle = line2_angle - line1_angle;
+       distance = angle * radius;
+       if (distance < 0)
+               distance = -distance;
+       angle = simple_modulo_2pi(angle);
+       angle = DEG(angle);
+       printf_P(PSTR("(%d,%d,%d) "),
+                position_get_x_s16(&mainboard.pos),
+                position_get_y_s16(&mainboard.pos),
+                position_get_a_deg_s16(&mainboard.pos));
+       printf_P(PSTR("circle distance=%2.2f angle=%2.2f\r\n"),
+                distance, angle);
+
+       /* take some margin on dist to avoid deceleration */
+       trajectory_d_a_rel(&mainboard.traj, distance + 250, angle);
+
+       /* circle exit condition */
+       err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
+                                   TRAJ_FLAGS_NO_NEAR);
+
+       strat_set_speed(500, 500);
+       printf_P(PSTR("(%d,%d,%d) "),
+                position_get_x_s16(&mainboard.pos),
+                position_get_y_s16(&mainboard.pos),
+                position_get_a_deg_s16(&mainboard.pos));
+       printf_P(PSTR("line\r\n"));
+       trajectory_line_abs(&mainboard.traj,
+                           line2x1, line2y1,
+                           line2x2, line2y2, 150.);
+}
+
+static void halfturn(double line1x1, double line1y1,
+                    double line1x2, double line1y2,
+                    double line2x1, double line2y1,
+                    double line2x2, double line2y2,
+                    double radius, double dist, double dir)
+{
+       uint8_t err;
+       double speed_d, speed_a;
+       double distance, angle;
+
+       printf_P(PSTR("%s()\r\n"), __FUNCTION__);
+
+       strat_set_speed(TEST_SPEED, TEST_SPEED);
+       quadramp_set_2nd_order_vars(&mainboard.angle.qr, TEST_ACC, TEST_ACC);
+
+       circle_get_da_speed_from_radius(&mainboard.traj, radius,
+                                       &speed_d, &speed_a);
+       trajectory_line_abs(&mainboard.traj,
+                           line1x1, line1y1,
+                           line1x2, line1y2, 150.);
+       err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line1x2, line1y2) <
+                                   dist, TRAJ_FLAGS_NO_NEAR);
+       /* circle */
+       strat_set_speed(speed_d, speed_a);
+       angle = dir * M_PI/2.;
+       distance = angle * radius;
+       if (distance < 0)
+               distance = -distance;
+       angle = simple_modulo_2pi(angle);
+       angle = DEG(angle);
+
+       /* take some margin on dist to avoid deceleration */
+       DEBUG(E_USER_STRAT, "circle1 distance=%2.2f angle=%2.2f",
+             distance, angle);
+       trajectory_d_a_rel(&mainboard.traj, distance + 500, angle);
+
+       /* circle exit condition */
+       err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
+                                   TRAJ_FLAGS_NO_NEAR);
+
+       DEBUG(E_USER_STRAT, "miniline");
+       err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line2x1, line2y1) <
+                                   dist, TRAJ_FLAGS_NO_NEAR);
+       DEBUG(E_USER_STRAT, "circle2");
+       /* take some margin on dist to avoid deceleration */
+       trajectory_d_a_rel(&mainboard.traj, distance + 500, angle);
+
+       err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
+                                   TRAJ_FLAGS_NO_NEAR);
+
+       strat_set_speed(500, 500);
+       DEBUG(E_USER_STRAT, "line");
+       trajectory_line_abs(&mainboard.traj,
+                           line2x1, line2y1,
+                           line2x2, line2y2, 150.);
+}
+
+
+/* function called when cmd_test is parsed successfully */
+static void subtraj_test(void)
+{
+#ifdef HOST_VERSION
+       strat_reset_pos(400, 400, 90);
+       mainboard.angle.on = 1;
+       mainboard.distance.on = 1;
+#endif
+       printf_P(PSTR("%s()\r\n"), __FUNCTION__);
+       while (!cmdline_keypressed()) {
+               /****** PASS1 */
+
+#define DIST_HARD_TURN   260
+#define RADIUS_HARD_TURN 100
+#define DIST_EASY_TURN   190
+#define RADIUS_EASY_TURN 190
+#define DIST_HALF_TURN   225
+#define RADIUS_HALF_TURN 130
+
+               /* hard turn */
+               line2line(375, 597, 375, 1847,
+                         375, 1847, 1050, 1472,
+                         RADIUS_HARD_TURN, DIST_HARD_TURN);
+
+               /* easy left and easy right !*/
+               line2line(825, 1596, 1050, 1472,
+                         1050, 1472, 1500, 1722,
+                         RADIUS_EASY_TURN, DIST_EASY_TURN);
+               line2line(1050, 1472, 1500, 1722,
+                         1500, 1722, 2175, 1347,
+                         RADIUS_EASY_TURN, DIST_EASY_TURN);
+               line2line(1500, 1722, 2175, 1347,
+                         2175, 1347, 2175, 847,
+                         RADIUS_EASY_TURN, DIST_EASY_TURN);
+
+               /* half turns */
+               halfturn(2175, 1347, 2175, 722,
+                        2625, 722, 2625, 1597,
+                        RADIUS_HALF_TURN, DIST_HALF_TURN, 1.);
+               halfturn(2625, 847, 2625, 1722,
+                         2175, 1722, 2175, 1097,
+                         RADIUS_HALF_TURN, DIST_HALF_TURN, 1.);
+
+               /* easy turns */
+               line2line(2175, 1597, 2175, 1097,
+                         2175, 1097, 1500, 722,
+                         RADIUS_EASY_TURN, DIST_EASY_TURN);
+               line2line(2175, 1097, 1500, 722,
+                         1500, 722, 1050, 972,
+                         RADIUS_EASY_TURN, DIST_EASY_TURN);
+               line2line(1500, 722, 1050, 972,
+                         1050, 972, 375, 597,
+                         RADIUS_EASY_TURN, DIST_EASY_TURN);
+
+               /* hard turn */
+               line2line(1050, 972, 375, 597,
+                         375, 597, 375, 1097,
+                         RADIUS_HARD_TURN, DIST_HARD_TURN);
+
+               /****** PASS2 */
+
+               /* easy turn */
+               line2line(375, 597, 375, 1097,
+                         375, 1097, 1050, 1472,
+                         RADIUS_EASY_TURN, DIST_EASY_TURN);
+
+               /* hard turn */
+               line2line(375, 1097, 1050, 1472,
+                         1050, 1472, 375, 1847,
+                         RADIUS_HARD_TURN, DIST_HARD_TURN);
+
+               /* hard turn */
+               line2line(1050, 1472, 375, 1847,
+                         375, 1847, 375, 1347,
+                         RADIUS_HARD_TURN, DIST_HARD_TURN);
+
+               /* easy turn */
+               line2line(375, 1847, 375, 1347,
+                         375, 1347, 1050, 972,
+                         RADIUS_EASY_TURN, DIST_EASY_TURN);
+
+               /* hard turn */
+               line2line(375, 1347, 1050, 972,
+                         1050, 972, 375, 597,
+                         RADIUS_HARD_TURN, DIST_HARD_TURN);
+
+               /* hard turn */
+               line2line(1050, 972, 375, 597,
+                         375, 597, 375, 1847,
+                         RADIUS_HARD_TURN, DIST_HARD_TURN);
+
+       }
+       trajectory_hardstop(&mainboard.traj);
+}
+
+
 /* this structure is filled when cmd_subtraj is parsed successfully */
 struct cmd_subtraj_result {
        fixed_string_t arg0;
@@ -1052,9 +1334,11 @@ struct cmd_subtraj_result {
 /* function called when cmd_subtraj is parsed successfully */
 static void cmd_subtraj_parsed(void *parsed_result, void *data)
 {
-/*     struct cmd_subtraj_result *res = parsed_result; */
+       struct cmd_subtraj_result *res = parsed_result;
+
+       if (!strcmp_P(res->arg1, PSTR("test")))
+               subtraj_test();
 
-       printf_P(PSTR("TODO\r\n"));
        trajectory_hardstop(&mainboard.traj);
 }
 
@@ -1073,12 +1357,13 @@ parse_pgm_inst_t cmd_subtraj = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_subtraj,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_subtraj_arg0, 
-               (prog_void *)&cmd_subtraj_arg1, 
-               (prog_void *)&cmd_subtraj_arg2, 
-               (prog_void *)&cmd_subtraj_arg3, 
-               (prog_void *)&cmd_subtraj_arg4, 
-               (prog_void *)&cmd_subtraj_arg5, 
+               (prog_void *)&cmd_subtraj_arg0,
+               (prog_void *)&cmd_subtraj_arg1,
+               (prog_void *)&cmd_subtraj_arg2,
+               (prog_void *)&cmd_subtraj_arg3,
+               (prog_void *)&cmd_subtraj_arg4,
+               (prog_void *)&cmd_subtraj_arg5,
                NULL,
        },
 };
+
index 905d533..b8d6e08 100644 (file)
@@ -126,12 +126,19 @@ static void do_cs(void *dummy)
                 * compensation) */
                position_manage(&mainboard.pos);
        }
-       if (mainboard.flags & DO_BD) {
+       if ((mainboard.flags & DO_BD) && (mainboard.flags & DO_POWER)) {
                bd_manage_from_cs(&mainboard.angle.bd, &mainboard.angle.cs);
                bd_manage_from_cs(&mainboard.distance.bd, &mainboard.distance.cs);
 #ifndef HOST_VERSION
                bd_manage_from_cs(&mainboard.left_cobroller.bd, &mainboard.left_cobroller.cs);
                bd_manage_from_cs(&mainboard.right_cobroller.bd, &mainboard.right_cobroller.cs);
+               if (mainboard.flags & DO_ERRBLOCKING) {
+                       if (bd_get(&mainboard.left_cobroller.bd) ||
+                           bd_get(&mainboard.left_cobroller.bd)) {
+                               printf_P(PSTR("MOTOR BLOCKED STOP ALL\r\n"));
+                               mainboard.flags &= ~(DO_POWER | DO_ERRBLOCKING);
+                       }
+               }
 #endif
        }
 #ifndef HOST_VERSION
@@ -226,7 +233,7 @@ void microb_cs_init(void)
                          &mainboard.angle.cs);
        trajectory_set_robot_params(&mainboard.traj, &mainboard.rs, &mainboard.pos);
        trajectory_set_speed(&mainboard.traj, SPEED_DIST_FAST, SPEED_ANGLE_FAST); /* d, a */
-       trajectory_set_speed(&mainboard.traj, ACC_DIST, ACC_ANGLE); /* d, a */
+       trajectory_set_acc(&mainboard.traj, ACC_DIST, ACC_ANGLE); /* d, a */
        /* distance window, angle window, angle start */
        trajectory_set_windows(&mainboard.traj, 200., 5.0, 30.);
 
index ad8cff2..64fa521 100644 (file)
@@ -1,6 +1,9 @@
-import math, sys, time, os, random
+import math, sys, time, os, random, re
 from visual import *
 
+FLOAT = "([-+]?[0-9]*\.?[0-9]+)"
+INT = "([-+]?[0-9][0-9]*)"
+
 AREA_X = 3000.
 AREA_Y = 2100.
 
@@ -16,11 +19,11 @@ scene.autoscale=0
 # all positions of robot every 5ms
 save_pos = []
 
-robot = box(pos = (0, 0, ROBOT_HEIGHT/2),
-            size = (250,320,ROBOT_HEIGHT),
-            color = (0.3, 0.3, 0.3) )
+robot = box(color=(0.4, 0.4, 0.4))
+lspickle = box(color=(0.4, 0.4, 0.4))
+rspickle = box(color=(0.4, 0.4, 0.4))
 
-last_pos = robot.pos.x, robot.pos.y, robot.pos.z
+last_pos = (0.,0.,0.)
 hcenter_line = curve()
 hcenter_line.pos = [(-AREA_X/2, 0., 0.3), (AREA_X/2, 0., 0.3)]
 vcenter_line = curve()
@@ -50,6 +53,11 @@ def square(sz):
 sq1 = square(250)
 sq2 = square(500)
 
+robot_x = 0.
+robot_y = 0.
+robot_a = 0.
+robot_lspickle = 0
+robot_rspickle = 0
 robot_trail = curve()
 robot_trail_list = []
 max_trail = 500
@@ -181,6 +189,21 @@ def init_waypoints():
 def toggle_obj_disp():
     global area_objects
 
+    """
+    if area_objects == []:
+        c = sphere(radius=5, color=(0., 0.,1.),
+                   pos=(1238.-AREA_X/2, 1313.-AREA_Y/2, 5))
+        area_objects.append(c)
+        c = sphere(radius=5, color=(0., 0.,1.),
+                   pos=(1364.-AREA_X/2, 1097.-AREA_Y/2, 5))
+        area_objects.append(c)
+        c = sphere(radius=5, color=(0., 0.,1.),
+                   pos=(1453.-AREA_X/2, 1176.-AREA_Y/2, 5))
+        area_objects.append(c)
+        c = sphere(radius=5, color=(0., 0.,1.),
+                   pos=(1109.-AREA_X/2, 1050.-AREA_Y/2, 5))
+        area_objects.append(c)
+"""
     if area_objects == []:
         i = 0
         j = 0
@@ -223,18 +246,32 @@ def toggle_obj_disp():
                 o.visible = 1
 
 
-def set_robot(x, y, a):
+def set_robot():
     global robot, last_pos, robot_trail, robot_trail_list
-    global save_pos
+    global save_pos, robot_x, robot_y, robot_a
 
-    robot.pos = (x - AREA_X/2, y - AREA_Y/2, ROBOT_HEIGHT/2)
-    robot.axis = (math.cos(a*math.pi/180),
-                  math.sin(a*math.pi/180),
-                  0)
+    axis = (math.cos(robot_a*math.pi/180),
+            math.sin(robot_a*math.pi/180),
+            0)
+
+    robot.pos = (robot_x - AREA_X/2, robot_y - AREA_Y/2, ROBOT_HEIGHT/2)
+    robot.axis = axis
     robot.size = (250, 320, ROBOT_HEIGHT)
 
+    lspickle.pos = (robot_x - AREA_X/2 + (robot_lspickle*70) * math.cos((robot_a-90)*math.pi/180),
+                    robot_y - AREA_Y/2 + (robot_lspickle*70) * math.sin((robot_a-90)*math.pi/180),
+                    ROBOT_HEIGHT/2)
+    lspickle.axis = axis
+    lspickle.size = (20, 320, 5)
+
+    rspickle.pos = (robot_x - AREA_X/2 + (robot_rspickle*70) * math.cos((robot_a+90)*math.pi/180),
+                    robot_y - AREA_Y/2 + (robot_rspickle*70) * math.sin((robot_a+90)*math.pi/180),
+                    ROBOT_HEIGHT/2)
+    rspickle.axis = axis
+    rspickle.size = (20, 320, 5)
+
     # save position
-    save_pos.append((robot.pos.x, robot.pos, a))
+    save_pos.append((robot.pos.x, robot.pos, robot_a))
 
     pos = robot.pos.x, robot.pos.y, 0.3
     if pos != last_pos:
@@ -272,11 +309,30 @@ while True:
         fw = open("/tmp/.robot_dis2sim", "w", 0)
         while True:
             l = fr.readline()
-            try:
-                x,y,a = map(lambda x:int(x), l[:-1].split(" "))
-                set_robot(x,y,a)
-            except ValueError:
-                pass
+            m = re.match("pos=%s,%s,%s"%(INT,INT,INT), l)
+            if m:
+                robot_x = int(m.groups()[0])
+                robot_y = int(m.groups()[1])
+                robot_a = int(m.groups()[2])
+                set_robot()
+            m = re.match("ballboard=%s"%(INT), l)
+            if m:
+                print int(m.groups()[0])
+            m = re.match("cobboard=%s"%(INT), l)
+            if m:
+                mode = int(m.groups()[0])
+                if (mode & 0x01) == 0:
+                    robot_lspickle = 0
+                elif (mode & 0x02) == 0:
+                    robot_lspickle = 1
+                else:
+                    robot_lspickle = 2
+                if (mode & 0x04) == 0:
+                    robot_rspickle = 0
+                elif (mode & 0x08) == 0:
+                    robot_rspickle = 1
+                else:
+                    robot_rspickle = 2
 
             if scene.kb.keys == 0:
                 continue
index 4e5c174..a4aa028 100644 (file)
@@ -53,6 +53,9 @@
 #include "main.h"
 #include "sensor.h"
 #include "i2c_protocol.h"
+#ifdef HOST_VERSION
+#include "robotsim.h"
+#endif
 
 #define I2C_STATE_MAX 4
 
@@ -321,7 +324,7 @@ static int8_t
 i2c_send_command(uint8_t addr, uint8_t * buf, uint8_t size)
 {
 #ifdef HOST_VERSION
-       return 0;
+       return robotsim_i2c(addr, buf, size);
 #else
        uint8_t flags;
         microseconds us = time_get_us2();
@@ -458,6 +461,6 @@ int8_t i2c_ballboard_set_mode(uint8_t mode)
        struct i2c_cmd_ballboard_set_mode buf;
        buf.hdr.cmd = I2C_CMD_BALLBOARD_SET_MODE;
        buf.mode = mode;
-       return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+       return i2c_send_command(I2C_BALLBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
 }
 
index 88d4f97..98d3749 100755 (executable)
@@ -133,9 +133,21 @@ void do_time_monitor(void *dummy)
 
 void do_led_blink(void *dummy)
 {
-#if 1 /* simple blink */
-       LED1_TOGGLE();
-#endif
+       static uint8_t a = 0;
+
+       if (mainboard.flags & DO_ERRBLOCKING) {
+               if (a & 1)
+                       LED1_ON();
+               else
+                       LED1_OFF();
+       }
+       else {
+               if (a & 4)
+                       LED1_ON();
+               else
+                       LED1_OFF();
+       }
+       a++;
 }
 
 static void main_timer_interrupt(void)
@@ -171,7 +183,7 @@ int main(void)
        memset(&gen, 0, sizeof(gen));
        memset(&mainboard, 0, sizeof(mainboard));
        mainboard.flags = DO_ENCODERS | DO_CS | DO_RS |
-               DO_POS | DO_POWER | DO_BD;
+               DO_POS | DO_POWER | DO_BD | DO_ERRBLOCKING;
 
        /* UART */
        uart_init();
@@ -262,13 +274,13 @@ int main(void)
        /* all cs management */
        microb_cs_init();
 
+       /* TIME */
+       time_init(TIME_PRIO);
+
 #ifndef HOST_VERSION
        /* sensors, will also init hardware adc */
        sensor_init();
 
-       /* TIME */
-       time_init(TIME_PRIO);
-
        /* start i2c slave polling */
        scheduler_add_periodical_event_priority(i2c_poll_slaves, NULL,
                                                8000L / SCHEDULER_UNIT, I2C_POLL_PRIO);
@@ -281,7 +293,7 @@ int main(void)
 
        /* strat-related event */
        scheduler_add_periodical_event_priority(strat_event, NULL,
-                                               25000L / SCHEDULER_UNIT,
+                                               10000L / SCHEDULER_UNIT,
                                                STRAT_PRIO);
 
 #ifndef HOST_VERSION
index 48655eb..6e3f8ad 100755 (executable)
@@ -168,6 +168,7 @@ struct mainboard {
 #define DO_BD       16
 #define DO_TIMER    32
 #define DO_POWER    64
+#define DO_ERRBLOCKING 128
        uint8_t flags;                /* misc flags */
 
        /* control systems */
index d0aa3ae..5fc2e37 100644 (file)
@@ -48,6 +48,7 @@
 #include <parse.h>
 #include <rdline.h>
 
+#include "../common/i2c_commands.h"
 #include "strat.h"
 #include "strat_utils.h"
 #include "main.h"
@@ -67,7 +68,7 @@ void robotsim_dump(void)
        char buf[BUFSIZ];
        int len;
 
-       len = snprintf(buf, sizeof(buf), "%d %d %d\n",
+       len = snprintf(buf, sizeof(buf), "pos=%d,%d,%d\n",
                      position_get_x_s16(&mainboard.pos),
                      position_get_y_s16(&mainboard.pos),
                      position_get_a_deg_s16(&mainboard.pos));
@@ -76,6 +77,81 @@ void robotsim_dump(void)
        hostsim_unlock();
 }
 
+static int8_t
+robotsim_i2c_ballboard_set_mode(struct i2c_cmd_ballboard_set_mode *cmd)
+{
+       char buf[BUFSIZ];
+       int len;
+
+       ballboard.mode = cmd->mode;
+       len = snprintf(buf, sizeof(buf), "ballboard=%d\n", cmd->mode);
+       hostsim_lock();
+       write(fdw, buf, len);
+       hostsim_unlock();
+       return 0;
+}
+
+static int8_t
+robotsim_i2c_cobboard_set_mode(struct i2c_cmd_cobboard_set_mode *cmd)
+{
+       char buf[BUFSIZ];
+       int len;
+
+       cobboard.mode = cmd->mode;
+       len = snprintf(buf, sizeof(buf), "cobboard=%d\n", cmd->mode);
+       hostsim_lock();
+       write(fdw, buf, len);
+       hostsim_unlock();
+       return 0;
+}
+
+static int8_t
+robotsim_i2c_ballboard(uint8_t addr, uint8_t *buf, uint8_t size)
+{
+       void *void_cmd = buf;
+
+       switch (buf[0]) {
+       case I2C_CMD_BALLBOARD_SET_MODE:
+               {
+                       struct i2c_cmd_ballboard_set_mode *cmd = void_cmd;
+                       robotsim_i2c_ballboard_set_mode(cmd);
+                       break;
+               }
+
+       default:
+               break;
+       }
+       return 0;
+}
+
+static int8_t
+robotsim_i2c_cobboard(uint8_t addr, uint8_t *buf, uint8_t size)
+{
+       void *void_cmd = buf;
+
+       switch (buf[0]) {
+       case I2C_CMD_COBBOARD_SET_MODE:
+               {
+                       struct i2c_cmd_cobboard_set_mode *cmd = void_cmd;
+                       robotsim_i2c_cobboard_set_mode(cmd);
+                       break;
+               }
+       default:
+               break;
+       }
+       return 0;
+}
+
+int8_t
+robotsim_i2c(uint8_t addr, uint8_t *buf, uint8_t size)
+{
+       if (addr == I2C_BALLBOARD_ADDR)
+               return robotsim_i2c_ballboard(addr, buf, size);
+       else if (addr == I2C_COBBOARD_ADDR)
+               return robotsim_i2c_cobboard(addr, buf, size);
+       return 0;
+}
+
 /* must be called periodically */
 void robotsim_update(void)
 {
index 8777de8..a9316b9 100644 (file)
@@ -1,6 +1,6 @@
-/*  
+/*
  *  Copyright Droids Corporation, Microb Technology, Eirbot (2005)
- * 
+ *
  *  This program is free software; you can redistribute it and/or modify
  *  it under the terms of the GNU General Public License as published by
  *  the Free Software Foundation; either version 2 of the License, or
@@ -19,6 +19,7 @@
  *
  */
 
+int8_t robotsim_i2c(uint8_t addr, uint8_t *buf, uint8_t size);
 void robotsim_update(void);
 void robotsim_pwm(void *arg, int32_t val);
 int32_t robotsim_encoder_get(void *arg);
index 6743d37..97d159c 100644 (file)
@@ -1,6 +1,6 @@
-/*  
+/*
  *  Copyright Droids, Microb Technology (2009)
- * 
+ *
  *  This program is free software; you can redistribute it and/or modify
  *  it under the terms of the GNU General Public License as published by
  *  the Free Software Foundation; either version 2 of the License, or
@@ -17,7 +17,7 @@
  *
  *  Revision : $Id: strat.c,v 1.6 2009-11-08 17:24:33 zer0 Exp $
  *
- *  Olivier MATZ <zer0@droids-corp.org> 
+ *  Olivier MATZ <zer0@droids-corp.org>
  */
 
 #include <stdio.h>
@@ -70,7 +70,10 @@ struct strat_infos strat_infos = {
        .conf = {
                .flags = 0,
        },
-
+       /* status */
+       .status = {
+               .flags = 0,
+       },
 };
 
 /*************************************************************/
@@ -150,7 +153,7 @@ void strat_init(void)
        interrupt_traj_reset();
 
        /* used in strat_base for END_TIMER */
-       mainboard.flags = DO_ENCODERS | DO_CS | DO_RS | 
+       mainboard.flags = DO_ENCODERS | DO_CS | DO_RS |
                DO_POS | DO_BD | DO_TIMER | DO_POWER;
 }
 
@@ -174,6 +177,17 @@ void strat_exit(void)
 /* called periodically */
 void strat_event(void *dummy)
 {
+#if 0
+       /* pack or deploy spickle */
+       if (strat_infos.status.flags & STRAT_STATUS_LHARVEST) {
+               if (sensor_get(S_LCOB_PRESENT)) {
+                       if (sensor_get(S_LCOB_WHITE))
+                               i2c_ballboard_set_mode();
+                       else
+                               ;
+               }
+       }
+#endif
        /* limit speed when opponent is close */
        strat_limit_speed();
 }
@@ -197,7 +211,7 @@ uint8_t strat_main(void)
 {
        uint8_t err;
 
-       /* do static cols + first temple */
+       /* */
        err = strat_beginning();
 
        return err;
index 53d9192..d852396 100644 (file)
 
 /* strat infos structures */
 
-struct bbox {
+struct strat_bbox {
        int32_t x1;
        int32_t y1;
        int32_t x2;
        int32_t y2;
 };
 
-struct conf {
+struct strat_conf {
 #define STRAT_CONF_XXX   0x01
        uint8_t flags;
 };
 
+struct strat_status {
+#define STRAT_STATUS_LHARVEST 0x01
+#define STRAT_STATUS_RHARVEST 0x02
+       uint8_t flags;
+};
+
 /* all infos related to strat */
 struct strat_infos {
        uint8_t dump_enabled;
-       struct conf conf;
-       struct bbox area_bbox;
+       struct strat_conf conf;
+       struct strat_bbox area_bbox;
+       struct strat_status status;
 };
 extern struct strat_infos strat_infos;