[Krobot-commits] [SCM] krobot branch, master, updated. aa100442b8a79fa3dc718c80e03594675cf67ab1

[Xavier L. <Ba...@us...>]

This is an automated email from the git hooks/post-receive script. It was
generated because a ref change was pushed to the repository containing
the project "krobot".

The branch, master has been updated
       via  aa100442b8a79fa3dc718c80e03594675cf67ab1 (commit)
      from  626a08aeb7e638f96d01ae519611fd42d3f4506b (commit)

Those revisions listed above that are new to this repository have
not appeared on any other notification email; so we list those
revisions in full, below.

- Log -----------------------------------------------------------------
commit aa100442b8a79fa3dc718c80e03594675cf67ab1
Author: Xavier Lagorce <Xav...@cr...>
Date:   Wed Mar 27 23:33:27 2013 +0100

    [Controller_motor_STM32] Wheels can have different radii (and new structural values for the robot)

-----------------------------------------------------------------------

Changes:
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/main.c b/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/main.c
index e4b87bf..66e9f8c 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/main.c
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/main.c
@@ -17,8 +17,11 @@
 #include "command_generator.h"
 #include "differential_drive.h"
 
-#define PROP_WHEEL_RADIUS 0.049245
-#define PROP_SHAFT_WIDTH 0.22264
+//#define PROP_WHEEL_RADIUS 0.049245
+#define PROP_WHEEL_RADIUS_LEFT 0.049207
+#define PROP_WHEEL_RADIUS_RIGHT 0.049283
+#define PROP_WHEEL_RADIUS ((PROP_WHEEL_RADIUS_RIGHT+PROP_WHEEL_RADIUS_LEFT)/2)
+#define PROP_SHAFT_WIDTH 0.22587
 
 #define INDEP_WHEEL_RADIUS 0.0359
 #define INDEP_SHAFT_WIDTH 0.266
@@ -53,7 +56,7 @@ static void init(void)
         // Start control of drive motors
         tc_init();
         dd_start(CONTROL_ODOMETRY, // Use odometry CONTROL_ODOMETRY for control
-                 PROP_WHEEL_RADIUS, PROP_SHAFT_WIDTH, // Structural parameters
+                 PROP_WHEEL_RADIUS_LEFT, PROP_WHEEL_RADIUS_RIGHT, PROP_SHAFT_WIDTH, // Structural parameters
                  8*2*M_PI, // Absolute wheel speed limitation
                  0.5, // Linear velocity limitation
                  1.0, // Linear acceleration limitation
@@ -82,11 +85,13 @@ static void init(void)
 
         // Start odometrys
         odometryInit(0, 1e-3,
-                     PROP_WHEEL_RADIUS, PROP_SHAFT_WIDTH,
+                     PROP_WHEEL_RADIUS_LEFT, PROP_WHEEL_RADIUS_RIGHT,
+                     PROP_SHAFT_WIDTH,
                      ENCODER3, ENCODER4,
                      2.0*M_PI/2000.0/15, -2.0*M_PI/2000.0/15);
         odometryInit(1, 1e-3,
-                     INDEP_WHEEL_RADIUS, INDEP_SHAFT_WIDTH,
+                     INDEP_WHEEL_RADIUS, INDEP_WHEEL_RADIUS,
+                     INDEP_SHAFT_WIDTH,
                      ENCODER1, ENCODER2,
                      2.0*M_PI/1024.0, -2.0*M_PI/1024.0);
 
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/lib/differential_drive.c b/elec/boards/Controller_Motor_STM32/Firmwares/lib/differential_drive.c
index ab84dce..f2b0444 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/lib/differential_drive.c
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/lib/differential_drive.c
@@ -26,7 +26,7 @@ typedef struct {
 typedef struct {
   uint8_t initialized, enabled, running, working;
   uint8_t odometry_process;
-  float wheel_radius, shaft_width;
+  float left_wheel_radius, right_wheel_radius, shaft_width;
   float last_lin_acceleration, last_rot_acceleration;
   float u, v_max, at_max, ar_max;
   command_generator_t left_wheel_speed, right_wheel_speed;
@@ -163,11 +163,13 @@ static void NORETURN traj_following_process(void) {
 }
 
 void dd_start(uint8_t odometry_process,
-              float wheel_radius, float shaft_width, float max_wheel_speed,
+              float left_wheel_radius, float right_wheel_radius,
+              float shaft_width, float max_wheel_speed,
               float v_max, float at_max, float ar_max,
               float k1, float k2, float k3, float Ts) {
   params.odometry_process = odometry_process;
-  params.wheel_radius = wheel_radius;
+  params.left_wheel_radius = left_wheel_radius;
+  params.right_wheel_radius = right_wheel_radius;
   params.shaft_width = shaft_width;
   params.last_lin_acceleration = 0.0;
   params.last_rot_acceleration = 0.0;
@@ -199,14 +201,14 @@ void dd_start(uint8_t odometry_process,
                    tc_get_speed_generator(DD_LINEAR_SPEED_TC),
                    tc_get_position_generator(DD_ROTATIONAL_SPEED_TC),
                    tc_get_speed_generator(DD_ROTATIONAL_SPEED_TC),
-                   wheel_radius, shaft_width, max_wheel_speed,
+                   left_wheel_radius, shaft_width, max_wheel_speed,
                    -1);
   new_dd_generator(&params.right_wheel_speed,
                    tc_get_position_generator(DD_LINEAR_SPEED_TC),
                    tc_get_speed_generator(DD_LINEAR_SPEED_TC),
                    tc_get_position_generator(DD_ROTATIONAL_SPEED_TC),
                    tc_get_speed_generator(DD_ROTATIONAL_SPEED_TC),
-                   wheel_radius, shaft_width, max_wheel_speed,
+                   right_wheel_radius, shaft_width, max_wheel_speed,
                    1);
   new_ramp2_generator(&params.left_wheel, 0.0, &params.left_wheel_speed);
   new_ramp2_generator(&params.right_wheel, 0.0, &params.right_wheel_speed);
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/lib/differential_drive.h b/elec/boards/Controller_Motor_STM32/Firmwares/lib/differential_drive.h
index 2c0a7e4..35d2388 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/lib/differential_drive.h
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/lib/differential_drive.h
@@ -33,7 +33,8 @@
 
 /* Initializes the differential drive
  *  - odometry_process : ID of the odometry process to base the control on
- *  - wheel_radius : radius of the wheels (in meters)
+ *  - left_wheel_radius : radius of the left wheel (in meters)
+ *  - right_wheel_radius : radius of the right wheel (in meters)
  *  - shaft_width : propulsion shaft width (in meters)
  *  - max_wheel_speed : maximum wheel speed (in rad/s)
  *  - v_max : maximum linear speed (in m/s)
@@ -46,7 +47,8 @@
  *        DD_LINEAR_SPEED_TC and DD_ROTATIONAL_SPEED_TC
  */
 void dd_start(uint8_t odometry_process,
-              float wheel_radius, float shaft_width, float max_wheel_speed,
+              float left_wheel_radius, float right_wheel_radius,
+              float shaft_width, float max_wheel_speed,
               float v_max, float at_max, float ar_max,
               float k1, float k2, float k3, float Ts);
 
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/lib/odometry.c b/elec/boards/Controller_Motor_STM32/Firmwares/lib/odometry.c
index 33a597d..902a00d 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/lib/odometry.c
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/lib/odometry.c
@@ -14,7 +14,8 @@ static cpu_stack_t stack_odometry[MAX_ODOMETRY_PROCESSES][(ODOMETRY_STACK_SIZE +
 
 typedef struct {
   robot_state_t robot_state;
-  float wheel_radius, shaft_width, left_encoder_gain, right_encoder_gain;
+  float left_wheel_radius, right_wheel_radius, shaft_width;
+  float left_encoder_gain, right_encoder_gain;
   uint8_t left_encoder, right_encoder;
   float Ts;
   uint8_t enable;
@@ -26,7 +27,8 @@ odometry_state_t state[MAX_ODOMETRY_PROCESSES];
 static void NORETURN odometry_process(void);
 
 void odometryInit(uint8_t process_num, float Ts,
-                  float wheel_radius, float shaft_width,
+                  float left_wheel_radius, float right_wheel_radius,
+                  float shaft_width,
                   uint8_t left_encoder, uint8_t right_encoder,
                   float left_encoder_gain, float right_encoder_gain) {
 
@@ -36,7 +38,8 @@ void odometryInit(uint8_t process_num, float Ts,
   state[process_num].robot_state.theta = 0;
 
   // Initialize robot parameters
-  state[process_num].wheel_radius = wheel_radius;
+  state[process_num].left_wheel_radius = left_wheel_radius;
+  state[process_num].right_wheel_radius = right_wheel_radius;
   state[process_num].shaft_width = shaft_width;
   state[process_num].left_encoder = left_encoder;
   state[process_num].right_encoder = right_encoder;
@@ -108,10 +111,9 @@ static void NORETURN odometry_process(void) {
       last_pos_r = pos_r;
 
       // New state computation
-      state->robot_state.x += state->wheel_radius * (delta_r + delta_l) / 2.0 * cos(state->robot_state.theta);
-      state->robot_state.y += state->wheel_radius * (delta_r + delta_l) / 2.0 * sin(state->robot_state.theta);
-      state->robot_state.theta += state->wheel_radius / state->shaft_width * (delta_r - delta_l);
-
+      state->robot_state.x += (state->right_wheel_radius * delta_r + state->left_wheel_radius * delta_l) / 2.0 * cos(state->robot_state.theta);
+      state->robot_state.y += (state->right_wheel_radius * delta_r + state->left_wheel_radius * delta_l) / 2.0 * sin(state->robot_state.theta);
+      state->robot_state.theta += (state->right_wheel_radius * delta_r - state->left_wheel_radius * delta_l) / state->shaft_width;
       // Normalization of theta
       if (state->robot_state.theta > M_PI) {
         state->robot_state.theta -= 2*M_PI;
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/lib/odometry.h b/elec/boards/Controller_Motor_STM32/Firmwares/lib/odometry.h
index dcafed4..588bf8b 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/lib/odometry.h
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/lib/odometry.h
@@ -23,7 +23,8 @@ typedef struct {
 } robot_state_t;
 
 void odometryInit(uint8_t process_num, float Ts,
-                  float wheel_radius, float shaft_width,
+                  float left_wheel_radius, float right_wheel_radius,
+                  float shaft_width,
                   uint8_t left_encoder, uint8_t right_encoder,
                   float left_encoder_gain, float right_encoder_gain);
 void odo_disable(uint8_t process_num);


hooks/post-receive
-- 
krobot