krobot-commits

[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  cb5df192b9fad7e421db94ddf32b6643e2cda29a (commit)
      from  d73605b0d6b8ce682a0db9516748f0c3636a30cd (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 cb5df192b9fad7e421db94ddf32b6643e2cda29a
Author: Xavier Lagorce <Xav...@cr...>
Date:   Fri Apr 1 01:11:00 2011 +0200

    [Controller_Motor_STM32] Refactored trajectory_generator
    
     * trajectory generators are not linked to a specific motor controller
       (preparation to a new differential driver)
     * motor_controller now uses a structure to specify parameters
     * adapted can_monitor and main to reflect the changes

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

Changes:
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/can_monitor.c b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/can_monitor.c
index ff3ffa8..b1d7395 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/can_monitor.c
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/can_monitor.c
@@ -214,8 +214,8 @@ static void NORETURN canMonitorListen_process(void) {
     txm.sid = 0;
 
     // Initialize robot representation
-    robot.left_wheel = MOTOR3;
-    robot.right_wheel = MOTOR4;
+    robot.left_wheel = 2;
+    robot.right_wheel = 3;
     robot.wheel_radius = 0.049245;
     robot.shaft_width = 0.259;
 
@@ -239,21 +239,21 @@ static void NORETURN canMonitorListen_process(void) {
           case CAN_MSG_MOVE:
             move_msg.data32[0] = frame.data32[0];
             move_msg.data32[1] = frame.data32[1];
-            if (!tc_is_working(MOTOR3 | MOTOR4))
+            if (!tc_is_working(TC_MASK(2) | TC_MASK(3)))
               tc_move(&robot, move_msg.data.distance / 1000.0, move_msg.data.speed / 1000.0, move_msg.data.acceleration / 1000.0);
             break;
           case CAN_MSG_TURN:
             turn_msg.data32[0] = frame.data32[0];
             turn_msg.data32[1] = frame.data32[1];
-            if (!tc_is_working(MOTOR3 | MOTOR4))
+            if (!tc_is_working(TC_MASK(2) | TC_MASK(3)))
               tc_turn(&robot, turn_msg.data.angle / 100.0, turn_msg.data.speed / 100.0, turn_msg.data.acceleration / 100.0);
             break;
           case CAN_MSG_STOP:
             stop_msg.data32[0] = frame.data32[0];
             stop_msg.data32[1] = frame.data32[1];
             if (stop_msg.data.stop == 1) {
-              tc_delete_controller(MOTOR3);
-              tc_delete_controller(MOTOR4);
+              mc_delete_controller(MOTOR3);
+              mc_delete_controller(MOTOR4);
             }
             break;
           case CAN_MSG_ODOMETRY_SET:
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 a60c52e..347a7c3 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
@@ -22,7 +22,7 @@ PROC_DEFINE_STACK(stack_ind, KERN_MINSTACKSIZE * 2);
 
 static void init(void)
 {
-        trajectory_controller_params_t params;
+        motor_controller_params_t params;
 
 	IRQ_ENABLE;
 
@@ -57,10 +57,12 @@ static void init(void)
         params.T = 0.005;
         // Initialize left motor
         params.encoder = ENCODER3;
-        tc_new_controller(MOTOR3, &params);
+        tc_new_controller(2);
+        mc_new_controller(&params, tc_get_position_generator(0));
         // Initialize right motor
         params.encoder = ENCODER4;
-        tc_new_controller(MOTOR4, &params);
+        tc_new_controller(3);
+        mc_new_controller(&params, tc_get_position_generator(1));
 
         // Start odometry
         odometryInit(1e-3, 0.049245, 0.259, -2.0*M_PI/2000.0/15.0);
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/motor_controller.c b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/motor_controller.c
index fdfcc9b..93d092b 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/motor_controller.c
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/motor_controller.c
@@ -171,43 +171,46 @@ void motorControllerInit() {
   motorsInit();
 }
 
-uint8_t mc_new_controller(uint8_t motor, uint8_t encoder, float encoder_gain, float G0, float tau, float T, float *k, float l, float *l0, command_generator_t *generator) {
+uint8_t mc_new_controller(motor_controller_params_t *cntr_params, command_generator_t *generator) {
   uint8_t motor_ind;
   control_params_t *params;
+  float tau, T;
 
   // Find the motor index
-  motor_ind = get_motor_index(motor);
+  motor_ind = get_motor_index(cntr_params->motor);
   params = &(controllers[motor_ind]);
 
   if (params->enable == 0) {
     // define user parameters
-    params->motor = motor;
-    params->encoder = encoder;
-    params->encoder_gain = encoder_gain;
-    params->tau = tau;
-    params->T = ms_to_ticks((mtime_t)(T*1000));
-    params->k[0] = k[0]; params->k[1] = k[1];
-    params->l = l;
-    params->l0[0] = l0[0]; params->l0[1] = l0[1];
+    params->motor = cntr_params->motor;
+    params->encoder = cntr_params->encoder;
+    params->encoder_gain = cntr_params->encoder_gain;
+    params->tau = cntr_params->tau;
+    params->T = ms_to_ticks((mtime_t)(cntr_params->T*1000));
+    params->k[0] = cntr_params->k[0]; params->k[1] = cntr_params->k[1];
+    params->l = cntr_params->l;
+    params->l0[0] = cntr_params->l0[0]; params->l0[1] = cntr_params->l0[1];
 
     // compute other parameters
     params->last_command = 0;
     params->last_estimate[0] = 0; params->last_estimate[1] = 0;
     params->last_output = params->last_estimate[0];
-    params->last_encoder_pos = getEncoderPosition(encoder);
+    params->last_encoder_pos = getEncoderPosition(cntr_params->encoder);
     params->reference = generator;
 
+    tau = cntr_params->tau;
+    T = cntr_params->T;
     params->F[0] = 1;
     params->F[1] = tau*(1-exp(-T/tau));
     params->F[2] = 0;
     params->F[3] = exp(-T/tau);
-    params->G[0] = G0*(T+tau*exp(-T/tau)-tau);
-    params->G[1] = G0*(1-exp(-T/tau));
+    params->G[0] = cntr_params->G0*(T+tau*exp(-T/tau)-tau);
+    params->G[1] = cntr_params->G0*(1-exp(-T/tau));
 
     // enable the controller
     params->enable = 1;
-    enableMotor(motor);
-    motorSetSpeed(motor, 0);
+    enableMotor(cntr_params->motor);
+    motorSetSpeed(cntr_params->motor, 0);
 
     // start the controller
     proc_new(motorController_process, params, KERN_MINSTACKSIZE * 16, NULL);
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/motor_controller.h b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/motor_controller.h
index 0c1a4e1..3b4df3f 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/motor_controller.h
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/motor_controller.h
@@ -13,12 +13,24 @@
 #define CONTROLLER_OK 0
 #define CONTROLLER_ALREADY_USED 1
 
+typedef struct {
+  uint8_t motor;                    // Motor ID to control
+  uint8_t encoder;                  // Encoder ID to measure motor position from
+  float encoder_gain;               // Gain to convert encoder value unit to reference unit
+  float G0;                         // DC motor static gain
+  float tau;                        // DC motor time constant
+  float k[2];                       // State control gain
+  float l;                          // Reference factoring gain
+  float l0[2];                      // State observer gain
+  float T;                          // Sampling period of the controller in seconds
+} motor_controller_params_t;
+
 void motorControllerInit(void);
 float mc_getSpeed(uint8_t motor);
 float mc_getPosition(uint8_t motor);
 void mc_setReference(uint8_t motor, command_generator_t *generator);
 
 
-uint8_t mc_new_controller(uint8_t motor, uint8_t encoder, float encoder_gain, float G0, float tau, float T, float *k, float l, float *l0, command_generator_t *generator);
+uint8_t mc_new_controller(motor_controller_params_t *cntr_params, command_generator_t *generator);
 void mc_delete_controller(uint8_t motor);
 #endif
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/trajectory_controller.c b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/trajectory_controller.c
index a74dd55..605913d 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/trajectory_controller.c
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/trajectory_controller.c
@@ -36,22 +36,14 @@ typedef struct {
 void trapezoid_callback(command_generator_t *generator);
 void acc_stop_callback(command_generator_t *generator);
 
-trajectory_controller_t controllers[4];
-
-static inline uint8_t get_motor_index(uint8_t motor_id) {
-  uint8_t motor_ind;
-
-  for (motor_ind = 0; (motor_id >> (motor_ind+1)) != 0; motor_ind++) ;
-
-  return motor_ind;
-}
+trajectory_controller_t controllers[NUM_TC_MAX];
 
 void trapezoid_callback(command_generator_t *generator) {
   trajectory_controller_t *cont = NULL;
   uint8_t i;
 
   // Select the correct controller depending on chich callbacl triggered
-  for (i=0; i < 4; i++) {
+  for (i=0; i < NUM_TC_MAX; i++) {
     if (generator == &(controllers[i].position)
         || generator == &(controllers[i].speed) ) {
       cont = &controllers[i];
@@ -104,7 +96,7 @@ void acc_stop_callback(command_generator_t *generator) {
   uint8_t i;
 
   // Select the correct controller depending on chich callbacl triggered
-  for (i=0; i < 4; i++) {
+  for (i=0; i < NUM_TC_MAX; i++) {
     if (generator == &(controllers[i].position)
         || generator == &(controllers[i].speed) ) {
       cont = &controllers[i];
@@ -132,7 +124,7 @@ void tc_init(void) {
   uint8_t i;
 
   // All Trajectory controllers are disabled
-  for (i=0; i < 4; i++) {
+  for (i=0; i < NUM_TC_MAX; i++) {
     controllers[i].enabled = 0;
   }
 
@@ -140,11 +132,14 @@ void tc_init(void) {
   motorControllerInit();
 }
 
-void tc_new_controller(uint8_t motor, trajectory_controller_params_t *params) {
+void tc_new_controller(uint8_t cntr_index) {
   trajectory_controller_t *cont;
 
+  if (cntr_index >= NUM_TC_MAX)
+    return;
+
   // Get corresponding controller
-  cont = &controllers[get_motor_index(motor)];
+  cont = &controllers[cntr_index];
 
   // Do nothing if the controller already exists
   if (cont->enabled == 1)
@@ -154,19 +149,7 @@ void tc_new_controller(uint8_t motor, trajectory_controller_params_t *params) {
   new_ramp_generator(&cont->speed, 0., 0.);
   new_ramp2_generator(&cont->position, 0., &cont->speed);
 
-  // Start control of drive motor
-  mc_new_controller(motor,
-                    params->encoder,
-                    params->encoder_gain,
-                    params->G0,
-                    params->tau,
-                    params->T,
-                    params->k,
-                    -params->k[0],
-                    params->l0,
-                    &cont->position);
-
-  // Start the generator
+  // Start the generators
   start_generator(&cont->position);
   start_generator(&cont->speed);
 
@@ -176,31 +159,48 @@ void tc_new_controller(uint8_t motor, trajectory_controller_params_t *params) {
   cont->enabled = 1;
 }
 
-void tc_delete_controller(uint8_t motor) {
+void tc_delete_controller(uint8_t cntr_index) {
   trajectory_controller_t *cont;
 
-  cont = &controllers[get_motor_index(motor)];
+  if (cntr_index >= NUM_TC_MAX)
+    return;
+
+  cont = &controllers[cntr_index];
 
   if (cont->enabled == 1) {
-    mc_delete_controller(motor);
     cont->working = 0;
     cont->enabled = 0;
+    pause_generator(&cont->speed);
+    pause_generator(&cont->position);
   }
 }
 
-uint8_t tc_is_working(uint8_t motors) {
+uint8_t tc_is_working(uint8_t cntr_indexes) {
   uint8_t state = 0, i;
 
   // For each motor in motor, test if the corresponding controller is working
-  for (i=0; i < 4; i++) {
-    if ( ((motors & (1<<i)) != 0) && controllers[i].working == 1)
-      state |= 1 << i;
+  for (i=0; i < NUM_TC_MAX; i++) {
+    if ( ((cntr_indexes & TC_MASK(i)) != 0) && controllers[i].working == 1)
+      state |= TC_MASK(i);
   }
 
   return state;
 }
 
-void tc_goto(uint8_t motor, float angle, float speed, float acceleration) {
+command_generator_t* tc_get_position_generator(uint8_t cntr_index) {
+ trajectory_controller_t *cont;
+
+  // Get the controller and verifies it is enabled
+  if (cntr_index >= NUM_TC_MAX)
+    return NULL;
+  cont = &controllers[cntr_index];
+  if (!cont->enabled)
+    return NULL;
+
+  return &cont->position;
+}
+
+void tc_goto(uint8_t cntr_index, float angle, float speed, float acceleration) {
   float acc_dist, t_acc, t_end;
   trajectory_controller_t *cont;
 
@@ -209,7 +209,9 @@ void tc_goto(uint8_t motor, float angle, float speed, float acceleration) {
     return;
 
   // Get the controller and verifies it is enabled
-  cont = &controllers[get_motor_index(motor)];
+  if (cntr_index >= NUM_TC_MAX)
+    return;
+  cont = &controllers[cntr_index];
   if (!cont->enabled)
     return;
 
@@ -255,7 +257,7 @@ void tc_goto(uint8_t motor, float angle, float speed, float acceleration) {
   cont->working = 1;
 }
 
-void tc_goto_speed(uint8_t motor, float speed, float acceleration) {
+void tc_goto_speed(uint8_t cntr_index, float speed, float acceleration) {
   trajectory_controller_t *cont;
 
   // Verify parameters
@@ -263,7 +265,9 @@ void tc_goto_speed(uint8_t motor, float speed, float acceleration) {
     return;
 
   // Get the controller and verifies it is enabled
-  cont = &controllers[get_motor_index(motor)];
+  if (cntr_index >= NUM_TC_MAX)
+    return;
+  cont = &controllers[cntr_index];
   if (!cont->enabled)
     return;
 
@@ -281,8 +285,8 @@ void tc_move(tc_robot_t *robot, float distance, float speed, float acceleration)
   float dis_s, spe_s, acc_s;
 
   // Let's pause the wheel's speed generators to synchronize movement start
-  pause_generator(&controllers[get_motor_index(robot->left_wheel)].speed);
-  pause_generator(&controllers[get_motor_index(robot->right_wheel)].speed);
+  pause_generator(&controllers[robot->left_wheel].speed);
+  pause_generator(&controllers[robot->right_wheel].speed);
 
   // Compute parameters
   dis_s = distance / robot->wheel_radius * 180.0 / M_PI;
@@ -294,16 +298,16 @@ void tc_move(tc_robot_t *robot, float distance, float speed, float acceleration)
   tc_goto(robot->right_wheel, dis_s, spe_s, acc_s);
 
   // Go
-  start_generator(&controllers[get_motor_index(robot->left_wheel)].speed);
-  start_generator(&controllers[get_motor_index(robot->right_wheel)].speed);
+  start_generator(&controllers[robot->left_wheel].speed);
+  start_generator(&controllers[robot->right_wheel].speed);
 }
 
 void tc_turn(tc_robot_t *robot, float angle, float speed, float acceleration) {
   float angle_s, spe_s, acc_s;
 
   // Let's pause the wheel's speed generators to synchronize movement start
-  pause_generator(&controllers[get_motor_index(robot->left_wheel)].speed);
-  pause_generator(&controllers[get_motor_index(robot->right_wheel)].speed);
+  pause_generator(&controllers[robot->left_wheel].speed);
+  pause_generator(&controllers[robot->right_wheel].speed);
 
   // Compute parameters
   angle_s = angle * robot->shaft_width / 2.0 / robot->wheel_radius;
@@ -315,6 +319,6 @@ void tc_turn(tc_robot_t *robot, float angle, float speed, float acceleration) {
   tc_goto(robot->right_wheel, angle_s, spe_s, acc_s);
 
   // Go
-  start_generator(&controllers[get_motor_index(robot->left_wheel)].speed);
-  start_generator(&controllers[get_motor_index(robot->right_wheel)].speed);
+  start_generator(&controllers[robot->left_wheel].speed);
+  start_generator(&controllers[robot->right_wheel].speed);
 }
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/trajectory_controller.h b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/trajectory_controller.h
index b2361d6..00b3a63 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/trajectory_controller.h
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/trajectory_controller.h
@@ -14,46 +14,46 @@
 #include "command_generator.h"
 #include <math.h>
 
-typedef struct {
-  uint8_t encoder;                  // Encoder ID to measure motor position from
-  float encoder_gain;               // Gain to convert encoder value unit to reference unit
-  float G0;                         // DC motor static gain
-  float tau;                        // DC motor time constant
-  float k[2];                       // State control gain
-  float l;                          // Reference factoring gain
-  float l0[2];                      // State observer gain
-  float T;                          // Sampling period of the controller in seconds
-} trajectory_controller_params_t;
+#define NUM_TC_MAX 4
+#define TC_MASK(num) (1 << (num))
 
 typedef struct {
-  uint8_t left_wheel;               // Left wheel motor ID
-  uint8_t right_wheel;              // Right wheel motor ID
+  uint8_t left_wheel;               // Left wheel trajectory controller index
+  uint8_t right_wheel;              // Right wheel trajectory controller index
   float wheel_radius;               // Radius of the wheels
   float shaft_width;                // Width of the propulsion shaft
 } tc_robot_t;
 
-/* Initialize the trajectory controller system
- *
+/*
+ * Initialize the trajectory controller system
  */
 void tc_init(void);
 
 /* Initialize a new trajectory controller with given parameters
  *
- * This function will initialize a new trajectory controller and the
- * necessary motor controller and generators.
+ * This function will initialize a new trajectory controller
+ *  - cntr_index : index of the trajectory controller to initialize. It should
+ *                 be between 0 and (NUM_TC_MAX-1)
  */
-void tc_new_controller(uint8_t motor, trajectory_controller_params_t *params);
+void tc_new_controller(uint8_t cntr_index);
 
-/* Deletes a given trajectory controller
- *
+/*
+ * Deletes a given trajectory controller
  */
-void tc_delete_controller(uint8_t motor);
+void tc_delete_controller(uint8_t cntr_index);
 
-/* Indicates if the last planified action on 'motors' is finished
+/* Indicates if the last planified action on 'cntr_indexes' is finished
+ *  - cntr_indexes : logical OR of TC_MASK(cntr_index) for interesting indexes
  *
  * Returns the logical OR of working controllers among the ones specified in motors
  */
-uint8_t tc_is_working(uint8_t motors);
+uint8_t tc_is_working(uint8_t cntr_indexes);
+
+/*
+ * Returns the position generator outputting the result of the 'cntr_index'th
+ *  Trajectory Generator.
+ */
+command_generator_t* tc_get_position_generator(uint8_t cntr_index);
 
 /* Moves of a given angle
  *  - angle : angle to move of, can be positive (forward) or negative
@@ -62,12 +62,12 @@ uint8_t tc_is_working(uint8_t motors);
  *  - speed : moving speed (in units per second) should be positive.
  *  - acceleration : in units per second per second, should be positive.
  */
-void tc_goto(uint8_t motor, float angle, float speed, float acceleration);
+void tc_goto(uint8_t cntr_index, float angle, float speed, float acceleration);
 
 /* Accelerates or brakes the robot to the given speed
  *
  */
-void tc_goto_speed(uint8_t motor, float speed, float acceleration);
+void tc_goto_speed(uint8_t cntr_index, float speed, float acceleration);
 
 /* Moves along a line
  *  - robot : pointer to a structure describing the robot configuration


hooks/post-receive
-- 
krobot