command circle set coef

diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager.c b/modules/devices/robot/trajectory_manager/trajectory_manager.c
index 5fe3865..773388e 100644 (file)
--- a/modules/devices/robot/trajectory_manager/trajectory_manager.c
+++ b/modules/devices/robot/trajectory_manager/trajectory_manager.c
@@ -77,7 +77,7 @@ void trajectory_set_robot_params(struct trajectory *traj,
 }
 
 /** set speed consign */
 }
 
 /** set speed consign */

-void trajectory_set_speed( struct trajectory *traj, int16_t d_speed, int16_t a_speed)
+void trajectory_set_speed(struct trajectory *traj, int16_t d_speed, int16_t a_speed)

 {
        uint8_t flags;
        IRQ_LOCK(flags);
 {
        uint8_t flags;
        IRQ_LOCK(flags);


@@ -98,5 +98,11 @@ void trajectory_set_windows(struct trajectory *traj, double d_win,
        IRQ_UNLOCK(flags);
 }
 
        IRQ_UNLOCK(flags);
 }
 

-
-
+/** set corrective coef for circle */
+void trajectory_set_circle_coef(struct trajectory *traj, double coef)
+{
+       uint8_t flags;
+       IRQ_LOCK(flags);
+       traj->circle_coef = coef ;
+       IRQ_UNLOCK(flags);
+}

diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager.h b/modules/devices/robot/trajectory_manager/trajectory_manager.h
index 3ebd586..f18e272 100644 (file)
--- a/modules/devices/robot/trajectory_manager/trajectory_manager.h
+++ b/modules/devices/robot/trajectory_manager/trajectory_manager.h
@@ -70,7 +70,8 @@ struct trajectory {
        double a_win_rad;  /**<< angle window (for END_NEAR) */
        double a_start_rad;/**<< in xy consigns, start to move in distance
                            *    when a_target < a_start */
        double a_win_rad;  /**<< angle window (for END_NEAR) */
        double a_start_rad;/**<< in xy consigns, start to move in distance
                            *    when a_target < a_start */

-  
+       double circle_coef;/**<< corrective circle coef */
+

        uint16_t d_speed;  /**<< distance speed consign */
        uint16_t a_speed;  /**<< angle speed consign */
 
        uint16_t d_speed;  /**<< distance speed consign */
        uint16_t a_speed;  /**<< angle speed consign */
 


@@ -78,7 +79,7 @@ struct trajectory {
        struct robot_system *robot;      /**<< associated robot_system */
        struct cs *csm_angle;     /**<< associated control system (angle) */
        struct cs *csm_distance;  /**<< associated control system (distance) */
        struct robot_system *robot;      /**<< associated robot_system */
        struct cs *csm_angle;     /**<< associated control system (angle) */
        struct cs *csm_distance;  /**<< associated control system (distance) */

-  
+

        int8_t scheduler_task;    /**<< id of current task (-1 if no running task) */
 };
 
        int8_t scheduler_task;    /**<< id of current task (-1 if no running task) */
 };
 


@@ -108,6 +109,12 @@ void trajectory_set_speed(struct trajectory *traj, int16_t d_speed, int16_t a_sp
 void trajectory_set_windows(struct trajectory *traj, double d_win, 
                            double a_win_deg, double a_start_deg);
 
 void trajectory_set_windows(struct trajectory *traj, double d_win, 
                            double a_win_deg, double a_start_deg);
 

+/**
+ * Set coef for circle trajectory. The objective of this value is to
+ * fix the radius of the circle which is not correctly what we asked.
+ */
+void trajectory_set_circle_coef(struct trajectory *traj, double coef);
+

 /** return true if the position consign is equal to the filtered
  * position consign (after quadramp filter), for angle and
  * distance. */
 /** return true if the position consign is equal to the filtered
  * position consign (after quadramp filter), for angle and
  * distance. */

diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c
index cc2a58a..85b2e57 100644 (file)
--- a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c
+++ b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c
@@ -485,12 +485,32 @@ void trajectory_manager_circle_event(struct trajectory *traj)
        double a = position_get_a_rad_double(traj->position);
        int32_t d_consign = 0, a_consign = 0;
        double angle_to_center_rad;
        double a = position_get_a_rad_double(traj->position);
        int32_t d_consign = 0, a_consign = 0;
        double angle_to_center_rad;

+       static int32_t d_prev, a_prev;
+       int32_t d_speed, a_speed;
+       int32_t d_pos, a_pos;
+
+       d_pos = rs_get_distance(traj->robot);
+       a_pos = rs_get_angle(traj->robot);
+       d_speed = d_pos - d_prev;
+       a_speed = a_pos - a_prev;
+       d_prev = d_pos;
+       a_prev = a_pos;

 
        /* These vectors contain target position of the robot in
         * its own coordinates */
        vect2_cart v2cart_pos;
        vect2_pol v2pol_target;
 
 
        /* These vectors contain target position of the robot in
         * its own coordinates */
        vect2_cart v2cart_pos;
        vect2_pol v2pol_target;
 

+       int32_t delta_d, delta_a;
+       double coef_deriv = traj->circle_coef;
+       double new_radius;
+       struct  quadramp_filter * q_d, * q_a;
+
+       q_d = traj->csm_distance->consign_filter_params;
+       q_a = traj->csm_angle->consign_filter_params;
+       delta_a = a_speed;//q_a->previous_var;
+       delta_d = d_speed;//q_d->previous_var;
+

        /* step 1 : process new commands to quadramps */
 
        /* process the command vector from current position to the
        /* step 1 : process new commands to quadramps */
 
        /* process the command vector from current position to the


@@ -500,17 +520,31 @@ void trajectory_manager_circle_event(struct trajectory *traj)
        vect2_cart2pol(&v2cart_pos, &v2pol_target);
        v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta - a);
 
        vect2_cart2pol(&v2cart_pos, &v2pol_target);
        v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta - a);
 

+       /* pas trop mal, mais oscille */
+       //new_radius = traj->target.circle.radius - delta_a * delta_d * coef_deriv;
+       if (v2pol_target.r > traj->target.circle.radius/2)
+               new_radius = traj->target.circle.radius - delta_a * delta_d * coef_deriv * traj->target.circle.radius / v2pol_target.r;
+       else
+               new_radius = traj->target.circle.radius - delta_a * delta_d * coef_deriv ;
+
+       /* oscille a mort */
+       //new_radius = traj->target.circle.radius - traj->target.circle.radius   * delta_a * delta_a * coef_deriv;
+
+       /* ? */
+       //new_radius = traj->target.circle.radius - traj->target.circle.radius   * delta_a * coef_deriv;
+
+

        /* wanted direction of center of circle:
         * if we are far, go in the center direction,
         * if we are at radius, we want to see the center at 90°
         * if we are nearer than radius, angle to center is > 90° */
        /* wanted direction of center of circle:
         * if we are far, go in the center direction,
         * if we are at radius, we want to see the center at 90°
         * if we are nearer than radius, angle to center is > 90° */

-       if (v2pol_target.r > traj->target.circle.radius) {
-               angle_to_center_rad = traj->target.circle.radius / v2pol_target.r;
+       if (v2pol_target.r > new_radius) {
+               angle_to_center_rad = new_radius / v2pol_target.r;

                angle_to_center_rad *= (M_PI / 2);
        }
        else {
                angle_to_center_rad = 1. - (v2pol_target.r /
                angle_to_center_rad *= (M_PI / 2);
        }
        else {
                angle_to_center_rad = 1. - (v2pol_target.r /

-                                           (2 * traj->target.circle.radius));
+                                           (2 * new_radius));

                angle_to_center_rad *= M_PI;
        }
 
                angle_to_center_rad *= M_PI;
        }
 


@@ -548,9 +582,10 @@ void trajectory_manager_circle_event(struct trajectory *traj)
 
        EVT_DEBUG(E_TRAJECTORY,"EVENT CIRCLE d_cur=%" PRIi32 ", d_consign=%" PRIi32
                  ", d_speed=%" PRIi32 ", a_cur=%" PRIi32 ", a_consign=%" PRIi32
 
        EVT_DEBUG(E_TRAJECTORY,"EVENT CIRCLE d_cur=%" PRIi32 ", d_consign=%" PRIi32
                  ", d_speed=%" PRIi32 ", a_cur=%" PRIi32 ", a_consign=%" PRIi32

-                 ", a_speed=%" PRIi32,
-             rs_get_distance(traj->robot), d_consign, get_quadramp_distance_speed(traj),
-             rs_get_angle(traj->robot), a_consign, get_quadramp_angle_speed(traj));
+                 ", a_speed=%" PRIi32 "radius = %f",
+                 rs_get_distance(traj->robot), d_consign, get_quadramp_distance_speed(traj),
+                 rs_get_angle(traj->robot), a_consign, get_quadramp_angle_speed(traj),
+                 new_radius);

 
        cs_set_consign(traj->csm_angle, a_consign);
        cs_set_consign(traj->csm_distance, d_consign);
 
        cs_set_consign(traj->csm_angle, a_consign);
        cs_set_consign(traj->csm_distance, d_consign);

diff --git a/projects/microb2009/mainboard/commands.c b/projects/microb2009/mainboard/commands.c
index 61fa530..bbcaacf 100644 (file)
--- a/projects/microb2009/mainboard/commands.c
+++ b/projects/microb2009/mainboard/commands.c
@@ -99,6 +99,8 @@ extern parse_pgm_inst_t cmd_traj_speed;
 extern parse_pgm_inst_t cmd_traj_speed_show;
 extern parse_pgm_inst_t cmd_trajectory;
 extern parse_pgm_inst_t cmd_trajectory_show;
 extern parse_pgm_inst_t cmd_traj_speed_show;
 extern parse_pgm_inst_t cmd_trajectory;
 extern parse_pgm_inst_t cmd_trajectory_show;

+extern parse_pgm_inst_t cmd_circle_coef;
+extern parse_pgm_inst_t cmd_circle_coef_show;

 extern parse_pgm_inst_t cmd_rs_gains;
 extern parse_pgm_inst_t cmd_rs_gains_show;
 extern parse_pgm_inst_t cmd_track;
 extern parse_pgm_inst_t cmd_rs_gains;
 extern parse_pgm_inst_t cmd_rs_gains_show;
 extern parse_pgm_inst_t cmd_track;


@@ -198,6 +200,8 @@ parse_pgm_ctx_t main_ctx[] = {
        (parse_pgm_inst_t *)&cmd_traj_speed_show,
        (parse_pgm_inst_t *)&cmd_trajectory,
        (parse_pgm_inst_t *)&cmd_trajectory_show,
        (parse_pgm_inst_t *)&cmd_traj_speed_show,
        (parse_pgm_inst_t *)&cmd_trajectory,
        (parse_pgm_inst_t *)&cmd_trajectory_show,

+       (parse_pgm_inst_t *)&cmd_circle_coef,
+       (parse_pgm_inst_t *)&cmd_circle_coef_show,

        (parse_pgm_inst_t *)&cmd_rs_gains,
        (parse_pgm_inst_t *)&cmd_rs_gains_show,
        (parse_pgm_inst_t *)&cmd_track,
        (parse_pgm_inst_t *)&cmd_rs_gains,
        (parse_pgm_inst_t *)&cmd_rs_gains_show,
        (parse_pgm_inst_t *)&cmd_track,

diff --git a/projects/microb2009/mainboard/commands_traj.c b/projects/microb2009/mainboard/commands_traj.c
index 82477a3..170dce8 100644 (file)
--- a/projects/microb2009/mainboard/commands_traj.c
+++ b/projects/microb2009/mainboard/commands_traj.c
@@ -125,6 +125,65 @@ parse_pgm_inst_t cmd_traj_speed_show = {
        },
 };
 
        },
 };
 

+/**********************************************************/
+/* circle coef configuration */
+
+/* this structure is filled when cmd_circle_coef is parsed successfully */
+struct cmd_circle_coef_result {
+       fixed_string_t arg0;
+       fixed_string_t arg1;
+       float circle_coef;
+};
+
+
+/* function called when cmd_circle_coef is parsed successfully */
+static void cmd_circle_coef_parsed(void *parsed_result, void *data)
+{
+       struct cmd_circle_coef_result *res = parsed_result;
+
+       if (!strcmp_P(res->arg1, PSTR("set"))) {
+               trajectory_set_circle_coef(&mainboard.traj, res->circle_coef);
+       }
+
+       printf_P(PSTR("circle_coef set %2.2f\r\n"), mainboard.traj.circle_coef);
+}
+
+prog_char str_circle_coef_arg0[] = "circle_coef";
+parse_pgm_token_string_t cmd_circle_coef_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg0, str_circle_coef_arg0);
+prog_char str_circle_coef_arg1[] = "set";
+parse_pgm_token_string_t cmd_circle_coef_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg1, str_circle_coef_arg1);
+parse_pgm_token_num_t cmd_circle_coef_val = TOKEN_NUM_INITIALIZER(struct cmd_circle_coef_result, circle_coef, FLOAT);
+
+prog_char help_circle_coef[] = "Set circle coef";
+parse_pgm_inst_t cmd_circle_coef = {
+       .f = cmd_circle_coef_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_circle_coef,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_circle_coef_arg0,
+               (prog_void *)&cmd_circle_coef_arg1,
+               (prog_void *)&cmd_circle_coef_val,
+               NULL,
+       },
+};
+
+/* show */
+
+prog_char str_circle_coef_show_arg[] = "show";
+parse_pgm_token_string_t cmd_circle_coef_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg1, str_circle_coef_show_arg);
+
+prog_char help_circle_coef_show[] = "Show circle coef";
+parse_pgm_inst_t cmd_circle_coef_show = {
+       .f = cmd_circle_coef_parsed,  /* function to call */
+       .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_show_arg,
+               NULL,
+       },
+};
+

 /**********************************************************/
 /* trajectory window configuration */
 
 /**********************************************************/
 /* trajectory window configuration */
 

diff --git a/projects/microb2009/microb_cmd/microbcmd.py b/projects/microb2009/microb_cmd/microbcmd.py
index 5080927..ee0dbaa 100755 (executable)
--- a/projects/microb2009/microb_cmd/microbcmd.py
+++ b/projects/microb2009/microb_cmd/microbcmd.py
@@ -12,6 +12,7 @@ import numpy
 import shlex
 import time
 import math
 import shlex
 import time
 import math

+import re

 import warnings
 warnings.filterwarnings("ignore","tempnam",RuntimeWarning, __name__)
 
 import warnings
 warnings.filterwarnings("ignore","tempnam",RuntimeWarning, __name__)
 


@@ -899,6 +900,39 @@ class Interp(cmd.Cmd):
             time.sleep(1)
             self.ser.write("pwm s3(3C) 250\n")
 
             time.sleep(1)
             self.ser.write("pwm s3(3C) 250\n")
 

+    def do_circle(self, arg):
+        arg = re.sub("  *", " ", arg)
+        arg = arg.strip()
+        arg = arg.split(" ")
+        if len(arg)!=4:
+            print "radius coef dspeed aspeed"
+            return
+        r = int(arg[0])
+        c = float(arg[1])
+        dspeed = int(arg[2])
+        aspeed = int(arg[3])
+
+        self.ser.write("event cs on\n")
+        time.sleep(0.3)
+
+        self.ser.write("traj_speed distance %d\n"%dspeed)
+        time.sleep(0.3)
+        self.ser.write("traj_speed angle %d\n"%aspeed)
+        time.sleep(0.3)
+
+        self.ser.write("goto d_rel 300\n")
+        time.sleep(3)
+
+        self.ser.write("position reset\n")
+        time.sleep(0.3)
+        self.ser.write("circle_coef set %f\n"%c)
+        time.sleep(0.3)
+
+        self.ser.write("goto circle_rel 700 460 %d 360\n"%r)
+        self.ser.flushInput()
+        time.sleep(0.3)
+
+

 if __name__ == "__main__":
     try:
         import readline,atexit
 if __name__ == "__main__":
     try:
         import readline,atexit

diff --git a/projects/microb2010/mainboard/commands.c b/projects/microb2010/mainboard/commands.c
index 107606e..26060d6 100644 (file)
--- a/projects/microb2010/mainboard/commands.c
+++ b/projects/microb2010/mainboard/commands.c
@@ -99,6 +99,8 @@ extern parse_pgm_inst_t cmd_traj_speed;
 extern parse_pgm_inst_t cmd_traj_speed_show;
 extern parse_pgm_inst_t cmd_trajectory;
 extern parse_pgm_inst_t cmd_trajectory_show;
 extern parse_pgm_inst_t cmd_traj_speed_show;
 extern parse_pgm_inst_t cmd_trajectory;
 extern parse_pgm_inst_t cmd_trajectory_show;

+extern parse_pgm_inst_t cmd_circle_coef;
+extern parse_pgm_inst_t cmd_circle_coef_show;

 extern parse_pgm_inst_t cmd_rs_gains;
 extern parse_pgm_inst_t cmd_rs_gains_show;
 extern parse_pgm_inst_t cmd_track;
 extern parse_pgm_inst_t cmd_rs_gains;
 extern parse_pgm_inst_t cmd_rs_gains_show;
 extern parse_pgm_inst_t cmd_track;


@@ -197,6 +199,8 @@ parse_pgm_ctx_t main_ctx[] = {
        (parse_pgm_inst_t *)&cmd_traj_speed_show,
        (parse_pgm_inst_t *)&cmd_trajectory,
        (parse_pgm_inst_t *)&cmd_trajectory_show,
        (parse_pgm_inst_t *)&cmd_traj_speed_show,
        (parse_pgm_inst_t *)&cmd_trajectory,
        (parse_pgm_inst_t *)&cmd_trajectory_show,

+       (parse_pgm_inst_t *)&cmd_circle_coef,
+       (parse_pgm_inst_t *)&cmd_circle_coef_show,

        (parse_pgm_inst_t *)&cmd_rs_gains,
        (parse_pgm_inst_t *)&cmd_rs_gains_show,
        (parse_pgm_inst_t *)&cmd_track,
        (parse_pgm_inst_t *)&cmd_rs_gains,
        (parse_pgm_inst_t *)&cmd_rs_gains_show,
        (parse_pgm_inst_t *)&cmd_track,

diff --git a/projects/microb2010/mainboard/commands_traj.c b/projects/microb2010/mainboard/commands_traj.c
index f3b38fd..ea124d3 100644 (file)
--- a/projects/microb2010/mainboard/commands_traj.c
+++ b/projects/microb2010/mainboard/commands_traj.c
@@ -125,6 +125,65 @@ parse_pgm_inst_t cmd_traj_speed_show = {
        },
 };
 
        },
 };
 

+/**********************************************************/
+/* circle coef configuration */
+
+/* this structure is filled when cmd_circle_coef is parsed successfully */
+struct cmd_circle_coef_result {
+       fixed_string_t arg0;
+       fixed_string_t arg1;
+       float circle_coef;
+};
+
+
+/* function called when cmd_circle_coef is parsed successfully */
+static void cmd_circle_coef_parsed(void *parsed_result, void *data)
+{
+       struct cmd_circle_coef_result *res = parsed_result;
+
+       if (!strcmp_P(res->arg1, PSTR("set"))) {
+               trajectory_set_circle_coef(&mainboard.traj, res->circle_coef);
+       }
+
+       printf_P(PSTR("circle_coef %2.2f\r\n"), mainboard.traj.circle_coef);
+}
+
+prog_char str_circle_coef_arg0[] = "circle_coef";
+parse_pgm_token_string_t cmd_circle_coef_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg0, str_circle_coef_arg0);
+prog_char str_circle_coef_arg1[] = "set";
+parse_pgm_token_string_t cmd_circle_coef_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg1, str_circle_coef_arg1);
+parse_pgm_token_num_t cmd_circle_coef_val = TOKEN_NUM_INITIALIZER(struct cmd_circle_coef_result, circle_coef, FLOAT);
+
+prog_char help_circle_coef[] = "Set circle coef";
+parse_pgm_inst_t cmd_circle_coef = {
+       .f = cmd_circle_coef_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_circle_coef,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_circle_coef_arg0,
+               (prog_void *)&cmd_circle_coef_arg1,
+               (prog_void *)&cmd_circle_coef_val,
+               NULL,
+       },
+};
+
+/* show */
+
+prog_char str_circle_coef_show_arg[] = "show";
+parse_pgm_token_string_t cmd_circle_coef_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg1, str_circle_coef_show_arg);
+
+prog_char help_circle_coef_show[] = "Show circle coef";
+parse_pgm_inst_t cmd_circle_coef_show = {
+       .f = cmd_circle_coef_parsed,  /* function to call */
+       .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_show_arg,
+               NULL,
+       },
+};
+

 /**********************************************************/
 /* trajectory window configuration */
 
 /**********************************************************/
 /* trajectory window configuration */