traj circle
authorzer0 <zer0@carbon.local>
Sat, 9 Jan 2010 20:53:45 +0000 (21:53 +0100)
committerzer0 <zer0@carbon.local>
Sat, 9 Jan 2010 20:53:45 +0000 (21:53 +0100)
modules/base/math/fixed_point/test/.config
modules/devices/robot/trajectory_manager/trajectory_manager.h
modules/devices/robot/trajectory_manager/trajectory_manager_core.c
projects/microb2009/mainboard/commands.c
projects/microb2009/mainboard/commands_traj.c

index 296da2c..4bcbaf7 100644 (file)
 # CONFIG_MCU_ATMEGA645 is not set
 # CONFIG_MCU_ATMEGA6450 is not set
 CONFIG_MCU_ATMEGA128=y
+# CONFIG_MCU_ATMEGA1281 is not set
 # CONFIG_MCU_AT90CAN128 is not set
 # CONFIG_MCU_AT94K is not set
 # CONFIG_MCU_AT90S1200 is not set
+# CONFIG_MCU_ATMEGA2560 is not set
+# CONFIG_MCU_ATMEGA256 is not set
 CONFIG_QUARTZ=16000000
 
 #
@@ -60,6 +63,7 @@ CONFIG_QUARTZ=16000000
 CONFIG_OPTM_S=y
 CONFIG_MATH_LIB=y
 # CONFIG_FDEVOPEN_COMPAT is not set
+# CONFIG_NO_PRINTF is not set
 # CONFIG_MINIMAL_PRINTF is not set
 # CONFIG_STANDARD_PRINTF is not set
 CONFIG_ADVANCED_PRINTF=y
@@ -78,13 +82,17 @@ CONFIG_MODULE_CIRBUF=y
 # CONFIG_MODULE_CIRBUF_LARGE is not set
 CONFIG_MODULE_FIXED_POINT=y
 # CONFIG_MODULE_VECT2 is not set
+# CONFIG_MODULE_GEOMETRY is not set
 # CONFIG_MODULE_SCHEDULER is not set
+# CONFIG_MODULE_SCHEDULER_STATS is not set
 # CONFIG_MODULE_SCHEDULER_CREATE_CONFIG is not set
 # CONFIG_MODULE_SCHEDULER_USE_TIMERS is not set
 CONFIG_MODULE_SCHEDULER_TIMER0=y
 # CONFIG_MODULE_SCHEDULER_MANUAL is not set
 # CONFIG_MODULE_TIME is not set
 # CONFIG_MODULE_TIME_CREATE_CONFIG is not set
+# CONFIG_MODULE_TIME_EXT is not set
+# CONFIG_MODULE_TIME_EXT_CREATE_CONFIG is not set
 
 #
 # Communication modules
@@ -94,7 +102,10 @@ CONFIG_MODULE_SCHEDULER_TIMER0=y
 # uart needs circular buffer, mf2 client may need scheduler
 #
 CONFIG_MODULE_UART=y
+# CONFIG_MODULE_UART_9BITS is not set
 CONFIG_MODULE_UART_CREATE_CONFIG=y
+# CONFIG_MODULE_SPI is not set
+# CONFIG_MODULE_SPI_CREATE_CONFIG is not set
 # CONFIG_MODULE_I2C is not set
 # CONFIG_MODULE_I2C_MASTER is not set
 # CONFIG_MODULE_I2C_MULTIMASTER is not set
@@ -113,6 +124,7 @@ CONFIG_MODULE_UART_CREATE_CONFIG=y
 # CONFIG_MODULE_TIMER_DYNAMIC is not set
 # CONFIG_MODULE_PWM is not set
 # CONFIG_MODULE_PWM_CREATE_CONFIG is not set
+# CONFIG_MODULE_PWM_NG is not set
 # CONFIG_MODULE_ADC is not set
 # CONFIG_MODULE_ADC_CREATE_CONFIG is not set
 
@@ -126,6 +138,7 @@ CONFIG_MODULE_UART_CREATE_CONFIG=y
 # CONFIG_MODULE_RDLINE_KILL_BUF is not set
 # CONFIG_MODULE_RDLINE_HISTORY is not set
 # CONFIG_MODULE_PARSE is not set
+# CONFIG_MODULE_PARSE_NO_FLOAT is not set
 
 #
 # External devices modules
@@ -134,6 +147,8 @@ CONFIG_MODULE_UART_CREATE_CONFIG=y
 # CONFIG_MODULE_LCD_CREATE_CONFIG is not set
 # CONFIG_MODULE_MULTISERVO is not set
 # CONFIG_MODULE_MULTISERVO_CREATE_CONFIG is not set
+# CONFIG_MODULE_AX12 is not set
+# CONFIG_MODULE_AX12_CREATE_CONFIG is not set
 
 #
 # Brushless motor drivers (you should enable pwm modules to see all)
@@ -144,22 +159,27 @@ CONFIG_MODULE_UART_CREATE_CONFIG=y
 # CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE_CREATE_CONFIG is not set
 
 #
-# Encoders
+# Encoders (you need comm/spi for encoders_spi)
 #
 # CONFIG_MODULE_ENCODERS_MICROB is not set
 # CONFIG_MODULE_ENCODERS_MICROB_CREATE_CONFIG is not set
 # CONFIG_MODULE_ENCODERS_EIRBOT is not set
 # CONFIG_MODULE_ENCODERS_EIRBOT_CREATE_CONFIG is not set
+# CONFIG_MODULE_ENCODERS_SPI is not set
+# CONFIG_MODULE_ENCODERS_SPI_CREATE_CONFIG is not set
 
 #
-# Robot specific modules
+# Robot specific modules (fixed point lib may be needed)
 #
 # CONFIG_MODULE_ROBOT_SYSTEM is not set
+# CONFIG_MODULE_ROBOT_SYSTEM_USE_F64 is not set
 # CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT is not set
 # CONFIG_MODULE_POSITION_MANAGER is not set
+# CONFIG_MODULE_COMPENSATE_CENTRIFUGAL_FORCE is not set
 # CONFIG_MODULE_TRAJECTORY_MANAGER is not set
 # CONFIG_MODULE_BLOCKING_DETECTION_MANAGER is not set
 # CONFIG_MODULE_OBSTACLE_AVOIDANCE is not set
+# CONFIG_MODULE_OBSTACLE_AVOIDANCE_CREATE_CONFIG is not set
 
 #
 # Control system modules
@@ -177,6 +197,16 @@ CONFIG_MODULE_UART_CREATE_CONFIG=y
 # CONFIG_MODULE_BIQUAD is not set
 
 #
+# Radio devices
+#
+
+#
+# Some radio devices require SPI to be activated
+#
+# CONFIG_MODULE_CC2420 is not set
+# CONFIG_MODULE_CC2420_CREATE_CONFIG is not set
+
+#
 # Crypto modules
 #
 
@@ -237,7 +267,9 @@ CONFIG_AVRDUDE_PROG_STK200=y
 # CONFIG_AVRDUDE_PROG_BSD is not set
 # CONFIG_AVRDUDE_PROG_DAPA is not set
 # CONFIG_AVRDUDE_PROG_JTAG1 is not set
+# CONFIG_AVRDUDE_PROG_AVR109 is not set
 CONFIG_AVRDUDE_PORT="/dev/parport0"
+CONFIG_AVRDUDE_BAUDRATE=19200
 
 #
 # Avarice
@@ -246,3 +278,4 @@ CONFIG_AVARICE_PORT="/dev/ttyS0"
 CONFIG_AVARICE_DEBUG_PORT=1234
 CONFIG_AVARICE_PROG_MKI=y
 # CONFIG_AVARICE_PROG_MKII is not set
+# CONFIG_AVRDUDE_CHECK_SIGNATURE is not set
index 648ec1c..3ebd586 100644 (file)
@@ -47,6 +47,15 @@ enum trajectory_state {
        RUNNING_CIRCLE,
 };
 
+struct circle_target {
+       vect2_cart center;   /**< center of the circle */
+       double radius;       /**< radius of the circle */
+       int32_t dest_angle;  /**< dst angle in inc */
+
+#define TRIGO   1 /* rotation is counterclockwise */
+#define FORWARD 2 /* go forward or backward */
+       uint8_t flags;   /**< flags for this trajectory */
+};
 
 struct trajectory {
        enum trajectory_state state; /*<< describe the type of target, and if we reached the target */
@@ -54,6 +63,7 @@ struct trajectory {
        union {
                vect2_cart cart;     /**<< target, if it is a x,y vector */
                struct rs_polar pol; /**<< target, if it is a d,a vector */
+               struct circle_target circle; /**<< target, if it is a circle */
        } target;
 
        double d_win;      /**<< distance window (for END_NEAR) */
@@ -160,4 +170,10 @@ void trajectory_goto_d_a_rel(struct trajectory *traj, double d, double a);
 /** go forward to a x,y relative point, using a trajectory event */
 void trajectory_goto_xy_rel(struct trajectory *traj, double x_rel_mm, double y_rel_mm);
 
+/** make the robot orbiting around (x,y) on a circle whose radius is
+ * radius_mm, and exit when relative destination angle is reached. The
+ * flags set if we go forward or backwards, and CW/CCW. */
+void trajectory_circle_rel(struct trajectory *traj, double x, double y,
+                          double radius_mm, double rel_a_deg, uint8_t flags);
+
 #endif //TRAJECTORY_MANAGER
index 7adde48..a7473c4 100644 (file)
 #define RESET_D  4
 #define RESET_A  8
 
+static uint8_t evt_debug_cpt = 0;
+#define EVT_DEBUG(args...) do {                                \
+               if (((evt_debug_cpt ++) & 0x07) == 0) { \
+                       DEBUG(args);                    \
+               }                                       \
+       } while (0)
+
+
 /**
  * update angle and/or distance
  * this function is not called directly by the user
@@ -355,7 +363,7 @@ void trajectory_manager_xy_event(struct trajectory *traj)
        case RUNNING_XY_B_ANGLE_OK:
 
                /* process the command vector from current position to
-                * absolute target, or to the center of the circle. */
+                * absolute target. */
                v2cart_pos.x = traj->target.cart.x - x;
                v2cart_pos.y = traj->target.cart.y - y;
                vect2_cart2pol(&v2cart_pos, &v2pol_target);
@@ -433,7 +441,7 @@ void trajectory_manager_xy_event(struct trajectory *traj)
                q_a = traj->csm_angle->consign_filter_params;
                /* if d_speed is not 0, we are in start_angle_win */
                if (get_quadramp_distance_speed(traj)) {
-                       if(is_robot_in_xy_window(traj, traj->d_win)) {
+                       if (is_robot_in_xy_window(traj, traj->d_win)) {
                                delete_event(traj);
                        }
                        /* ANGLE -> ANGLE_OK */
@@ -447,7 +455,7 @@ void trajectory_manager_xy_event(struct trajectory *traj)
        case RUNNING_XY_F_ANGLE_OK:
        case RUNNING_XY_B_ANGLE_OK:
                /* If we reached the destination */
-               if(is_robot_in_xy_window(traj, traj->d_win)) {
+               if (is_robot_in_xy_window(traj, traj->d_win)) {
                        delete_event(traj);
                }
        break;
@@ -458,10 +466,7 @@ void trajectory_manager_xy_event(struct trajectory *traj)
 
        /* step 3 : send the processed commands to cs */
 
-       DEBUG(E_TRAJECTORY, "EVENT XY cur=(%f,%f,%f) cart=(%f,%f) pol=(%f,%f)",
-             x, y, a, v2cart_pos.x, v2cart_pos.y, v2pol_target.r, v2pol_target.theta);
-
-       DEBUG(E_TRAJECTORY,"d_cur=%" PRIi32 ", d_consign=%" PRIi32 ", d_speed=%" PRIi32 ", "
+       EVT_DEBUG(E_TRAJECTORY,"EVENT XY 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));
@@ -471,14 +476,82 @@ void trajectory_manager_xy_event(struct trajectory *traj)
 }
 
 /* trajectory event for circles */
+/* XXX static */
 void trajectory_manager_circle_event(struct trajectory *traj)
 {
-#if 0
+       double coef = 1.0;
        double x = position_get_x_double(traj->position);
        double y = position_get_y_double(traj->position);
        double a = position_get_a_rad_double(traj->position);
-       int32_t d_consign=0, a_consign=0;
-#endif
+       int32_t d_consign = 0, a_consign = 0;
+       double angle_to_center_rad;
+
+       /* These vectors contain target position of the robot in
+        * its own coordinates */
+       vect2_cart v2cart_pos;
+       vect2_pol v2pol_target;
+
+       /* step 1 : process new commands to quadramps */
+
+       /* process the command vector from current position to the
+        * center of the circle. */
+       v2cart_pos.x = traj->target.circle.center.x - x;
+       v2cart_pos.y = traj->target.circle.center.y - y;
+       vect2_cart2pol(&v2cart_pos, &v2pol_target);
+       v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta - a);
+
+       /* 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 * 2)
+               angle_to_center_rad = 0;
+       else {
+               angle_to_center_rad = 1. - (v2pol_target.r /
+                                           (2 * traj->target.circle.radius));
+               angle_to_center_rad *= M_PI;
+       }
+
+       /* XXX check flags */
+       v2pol_target.theta -= angle_to_center_rad;
+
+       /* If the robot is correctly oriented to start moving in distance */
+       /* here limit dist speed depending on v2pol_target.theta */
+       if (ABS(v2pol_target.theta) > traj->a_start_rad)
+               set_quadramp_speed(traj, 0, traj->a_speed);
+       else {
+               coef = (traj->a_start_rad - ABS(v2pol_target.theta)) / traj->a_start_rad;
+               set_quadramp_speed(traj, traj->d_speed * coef, traj->a_speed);
+       }
+
+       /* XXX check flags */
+       d_consign = 40000 + 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.0);
+       a_consign += rs_get_angle(traj->robot);
+
+       /* step 2 : update state, or delete event if we reached the
+        * destination */
+
+       /* output angle -> delete event */
+       if (a_consign >= traj->target.circle.dest_angle) {
+               a_consign = traj->target.circle.dest_angle;
+               delete_event(traj);
+       }
+
+       /* step 3 : send the processed commands to cs */
+
+       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));
+
+       cs_set_consign(traj->csm_angle, a_consign);
+       cs_set_consign(traj->csm_distance, d_consign);
 }
 
 /* trajectory event */
@@ -510,6 +583,40 @@ void trajectory_manager_event(void * param)
 
 /*********** *CIRCLE */
 
+/* make the robot orbiting around (x,y) on a circle whose radius is
+ * radius_mm, and exit when relative destination angle is reached. The
+ * flags set if we go forward or backwards, and CW/CCW. */
+void trajectory_circle_rel(struct trajectory *traj,
+                          double x, double y,
+                          double radius_mm,
+                          double rel_a_deg,
+                          uint8_t flags)
+{
+       double dst_angle;
+
+       delete_event(traj);
+
+       traj->target.circle.center.x = x;
+       traj->target.circle.center.y = y;
+       traj->target.circle.radius = radius_mm;
+       traj->target.circle.flags = flags;
+
+       /* convert in steps  */
+       dst_angle = RAD(rel_a_deg) *
+               (traj->position->phys.distance_imp_per_mm) *
+               (traj->position->phys.track_mm) / 2.0;
+
+       traj->target.circle.dest_angle = rs_get_angle(traj->robot);
+       traj->target.circle.dest_angle += dst_angle;
+
+       DEBUG(E_TRAJECTORY, "Circle rel (x,y)=%2.2f,%2.2f r=%2.2f flags=%x dst_angle=%"PRIi32"",
+             x, y, radius_mm, flags, traj->target.circle.dest_angle);
+
+       traj->state = RUNNING_CIRCLE;
+       trajectory_manager_event(traj);
+       schedule_event(traj);
+}
+
 /*
  * Compute the fastest distance and angle speeds matching the radius
  * from current traj_speed
index 107606e..61fa530 100644 (file)
@@ -110,6 +110,7 @@ extern parse_pgm_inst_t cmd_pt_list_show;
 extern parse_pgm_inst_t cmd_goto1;
 extern parse_pgm_inst_t cmd_goto2;
 extern parse_pgm_inst_t cmd_goto3;
+extern parse_pgm_inst_t cmd_goto4;
 extern parse_pgm_inst_t cmd_position;
 extern parse_pgm_inst_t cmd_position_set;
 extern parse_pgm_inst_t cmd_strat_infos;
@@ -207,6 +208,7 @@ parse_pgm_ctx_t main_ctx[] = {
        (parse_pgm_inst_t *)&cmd_pt_list_show,
        (parse_pgm_inst_t *)&cmd_goto1,
        (parse_pgm_inst_t *)&cmd_goto2,
+       (parse_pgm_inst_t *)&cmd_goto4,
        (parse_pgm_inst_t *)&cmd_position,
        (parse_pgm_inst_t *)&cmd_position_set,
        (parse_pgm_inst_t *)&cmd_strat_infos,
index 715f167..82477a3 100644 (file)
@@ -489,6 +489,7 @@ struct cmd_goto_result {
        int32_t arg2;
        int32_t arg3;
        int32_t arg4;
+       int32_t arg5;
 };
 
 /* function called when cmd_goto is parsed successfully */
@@ -539,6 +540,11 @@ static void cmd_goto_parsed(void * parsed_result, void * data)
        else if (!strcmp_P(res->arg1, PSTR("da_rel"))) {
                trajectory_d_a_rel(&mainboard.traj, res->arg2, res->arg3);
        }
+       else if (!strcmp_P(res->arg1, PSTR("circle_rel"))) {
+               trajectory_circle_rel(&mainboard.traj, res->arg2, res->arg3,
+                                     res->arg4, res->arg5, 0);
+               return; /* XXX */
+       }
        t1 = time_get_us2();
        while ((err = test_traj_end(0xFF)) == 0) {
                t2 = time_get_us2();
@@ -592,6 +598,28 @@ parse_pgm_inst_t cmd_goto2 = {
        },
 };
 
+prog_char str_goto_arg1_c[] = "circle_rel";
+parse_pgm_token_string_t cmd_goto_arg1_c = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_c);
+parse_pgm_token_num_t cmd_goto_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg4, INT32);
+parse_pgm_token_num_t cmd_goto_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg5, INT32);
+
+/* 4 params */
+prog_char help_goto4[] = "Do a circle: (x,y, radius, angle)";
+parse_pgm_inst_t cmd_goto4 = {
+       .f = cmd_goto_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_goto4,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_goto_arg0,
+               (prog_void *)&cmd_goto_arg1_c,
+               (prog_void *)&cmd_goto_arg2,
+               (prog_void *)&cmd_goto_arg3,
+               (prog_void *)&cmd_goto_arg4,
+               (prog_void *)&cmd_goto_arg5,
+               NULL,
+       },
+};
+
 /**********************************************************/
 /* Position tests */