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

[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  ef8216ee6b78ce3a36bcedd5eca2f6305ff65bf1 (commit)
      from  08f9457bae8f067a43784a49e4271a7b7c1a8963 (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 ef8216ee6b78ce3a36bcedd5eca2f6305ff65bf1
Author: Xavier Lagorce <Xav...@cr...>
Date:   Thu Mar 31 14:56:17 2011 +0200

    [Controller_Motor_STM32] New version of trajectory controller
    
     This is a start for the new version of the trajectory controller.
    For the moment, it has the same functionnality as the old one but is a
    lot more generic (can control any motor independently).
    
    More is comming.

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

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 f3e9e55..ff3ffa8 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
@@ -49,6 +49,10 @@ typedef struct {
   uint16_t acceleration __attribute__((__packed__));  // Acceleration in 1/100 degrees/s^2
 } turn_msg_t;
 
+typedef struct {
+  uint8_t stop;  // stop everything
+} stop_msg_t;
+
 // Union to manipulate CAN messages' data easily
 typedef union {
   encoder_msg_t data;
@@ -80,6 +84,11 @@ typedef union {
   uint32_t data32[2];
 } turn_can_msg_t;
 
+typedef union {
+  stop_msg_t data;
+  uint32_t data32[2];
+} stop_can_msg_t;
+
 // Can messages IDs
 #define CAN_MSG_ENCODERS34 100 // encoder_can_msg_t
 #define CAN_MSG_MOTOR3 101 // motor_can_msg_t
@@ -88,6 +97,8 @@ typedef union {
 #define CAN_MSG_ODOMETRY 104 // odometry_can_msg_t
 #define CAN_MSG_MOVE 201 // move_can_msg_t
 #define CAN_MSG_TURN 202 // turn_can_msg_t
+#define CAN_MSG_ODOMETRY_SET 203 // odometry_can_msg_t
+#define CAN_MSG_STOP 204 // stop_can_msg_t
 
 // Process for communication
 static void NORETURN canMonitor_process(void);
@@ -165,8 +176,10 @@ static void NORETURN canMonitor_process(void) {
     msg_odo.data.x = (int16_t)(odometry.x * 1000.0);
     msg_odo.data.y = (int16_t)(odometry.y * 1000.0);
     odometry.theta = fmodf(odometry.theta, 360.0);
-    if (odometry.theta > 180.)
+    if (odometry.theta > 180.0)
       odometry.theta -= 360.0;
+    if (odometry.theta < -180.0)
+      odometry.theta += 360.0;
     msg_odo.data.theta = (int16_t)(odometry.theta * 100.0);
 
     txm.data32[0] = msg_odo.data32[0];
@@ -184,10 +197,15 @@ static void NORETURN canMonitorListen_process(void) {
     can_rx_frame frame;
     bool received = false;
     can_tx_frame txm;
+    robot_state_t odometry;
 
     status_can_msg_t status_msg;
     move_can_msg_t move_msg;
     turn_can_msg_t turn_msg;
+    odometry_can_msg_t odometry_msg;
+    stop_can_msg_t stop_msg;
+
+    tc_robot_t robot;
 
     // Initialize constant parameters of TX frame
     txm.dlc = 8;
@@ -195,6 +213,12 @@ static void NORETURN canMonitorListen_process(void) {
     txm.ide = 1;
     txm.sid = 0;
 
+    // Initialize robot representation
+    robot.left_wheel = MOTOR3;
+    robot.right_wheel = MOTOR4;
+    robot.wheel_radius = 0.049245;
+    robot.shaft_width = 0.259;
+
     while (1) {
       received = can_receive(CAND1, &frame, ms_to_ticks(100));
       if (received) {
@@ -202,7 +226,7 @@ static void NORETURN canMonitorListen_process(void) {
           // Handle requests
           switch (frame.eid) {
           case CAN_MSG_STATUS:
-            status_msg.data.is_moving = !tc_is_finished();
+            status_msg.data.is_moving = tc_is_working(MOTOR1 | MOTOR2 | MOTOR3 | MOTOR4);
             txm.data32[0] = status_msg.data32[0];
             txm.data32[1] = status_msg.data32[1];
             txm.eid = CAN_MSG_STATUS;
@@ -215,14 +239,30 @@ 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_finished())
-              tc_move(move_msg.data.distance / 1000.0, move_msg.data.speed / 1000.0, move_msg.data.acceleration / 1000.0);
+            if (!tc_is_working(MOTOR3 | MOTOR4))
+              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_finished())
-              tc_turn(turn_msg.data.angle / 100.0, turn_msg.data.speed / 100.0, turn_msg.data.acceleration / 100.0);
+            if (!tc_is_working(MOTOR3 | MOTOR4))
+              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);
+            }
+            break;
+          case CAN_MSG_ODOMETRY_SET:
+            odometry_msg.data32[0] = frame.data32[0];
+            odometry_msg.data32[1] = frame.data32[1];
+            odometry.x = ((float)odometry_msg.data.x) / 1000.0;
+            odometry.y = ((float)odometry_msg.data.y) / 1000.0;
+            odometry.theta = ((float)odometry_msg.data.theta) / 100.0;
+            odo_setState(&odometry);
             break;
           }
         }
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 75c88cf..a60c52e 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
@@ -18,12 +18,12 @@
 #include "command_generator.h"
 #include "trajectory_controller.h"
 
-command_generator_t genR, genL;
-
-PROC_DEFINE_STACK(stack_op, KERN_MINSTACKSIZE * 8);
+PROC_DEFINE_STACK(stack_ind, KERN_MINSTACKSIZE * 2);
 
 static void init(void)
 {
+        trajectory_controller_params_t params;
+
 	IRQ_ENABLE;
 
 	/* Initialize debugging module (allow kprintf(), etc.) */
@@ -44,7 +44,23 @@ static void init(void)
         canMonitorInit();
 
         // Start control of drive motors
-        init_trajectory_controller();
+        tc_init();
+        // Common parameters
+        params.encoder_gain = -360.0/2000.0/15.0;
+        params.G0 = 0.833;
+        params.tau = 0.015;
+        params.k[0] = -68.0325;
+        params.k[1] = -1.0205;
+        params.l = -params.k[0];
+        params.l0[0] = 0.0236;
+        params.l0[1] = 3.9715;
+        params.T = 0.005;
+        // Initialize left motor
+        params.encoder = ENCODER3;
+        tc_new_controller(MOTOR3, &params);
+        // Initialize right motor
+        params.encoder = ENCODER4;
+        tc_new_controller(MOTOR4, &params);
 
         // Start odometry
         odometryInit(1e-3, 0.049245, 0.259, -2.0*M_PI/2000.0/15.0);
@@ -64,30 +80,15 @@ static void init(void)
         }
 }
 
-static void NORETURN demo_process(void)
+static void NORETURN ind_process(void)
 {
-  while (1) {
-    // Light a LED on an unused motor to indicate we should be doing something
-    // with the motors very soon
-    LED1_ON();
-    timer_delay(1000);
-
-    // Move the robot !
-    tc_move(2., 1., 1.);
-
-    // Wait until the movement is finished
-    while (!tc_is_finished())
-      timer_delay(1000);
-
-    // Stop motor controllers to be able to freely move the robot
-    mc_delete_controller(MOTOR3);
-    mc_delete_controller(MOTOR4);
-
-    // Doing nothing more, so shutdown the LED
-    LED1_OFF();
-
-    // Cleanly stop the demo process
-    proc_exit();
+  while(1) {
+    if (tc_is_working(MOTOR3 | MOTOR4)) {
+      LED1_ON();
+    } else {
+      LED1_OFF();
+    }
+    timer_delay(500);
   }
 }
 
@@ -96,7 +97,7 @@ int main(void)
 	init();
 
 	/* Create a new child process */
-        //proc_new(demo_process, NULL, sizeof(stack_op), stack_op);
+        proc_new(ind_process, NULL, sizeof(stack_ind), stack_ind);
 
 	/*
 	 * The main process is kept to periodically report the stack
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 407af94..6186796 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
@@ -13,208 +13,210 @@
 #define TRAPEZOID_STATE_ACC      1
 #define TRAPEZOID_STATE_CONST    2
 #define TRAPEZOID_STATE_DEC      3
-#define TRAPEZOID_STATE_TRIANGLE 4
 
 #define SIGN(val) ((val) >= 0 ? 1 : -1)
 #define SELECT_THRESHOLD(dir) ((dir) == 1 ? GEN_CALLBACK_SUP : GEN_CALLBACK_INF)
 #define SELECT_THRESHOLD_DEC(dir) ((dir) == 1 ? GEN_CALLBACK_INF : GEN_CALLBACK_SUP)
 
-void trapezoid_callback(command_generator_t *generator);
-
 typedef struct {
   float angle, speed, acceleration, init_val;
   int8_t dir;
   uint8_t state, is_triangle;
-} ramp_automaton_t;
+} tc_automaton_t;
+
+typedef struct {
+  uint8_t enabled;                  // Is this controller enabled
+  uint8_t working;                  // Is this controller doing something ?
+  command_generator_t position;     // Position generator (RAMP2)
+  command_generator_t speed;        // Speed generator (RAMP)
+  tc_automaton_t aut;               // Automaton to control movement
+} trajectory_controller_t;
+
+
+void trapezoid_callback(command_generator_t *generator);
+void acc_stop_callback(command_generator_t *generator);
 
-command_generator_t right_wheel, left_wheel, right_wheel_speed, left_wheel_speed;
-ramp_automaton_t right_trap, left_trap;
+trajectory_controller_t controllers[4];
 
-uint8_t controller_state;
+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;
+}
 
 void trapezoid_callback(command_generator_t *generator) {
-  ramp_automaton_t *automaton;
-  command_generator_t *trap, *trap_speed;
-
-  // Select the correct trapezoid depending on which callback triggered
-  if (generator == &right_wheel || generator == &right_wheel_speed) {
-    automaton = &right_trap;
-    trap = &right_wheel;
-    trap_speed = &right_wheel_speed;
-  } else if (generator == &left_wheel || generator == &left_wheel_speed) {
-    automaton = &left_trap;
-    trap = &left_wheel;
-    trap_speed = &left_wheel_speed;
-  } else {
+  trajectory_controller_t *cont = NULL;
+  uint8_t i;
+
+  // Select the correct controller depending on chich callbacl triggered
+  for (i=0; i < 4; i++) {
+    if (generator == &(controllers[i].position)
+        || generator == &(controllers[i].speed) ) {
+      cont = &controllers[i];
+      break;
+    }
+  }
+  // Exit if we have not found any...
+  if (cont == NULL) {
+    // Disable the callback to prevent repeated useless triggering
+    remove_callback(generator);
     return;
   }
 
-  // Unregistering callbacks on the current ramp
-  remove_callback(trap);
-  remove_callback(trap_speed);
+  // Unregistering callbacks of the current phase
+  remove_callback(&cont->position);
+  remove_callback(&cont->speed);
 
   // Automaton evolution
-  switch (automaton->state) {
+  switch (cont->aut.state) {
   case TRAPEZOID_STATE_STOP:
     // The automaton is already stopped, this shouldn't happen
     break;
   case TRAPEZOID_STATE_ACC:
     // Speed is inceasing, we have to go to the constant speed phase of the movement
-    adjust_speed(trap_speed, 0.);
-    adjust_value(trap_speed, automaton->speed);
-    add_callback(trap, SELECT_THRESHOLD(automaton->dir), automaton->angle - automaton->speed*automaton->speed/automaton->acceleration/2., trapezoid_callback);
-    automaton->state = TRAPEZOID_STATE_CONST;
-    break;
-  case TRAPEZOID_STATE_TRIANGLE:
-    // Special case of triangle profile, we have to slow down at the end of the acceleration phase
-    adjust_speed(trap_speed, -automaton->acceleration);
-    adjust_value(trap_speed, automaton->speed);
-    add_callback(trap, SELECT_THRESHOLD(automaton->dir), automaton->angle, trapezoid_callback);
-    add_callback(trap_speed, SELECT_THRESHOLD_DEC(automaton->dir), 0.1*automaton->speed, trapezoid_callback);
-    automaton->state = TRAPEZOID_STATE_DEC;
+    adjust_speed(&cont->speed, 0.);
+    adjust_value(&cont->speed, cont->aut.speed);
+    add_callback(&cont->position, SELECT_THRESHOLD(cont->aut.dir), cont->aut.angle - cont->aut.speed*cont->aut.speed/cont->aut.acceleration/2., trapezoid_callback);
+    cont->aut.state = TRAPEZOID_STATE_CONST;
     break;
   case TRAPEZOID_STATE_CONST:
     // End of the constant speed phase, we have to slow down
-    adjust_speed(trap_speed, -automaton->acceleration);
-    add_callback(trap, SELECT_THRESHOLD(automaton->dir), automaton->angle, trapezoid_callback);
-    add_callback(trap_speed, SELECT_THRESHOLD_DEC(automaton->dir), 0.1*automaton->speed, trapezoid_callback);
-    automaton->state = TRAPEZOID_STATE_DEC;
+    adjust_speed(&cont->speed, -cont->aut.acceleration);
+    adjust_value(&cont->speed, cont->aut.speed);
+    add_callback(&cont->position, SELECT_THRESHOLD(cont->aut.dir), cont->aut.angle, trapezoid_callback);
+    add_callback(&cont->speed, SELECT_THRESHOLD_DEC(cont->aut.dir), 0.01*cont->aut.speed, trapezoid_callback);
+    cont->aut.state = TRAPEZOID_STATE_DEC;
     break;
   case TRAPEZOID_STATE_DEC:
     // End of the movement, stop the motor
-    adjust_speed(trap_speed, 0.);
-    adjust_value(trap_speed, 0.);
-    //adjust_value(trap, automaton->angle);
-    automaton->state = TRAPEZOID_STATE_STOP;
-    if (tc_is_finished())
-      LED1_OFF();
+    adjust_speed(&cont->speed, 0.);
+    adjust_value(&cont->speed, 0.);
+    cont->aut.state = TRAPEZOID_STATE_STOP;
+    cont->working = 0;
     break;
   }
 }
 
-void init_trajectory_controller(void) {
-  float k[] = {-68.0325, -1.0205};
-  float l0[] = {0.0236, 3.9715};
-
-  // Create generator for position and speed manipulation
-  new_ramp_generator(&right_wheel_speed, 0., 0.);
-  new_ramp_generator(&left_wheel_speed, 0., 0.);
-  new_ramp2_generator(&right_wheel, 0., &right_wheel_speed);
-  new_ramp2_generator(&left_wheel, 0., &left_wheel_speed);
+void acc_stop_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++) {
+    if (generator == &(controllers[i].position)
+        || generator == &(controllers[i].speed) ) {
+      cont = &controllers[i];
+      break;
+    }
+  }
+  // Exit if we have not found any...
+  if (cont == NULL) {
+    // Disable the callback to prevent repeated useless triggering
+    remove_callback(generator);
+    return;
+  }
 
-  // Start control of drive motors
-  mc_new_controller(MOTOR3, ENCODER3, -360.0/2000.0/15.0, 0.833, 0.015, 0.005, k, -k[0], l0, &left_wheel);
-  mc_new_controller(MOTOR4, ENCODER4, -360.0/2000.0/15.0, 0.833, 0.015, 0.005, k, -k[0], l0, &right_wheel);
+  // Unregistering associated callbacks
+  remove_callback(&cont->position);
+  remove_callback(&cont->speed);
 
-  // Start the generator
-  start_generator(&right_wheel);
-  start_generator(&left_wheel);
-  start_generator(&right_wheel_speed);
-  start_generator(&left_wheel_speed);
-
-  // Initial state of automatons
-  right_trap.state = TRAPEZOID_STATE_STOP;
-  left_trap.state = TRAPEZOID_STATE_STOP;
+  // Stopping acceleration
+  adjust_speed(&cont->speed, 0.);
+  cont->working = 0;
 }
 
-uint8_t tc_is_finished(void) {
-  // Is there currently a trapezoidal command running ?
-  if (right_trap.state == TRAPEZOID_STATE_STOP && left_trap.state == TRAPEZOID_STATE_STOP)
-    return 1;
+void tc_init(void) {
+  uint8_t i;
 
-  // One automaton is running, last planified action should be in progress
-  return 0;
-}
+  // All Trajectory controllers are disabled
+  for (i=0; i < 4; i++) {
+    controllers[i].enabled = 0;
+  }
 
-void tc_move(float distance, float speed, float acceleration) {
-  float acc_dist, t_acc, t_end;
+  // Initialize motor controller system
+  motorControllerInit();
+}
 
-  // Verify parameters
-  if (distance == 0 || speed <= 0 || acceleration <= 0)
-    return;
+void tc_new_controller(uint8_t motor, trajectory_controller_params_t *params) {
+  trajectory_controller_t *cont;
 
-  // Compute some common parameters
-  // For the right motor
-  right_trap.init_val = get_output_value(&right_wheel);
-  right_trap.dir = SIGN(distance);
-  right_trap.angle = right_trap.init_val + distance / WHEEL_R * 180 / M_PI;
-  right_trap.acceleration = right_trap.dir * acceleration / WHEEL_R * 180 / M_PI;
-  // For the left motor
-  left_trap.init_val = get_output_value(&left_wheel);
-  left_trap.dir = SIGN(distance);
-  left_trap.angle = left_trap.init_val + distance / WHEEL_R * 180 / M_PI;
-  left_trap.acceleration = left_trap.dir * acceleration / WHEEL_R * 180 / M_PI;
+  // Get corresponding controller
+  cont = &controllers[get_motor_index(motor)];
 
-  // Is the trapezoidal speed profile posible ?
-  t_acc = speed / acceleration;
-  t_end = (speed * speed + fabsf(distance) * acceleration) / (speed * acceleration);
+  // Do nothing if the controller already exists
+  if (cont->enabled == 1)
+    return;
 
-  if (t_end > (2. * t_acc)) {
-    // A trapezoidal speed profile is possible
-    right_trap.is_triangle = 0;
-    left_trap.is_triangle = 0;
+  // Create generators for position and speed manipulation
+  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);
 
-    // Compute trapezoid parameters for right motor
-    right_trap.speed = right_trap.dir * speed / WHEEL_R * 180 / M_PI;
-    right_trap.state = TRAPEZOID_STATE_ACC;
+  // Start the generator
+  start_generator(&cont->position);
+  start_generator(&cont->speed);
 
-    // Compute trapezoid parameters for left motor
-    left_trap.speed = left_trap.dir * speed / WHEEL_R * 180 / M_PI;
-    left_trap.state = TRAPEZOID_STATE_ACC;
+  // Initial state
+  cont->aut.state = TRAPEZOID_STATE_STOP;
+  cont->working = 0;
+  cont->enabled = 1;
+}
 
-    // This is the distance during which the robot will accelerate
-    acc_dist = SIGN(distance) * speed * speed / acceleration / 2.0 / WHEEL_R * 180.0 / M_PI;
+void tc_delete_controller(uint8_t motor) {
+  trajectory_controller_t *cont;
 
-    // Set accelerations for the trapezoid's first phase and associated callbacks
-    adjust_speed(&right_wheel_speed, right_trap.acceleration);
-    add_callback(&right_wheel, SELECT_THRESHOLD(right_trap.dir), right_trap.init_val + acc_dist, trapezoid_callback);
-    add_callback(&right_wheel_speed, SELECT_THRESHOLD(right_trap.dir), right_trap.speed, trapezoid_callback);
-    adjust_speed(&left_wheel_speed, left_trap.acceleration);
-    add_callback(&left_wheel, SELECT_THRESHOLD(left_trap.dir), left_trap.init_val + acc_dist, trapezoid_callback);
-    add_callback(&left_wheel_speed, SELECT_THRESHOLD(left_trap.dir), left_trap.speed, trapezoid_callback);
-  } else {
-    // A trapezoidal speed profile is not possible with the given acceleration, let's go for a triangle
-    right_trap.is_triangle = 1;
-    left_trap.is_triangle = 1;
+  cont = &controllers[get_motor_index(motor)];
 
-    // Compute triangle parameters for right motor
-    right_trap.speed = right_trap.dir * sqrt(distance / WHEEL_R * 180 / M_PI * right_trap.acceleration);
-    right_trap.state = TRAPEZOID_STATE_TRIANGLE;
+  if (cont->enabled == 1) {
+    mc_delete_controller(motor);
+    cont->working = 0;
+    cont->enabled = 0;
+  }
+}
 
-    // Compute triangle parameters for left motor
-    left_trap.speed = left_trap.dir * sqrt(distance / WHEEL_R * 180 / M_PI * left_trap.acceleration);
-    left_trap.state = TRAPEZOID_STATE_TRIANGLE;
+uint8_t tc_is_working(uint8_t motors) {
+  uint8_t state = 0, i;
 
-    // Set accelerations for the triangle's first phase and associated callbacks
-    adjust_speed(&right_wheel_speed, right_trap.acceleration);
-    add_callback(&right_wheel, SELECT_THRESHOLD(right_trap.dir), right_trap.init_val + distance / 2.0 / WHEEL_R * 180 / M_PI, trapezoid_callback);
-    add_callback(&right_wheel_speed, SELECT_THRESHOLD(right_trap.dir), right_trap.speed, trapezoid_callback);
-    adjust_speed(&left_wheel_speed, left_trap.acceleration);
-    add_callback(&left_wheel, SELECT_THRESHOLD(left_trap.dir), left_trap.init_val + distance / 2.0 / WHEEL_R * 180 / M_PI, trapezoid_callback);
-    add_callback(&left_wheel_speed, SELECT_THRESHOLD(left_trap.dir), left_trap.speed, trapezoid_callback);
+  // 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;
   }
 
-  LED1_ON();
+  return state;
 }
 
-void tc_turn(float angle, float speed, float acceleration) {
-  float acc_angle, t_acc, t_end;
+void tc_goto(uint8_t motor, float angle, float speed, float acceleration) {
+  float acc_dist, t_acc, t_end;
+  trajectory_controller_t *cont;
 
   // Verify parameters
   if (angle == 0 || speed <= 0 || acceleration <= 0)
     return;
 
+  // Get the controller and verifies it is enabled
+  cont = &controllers[get_motor_index(motor)];
+  if (!cont->enabled)
+    return;
+
   // Compute some common parameters
-  // For the right motor
-  right_trap.init_val = get_output_value(&right_wheel);
-  right_trap.dir = SIGN(angle);
-  right_trap.angle = right_trap.init_val + angle * STRUCT_B / 2.0 / WHEEL_R;
-  right_trap.acceleration = right_trap.dir * acceleration * STRUCT_B / 2.0 / WHEEL_R;
-  // For the left motor
-  left_trap.init_val = get_output_value(&left_wheel);
-  left_trap.dir = -right_trap.dir;
-  left_trap.angle = left_trap.init_val - angle * STRUCT_B / 2.0 / WHEEL_R;
-  left_trap.acceleration = left_trap.dir * acceleration * STRUCT_B / 2.0 / WHEEL_R;
+  cont->aut.init_val = get_output_value(&cont->position);
+  cont->aut.dir = SIGN(angle);
+  cont->aut.angle = cont->aut.init_val + angle;
+  cont->aut.acceleration = cont->aut.dir * acceleration;
 
   // Is the trapezoidal speed profile posible ?
   t_acc = speed / acceleration;
@@ -222,49 +224,74 @@ void tc_turn(float angle, float speed, float acceleration) {
 
   if (t_end > (2. * t_acc)) {
     // A trapezoidal speed profile is possible
-    right_trap.is_triangle = 0;
-    left_trap.is_triangle = 0;
+    cont->aut.is_triangle = 0;
 
-    // Compute trapezoid parameters for right motor
-    right_trap.speed = right_trap.dir * speed * STRUCT_B / 2.0 / WHEEL_R;
-    right_trap.state = TRAPEZOID_STATE_ACC;
+    // Compute trapezoid parameters
+    cont->aut.speed = cont->aut.dir * speed;
+    cont->aut.state = TRAPEZOID_STATE_ACC;
 
-    // Compute trapezoid parameters for left motor
-    left_trap.speed = left_trap.dir * speed * STRUCT_B / 2.0 / WHEEL_R;
-    left_trap.state = TRAPEZOID_STATE_ACC;
-
-    // This is the angle during which the robot will accelerate
-    acc_angle = SIGN(angle) * speed * speed / acceleration / 2.0 * STRUCT_B / 2.0 / WHEEL_R;
+    // This is the distance during which the robot will accelerate
+    acc_dist = cont->aut.dir * speed * speed / acceleration;
 
     // Set accelerations for the trapezoid's first phase and associated callbacks
-    adjust_speed(&right_wheel_speed, right_trap.acceleration);
-    add_callback(&right_wheel, SELECT_THRESHOLD(right_trap.dir), right_trap.init_val + acc_angle, trapezoid_callback);
-    add_callback(&right_wheel_speed, SELECT_THRESHOLD(right_trap.dir), right_trap.speed, trapezoid_callback);
-    adjust_speed(&left_wheel_speed, left_trap.acceleration);
-    add_callback(&left_wheel, SELECT_THRESHOLD(left_trap.dir), left_trap.init_val - acc_angle, trapezoid_callback);
-    add_callback(&left_wheel_speed, SELECT_THRESHOLD(left_trap.dir), left_trap.speed, trapezoid_callback);
+    adjust_speed(&cont->speed, cont->aut.acceleration);
+    add_callback(&cont->position, SELECT_THRESHOLD(cont->aut.dir), cont->aut.init_val + acc_dist, trapezoid_callback);
+    add_callback(&cont->speed, SELECT_THRESHOLD(cont->aut.dir), cont->aut.speed, trapezoid_callback);
   } else {
     // A trapezoidal speed profile is not possible with the given acceleration, let's go for a triangle
-    right_trap.is_triangle = 1;
-    left_trap.is_triangle = 1;
+    cont->aut.is_triangle = 1;
 
-    // Compute triangle parameters for right motor
-    right_trap.speed = right_trap.dir * sqrt(angle * STRUCT_B / 2.0 / WHEEL_R * right_trap.acceleration);
-    right_trap.state = TRAPEZOID_STATE_TRIANGLE;
-
-    // Compute triangle parameters for left motor
-    left_trap.speed = left_trap.dir * sqrt(- angle * STRUCT_B / 2.0 / WHEEL_R * left_trap.acceleration);
-    left_trap.state = TRAPEZOID_STATE_TRIANGLE;
+    // Compute triangle parameters
+    cont->aut.speed = cont->aut.dir * sqrt(angle * cont->aut.acceleration);
+    cont->aut.state = TRAPEZOID_STATE_CONST;
 
     // Set accelerations for the triangle's first phase and associated callbacks
-    adjust_speed(&right_wheel_speed, right_trap.acceleration);
-    add_callback(&right_wheel, SELECT_THRESHOLD(right_trap.dir), right_trap.init_val + angle / 2.0 * STRUCT_B / 2.0 / WHEEL_R, trapezoid_callback);
-    add_callback(&right_wheel_speed, SELECT_THRESHOLD(right_trap.dir), right_trap.speed, trapezoid_callback);
-    adjust_speed(&left_wheel_speed, left_trap.acceleration);
-    add_callback(&left_wheel, SELECT_THRESHOLD(left_trap.dir), left_trap.init_val - angle / 2.0 * STRUCT_B / 2.0 / WHEEL_R, trapezoid_callback);
-    add_callback(&left_wheel_speed, SELECT_THRESHOLD(left_trap.dir), left_trap.speed, trapezoid_callback);
+    adjust_speed(&cont->speed, cont->aut.acceleration);
+    add_callback(&cont->position, SELECT_THRESHOLD(cont->aut.dir), cont->aut.init_val + angle / 2.0, trapezoid_callback);
+    add_callback(&cont->speed, SELECT_THRESHOLD(cont->aut.dir), cont->aut.speed, trapezoid_callback);
   }
 
-  LED1_ON();
+  cont->working = 1;
+}
+
+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);
+
+  // Compute parameters
+  dis_s = distance / robot->wheel_radius * 180.0 / M_PI;
+  spe_s = speed / robot->wheel_radius * 180.0 / M_PI;
+  acc_s = acceleration / robot->wheel_radius * 180.0 / M_PI;
+
+  // Planify movements
+  tc_goto(robot->left_wheel, dis_s, spe_s, acc_s);
+  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);
 }
 
+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);
+
+  // Compute parameters
+  angle_s = angle * robot->shaft_width / 2.0 / robot->wheel_radius;
+  spe_s = speed * robot->shaft_width / 2.0 / robot->wheel_radius;
+  acc_s = acceleration * robot->shaft_width / 2.0 / robot->wheel_radius;
+
+  // Planify movements
+  tc_goto(robot->left_wheel, -angle_s, spe_s, acc_s);
+  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);
+}
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 0e494e8..77b73e9 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
@@ -10,42 +10,76 @@
 #ifndef __TRAJECTORY_CONTROLLER_H
 #define __TRAJECTORY_CONTROLLER_H
 
-// Robot parameters
-#define WHEEL_R    0.049245
-#define STRUCT_B   0.259
-
 #include "motor_controller.h"
 #include "command_generator.h"
 #include <math.h>
 
-/* Initialize the trajectory controller
+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;
+
+typedef struct {
+  uint8_t left_wheel;               // Left wheel motor ID
+  uint8_t right_wheel;              // Right wheel motor ID
+  float wheel_radius;               // Radius of the wheels
+  float shaft_width;                // Width of the propulsion shaft
+} tc_robot_t;
+
+/* 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.
+ */
+void tc_new_controller(uint8_t motor, trajectory_controller_params_t *params);
+
+/* Deletes a given trajectory controller
  *
- * This function will initialize the trajectory controller and the
- * necessary motor controller.
  */
-void init_trajectory_controller(void);
+void tc_delete_controller(uint8_t motor);
 
-/* Indicates if the last planified action is finished
+/* Indicates if the last planified action on 'motors' is finished
  *
- * Returns 1 if the last planified action is finished, 0 if it is not.
+ * Returns the logical OR of working controllers among the ones specified in motors
  */
-uint8_t tc_is_finished(void);
+uint8_t tc_is_working(uint8_t motors);
 
-/* Move along a line
+/* Moves of a given distance
+ *  - distance : distance to move of, can be positive (forward) or negative
+ *               (backward). The units corresponds to the given scaling factor
+ *               during initialization.
+ *  - speed : moving speed (in meters per second) should be positive.
+ *  - acceleration : in meters per second per second, should be positive.
+ */
+void tc_goto(uint8_t motor, float angle, float speed, float acceleration);
+
+/* Moves along a line
+ *  - robot : pointer to a structure describing the robot configuration
  *  - distance : distance to move of (in meters), can be positive (forward)
  *                or negative (backward).
  *  - speed : moving speed (in meters per second) should be positive.
  *  - acceleration : in meters per second per second, should be positive.
  */
-void tc_move(float distance, float speed, float acceleration);
-
+void tc_move(tc_robot_t *robot, float distance, float speed, float acceleration);
 
-/* Turn around the propulsion shaft center
+/* Turns around the propulsion shaft center
+ *  - robot : pointer to a structure describing the robot configuration
  *  - angle : angle to turn of (in degrees), can be positive (CCW)
  *                or negative (CW).
  *  - speed : turning speed (in degrees per second) should be positive.
  *  - acceleration : in degrees per second per second, should be positive.
  */
-void tc_turn(float angle, float speed, float acceleration);
+void tc_turn(tc_robot_t *robot, float angle, float speed, float acceleration);
 
 #endif /* __TRAJECTORY_CONTROLLER_H */


hooks/post-receive
-- 
krobot