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

[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  3b208ec28291deddce260e77fb3e05f8b810dfcd (commit)
       via  f462e4a5544e2ab3300fa71c30be6d64ae251405 (commit)
      from  83d83da9e7a092de93bfda899bdbada8d233bcab (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 3b208ec28291deddce260e77fb3e05f8b810dfcd
Author: Xavier Lagorce <Xav...@cr...>
Date:   Tue Apr 5 14:44:53 2011 +0200

    [Controller_Motor_STM32] Added the possibility to limit wheel's speed.
    
    This is to be used as a security to limit speed command reference in case of
    wrong high level computation.

commit f462e4a5544e2ab3300fa71c30be6d64ae251405
Author: Xavier Lagorce <Xav...@cr...>
Date:   Tue Apr 5 14:44:16 2011 +0200

    [Controller_Motor_STM32] Normalized angle used in odometry for internal computation

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

Changes:
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/command_generator.c b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/command_generator.c
index 264cf7d..bc54d89 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/command_generator.c
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/command_generator.c
@@ -8,7 +8,7 @@
  */
 
 #include "command_generator.h"
-
+#include <stdlib.h>
 
 command_generator_t* new_constant_generator(command_generator_t *generator, float value) {
   generator->type.t = GEN_CONSTANT;
@@ -46,7 +46,7 @@ command_generator_t* new_dd_generator(command_generator_t *generator,
                                       command_generator_t *linear_speed,
                                       command_generator_t *rotational_pos,
                                       command_generator_t *rotational_speed,
-                                      float wheel_radius, float shaft_width,
+                                      float wheel_radius, float shaft_width, float max_speed,
                                       uint8_t type) {
   generator->type.t = (type == 1) ? GEN_DD_RIGHT : GEN_DD_LEFT;
   generator->type.callback.type = GEN_CALLBACK_NONE;
@@ -56,6 +56,7 @@ command_generator_t* new_dd_generator(command_generator_t *generator,
   generator->dd.rotational_speed = rotational_speed;
   generator->dd.wheel_radius = wheel_radius;
   generator->dd.shaft_width = shaft_width;
+  generator->dd.max_speed = max_speed;
 
   return generator;
 }
@@ -148,7 +149,12 @@ float get_output_value(command_generator_t *generator) {
     // Compute output
     u1 = get_output_value(generator->dd.linear_speed);
     u2 = get_output_value(generator->dd.rotational_speed);
-    generator->type.last_output = (4.0*u1+u2*generator->dd.shaft_width) / (4.0 * generator->dd.wheel_radius);
+    generator->type.last_output = (2.0*u1+u2*generator->dd.shaft_width) / (2.0 * generator->dd.wheel_radius);
+    if (generator->type.last_output >= 0) {
+      generator->type.last_output = MIN(generator->type.last_output, generator->dd.max_speed);
+    } else {
+      generator->type.last_output = MAX(generator->type.last_output, -generator->dd.max_speed);
+    }
     break;
   case GEN_DD_LEFT:
     // Update position generators to allow callbacks
@@ -157,7 +163,12 @@ float get_output_value(command_generator_t *generator) {
     // Compute output
     u1 = get_output_value(generator->dd.linear_speed);
     u2 = get_output_value(generator->dd.rotational_speed);
-    generator->type.last_output = (4.0*u1-u2*generator->dd.shaft_width) / (4.0 * generator->dd.wheel_radius);
+    generator->type.last_output = (2.0*u1-u2*generator->dd.shaft_width) / (2.0 * generator->dd.wheel_radius);
+    if (generator->type.last_output >= 0) {
+      generator->type.last_output = MIN(generator->type.last_output, generator->dd.max_speed);
+    } else {
+      generator->type.last_output = MAX(generator->type.last_output, -generator->dd.max_speed);
+    }
     break;
   }
 
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/command_generator.h b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/command_generator.h
index 6d888d6..28aa810 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/command_generator.h
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/command_generator.h
@@ -77,6 +77,7 @@ typedef struct {
   command_generator_t *rotational_speed;
   float wheel_radius;
   float shaft_width;
+  float max_speed;
 } dd_generator_t;
 
 // Usable generator meta-type
@@ -124,6 +125,7 @@ command_generator_t* new_ramp2_generator(command_generator_t *generator,
  *  - rotational_speed : pointer to the generator giving the rotational speed of the drive
  *  - wheel_radius : radius of the wheels
  *  - shaft_width : width of the propulsion shaft
+ *  - max_speed : maximum wheel speed (in rad/s)
  *  - type : 1 for the right_wheel, -1 for the left_wheel
  */
 command_generator_t* new_dd_generator(command_generator_t *generator,
@@ -131,7 +133,7 @@ command_generator_t* new_dd_generator(command_generator_t *generator,
                                       command_generator_t *linear_speed,
                                       command_generator_t *rotational_pos,
                                       command_generator_t *rotational_speed,
-                                      float wheel_radius, float shaft_width,
+                                      float wheel_radius, float shaft_width, float max_speed,
                                       uint8_t type);
 
 /*
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/differential_drive.c b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/differential_drive.c
index 3e43444..9a03d1f 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/differential_drive.c
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/differential_drive.c
@@ -19,7 +19,7 @@ typedef struct {
 
 static dd_params_t params;
 
-void dd_start(float wheel_radius, float shaft_width) {
+void dd_start(float wheel_radius, float shaft_width, float max_wheel_speed) {
   params.wheel_radius = wheel_radius;
   params.shaft_radius = shaft_width;
   params.last_lin_acceleration = 0.0;
@@ -32,14 +32,14 @@ void dd_start(float wheel_radius, float shaft_width) {
                    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,
+                   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,
+                   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/Firmware/controller_motor_stm32/differential_drive.h b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/differential_drive.h
index 4e68b0a..61684ce 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/differential_drive.h
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/differential_drive.h
@@ -24,11 +24,12 @@
 /* Initializes the differential drive
  *  - wheel_radius : radius of the wheels (in meters)
  *  - shaft_width : propulsion shaft radius (in meters)
+ *  - max_speed : maximum wheel speed (in rad/s)
  *
  * Note : the differential drive system will use Trajectory controllers
  *        DD_LINEAR_SPEED_TC and DD_ROTATIONAL_SPEED_TC
  */
-void dd_start(float wheel_radius, float shaft_width);
+void dd_start(float wheel_radius, float shaft_width, float max_speed);
 
 /* Pauses or Resumes the differential drive system.
  * In pause mode, the drive will accept no further command and actions will be
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/main.c b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/main.c
index 5584084..647f9fa 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/main.c
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/main.c
@@ -47,7 +47,7 @@ static void init(void)
 
         // Start control of drive motors
         tc_init();
-        dd_start(WHEEL_RADIUS, SHAFT_WIDTH);
+        dd_start(WHEEL_RADIUS, SHAFT_WIDTH, 4.8*2*M_PI); // limit wheel speed to 1.5 m/s
         // Common parameters
         params.encoder_gain = -2.0*M_PI/2000.0/15.0;
         params.G0 = 0.0145;
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/odometry.c b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/odometry.c
index f6241fa..28d2038 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/odometry.c
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/odometry.c
@@ -91,6 +91,13 @@ static void NORETURN odometry_process(void) {
       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);
+
+      // Normalization of theta
+      if (state.robot_state.theta > M_PI) {
+        state.robot_state.theta -= 2*M_PI;
+      } else if (state.robot_state.theta < -M_PI) {
+        state.robot_state.theta += 2*M_PI;
+      }
     }
     timer_waitEvent(&timer); // Wait for the remaining of the sample period
   }


hooks/post-receive
-- 
krobot