Menu
▾
▴
[Krobot-commits] [SCM] krobot branch, master, updated. 239dbffd5fba4a3b3e9b1c3a96306914523dba14
|
From: Xavier L. <Ba...@us...> - 2011-04-05 16:04:52
|
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 239dbffd5fba4a3b3e9b1c3a96306914523dba14 (commit)
via e7280a871e4b495f78d9f389ada95544fd5400df (commit)
via 0cd6a2657478189d74a2a0f43dae9a9c8c5d95f3 (commit)
from 773e02438462c3eb9d7b58086faac1e5e096d280 (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 239dbffd5fba4a3b3e9b1c3a96306914523dba14
Author: Xavier Lagorce <Xav...@cr...>
Date: Tue Apr 5 18:04:05 2011 +0200
[Controller_Motor_STM32] Added support for an Hardware in the Loop simulator
commit e7280a871e4b495f78d9f389ada95544fd5400df
Author: Xavier Lagorce <Xav...@cr...>
Date: Tue Apr 5 15:55:00 2011 +0200
[Controller_Motor_STM32] Add indicator to monitor odometry thread
commit 0cd6a2657478189d74a2a0f43dae9a9c8c5d95f3
Author: Xavier Lagorce <Xav...@cr...>
Date: Tue Apr 5 15:43:39 2011 +0200
[Controller_Motor_STM32] Added some possibilities to monitor motor controllers status
-----------------------------------------------------------------------
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 2617638..0613ca0 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
@@ -10,8 +10,14 @@
#include "can_monitor.h"
#include "hw/hw_led.h"
+#define ROBOT_MODE_NORMAL 0
+#define ROBOT_MODE_HIL 1
+
+// Processes stacks
PROC_DEFINE_STACK(stack_can_send, KERN_MINSTACKSIZE * 8);
PROC_DEFINE_STACK(stack_can_receive, KERN_MINSTACKSIZE * 8);
+// globals
+uint8_t mode;
// Structures to represent data into CAN messages
typedef struct {
@@ -53,6 +59,10 @@ typedef struct {
uint8_t stop; // stop everything
} stop_msg_t;
+typedef struct {
+ uint8_t mode;
+} controller_mode_msg_t;
+
// Union to manipulate CAN messages' data easily
typedef union {
encoder_msg_t data;
@@ -89,6 +99,11 @@ typedef union {
uint32_t data32[2];
} stop_can_msg_t;
+typedef union {
+ controller_mode_msg_t data;
+ uint32_t data32[2];
+} controller_mode_can_msg_t;
+
// Can messages IDs
#define CAN_MSG_ENCODERS34 100 // encoder_can_msg_t
#define CAN_MSG_MOTOR3 101 // motor_can_msg_t
@@ -99,6 +114,7 @@ typedef union {
#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
+#define CAN_MSG_CONTROLLER_MODE 205 // controller_mode_msg_t
// Process for communication
static void NORETURN canMonitor_process(void);
@@ -117,6 +133,8 @@ void canMonitorInit(void) {
can_init();
can_start(CAND1, &cfg);
+ mode = ROBOT_MODE_NORMAL;
+
// Start communication process
proc_new(canMonitor_process, NULL, sizeof(stack_can_send), stack_can_send);
proc_new(canMonitorListen_process, NULL, sizeof(stack_can_receive), stack_can_receive);
@@ -171,21 +189,23 @@ static void NORETURN canMonitor_process(void) {
txm.eid = CAN_MSG_MOTOR4;
can_transmit(CAND1, &txm, ms_to_ticks(10));
- // Sending odometry data
- odo_getState(&odometry);
- 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, 2*M_PI);
- if (odometry.theta > M_PI)
- odometry.theta -= 2*M_PI;
- if (odometry.theta < -M_PI)
- odometry.theta += 2*M_PI;
- msg_odo.data.theta = (int16_t)(odometry.theta * 10000.0);
-
- txm.data32[0] = msg_odo.data32[0];
- txm.data32[1] = msg_odo.data32[1];
- txm.eid = CAN_MSG_ODOMETRY;
- can_transmit(CAND1, &txm, ms_to_ticks(10));
+ // Sending odometry data if not in HIL mode
+ if (mode != ROBOT_MODE_HIL) {
+ odo_getState(&odometry);
+ 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, 2*M_PI);
+ if (odometry.theta > M_PI)
+ odometry.theta -= 2*M_PI;
+ if (odometry.theta < -M_PI)
+ odometry.theta += 2*M_PI;
+ msg_odo.data.theta = (int16_t)(odometry.theta * 10000.0);
+
+ txm.data32[0] = msg_odo.data32[0];
+ txm.data32[1] = msg_odo.data32[1];
+ txm.eid = CAN_MSG_ODOMETRY;
+ can_transmit(CAND1, &txm, ms_to_ticks(10));
+ }
// Wait for the next transmission timer
timer_waitEvent(&timer_can);
@@ -204,6 +224,7 @@ static void NORETURN canMonitorListen_process(void) {
turn_can_msg_t turn_msg;
odometry_can_msg_t odometry_msg;
stop_can_msg_t stop_msg;
+ controller_mode_can_msg_t controller_mode_msg;
// Initialize constant parameters of TX frame
txm.dlc = 8;
@@ -256,6 +277,32 @@ static void NORETURN canMonitorListen_process(void) {
odometry.theta = ((float)odometry_msg.data.theta) / 10000.0;
odo_setState(&odometry);
break;
+ case CAN_MSG_ODOMETRY:
+ // We should only receiv such message in HIL mode
+ if (mode == ROBOT_MODE_HIL) {
+ 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) / 10000.0;
+ odo_setState(&odometry);
+ }
+ break;
+ case CAN_MSG_CONTROLLER_MODE:
+ controller_mode_msg.data32[0] = frame.data32[0];
+ controller_mode_msg.data32[1] = frame.data32[0];
+ if (controller_mode_msg.data.mode == 1) {
+ mc_change_mode(MOTOR3, CONTROLLER_MODE_HIL);
+ mc_change_mode(MOTOR4, CONTROLLER_MODE_HIL);
+ odo_disable();
+ mode = ROBOT_MODE_HIL;
+ } else {
+ mc_change_mode(MOTOR3, CONTROLLER_MODE_NORMAL);
+ mc_change_mode(MOTOR4, CONTROLLER_MODE_NORMAL);
+ odo_restart();
+ mode = ROBOT_MODE_NORMAL;
+ }
+ 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 647f9fa..926c6ad 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
@@ -61,11 +61,11 @@ static void init(void)
// Initialize left motor
params.motor = MOTOR3;
params.encoder = ENCODER3;
- mc_new_controller(¶ms, dd_get_left_wheel_generator());
+ mc_new_controller(¶ms, dd_get_left_wheel_generator(), CONTROLLER_MODE_NORMAL);
// Initialize right motor
params.motor = MOTOR4;
params.encoder = ENCODER4;
- mc_new_controller(¶ms, dd_get_right_wheel_generator());
+ mc_new_controller(¶ms, dd_get_right_wheel_generator(), CONTROLLER_MODE_NORMAL);
// Start odometry
odometryInit(1e-3, WHEEL_RADIUS, SHAFT_WIDTH, -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 537c174..967a552 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
@@ -18,6 +18,8 @@
typedef struct
{
uint8_t enable; // Is this controller enabled ?
+ uint8_t running; // Is this controller running ?
+ uint8_t mode; // Is this a real controller or an HIL controller
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
@@ -32,7 +34,7 @@ typedef struct
uint16_t last_encoder_pos; // Last encoder position measured
float F[4]; // evolution matrix
float G[2]; // command application matrix
- ticks_t T; // sampling period in systicks
+ ticks_t T; // sampling period in seconds
} control_params_t;
static control_params_t controllers[4];
@@ -45,8 +47,9 @@ static inline uint8_t get_motor_index(uint8_t motor_id) {
return motor_ind;
}
-// Callback for control
+// Process functions for control and simulation
static void NORETURN motorController_process(void);
+static void NORETURN motorController_HIL_process(void);
float mc_getSpeed(uint8_t motor) {
@@ -116,12 +119,16 @@ static void NORETURN motorController_process(void) {
// get data
params = (control_params_t *) proc_currentUserData();
+ // Indicate we are running
+ params->running = 1;
+
// configure timer
- timer_setDelay(&timer, ms_to_ticks(params->T));
+ timer_setDelay(&timer, ms_to_ticks((mtime_t)(params->T*1000)));
timer_setEvent(&timer);
while (1) {
if (params->enable == 0) {
+ params->running = 0;
proc_exit();
} else {
timer_add(&timer);
@@ -161,17 +168,80 @@ static void NORETURN motorController_process(void) {
}
}
+/* Hardware in the loop controller process
+ * This process is to be used for Hardware in the loop simulation. It will consider
+ * that the motor control process is perfect and send position data to the simulator
+ * through the CAN bus every sampling period.
+ */
+static void NORETURN motorController_HIL_process(void) {
+ control_params_t *params;
+ Timer timer;
+
+ // get data
+ params = (control_params_t *) proc_currentUserData();
+
+ // Indicate we are running
+ params->running = 1;
+
+ // configure timer
+ timer_setDelay(&timer, ms_to_ticks((time_t)(params->T*1000)));
+ timer_setEvent(&timer);
+
+ while (1) {
+ if (params->enable == 0) {
+ params->running = 0;
+ proc_exit();
+ } else {
+ timer_add(&timer);
+
+ // Compute "state estimation"
+ params->last_estimate[0] = get_output_value(params->reference);
+ params->last_estimate[1] = (params->last_estimate[0] - params->last_output) / params->T;
+
+ // Keep some infos
+ params->last_output = params->last_estimate[0];
+ }
+ timer_waitEvent(&timer); // Wait for the remaining of the sample period
+ }
+}
+
void motorControllerInit() {
uint8_t motor_ind;
- for (motor_ind = 0; motor_ind < 4; motor_ind++)
+ for (motor_ind = 0; motor_ind < 4; motor_ind++) {
controllers[motor_ind].enable = 0;
+ controllers[motor_ind].running = 0;
+ }
encodersInit();
motorsInit();
}
-uint8_t mc_new_controller(motor_controller_params_t *cntr_params, command_generator_t *generator) {
+uint8_t mc_is_controller_enabled(uint8_t motor) {
+ control_params_t *params;
+
+ params = &(controllers[get_motor_index(motor)]);
+ return params->enable;
+}
+
+uint8_t mc_is_controller_running(uint8_t motor) {
+ control_params_t *params;
+
+ params = &(controllers[get_motor_index(motor)]);
+ return params->running;
+}
+
+uint8_t mc_controller_mode(uint8_t motor) {
+ control_params_t *params;
+
+ params = &(controllers[get_motor_index(motor)]);
+ if (params->enable)
+ return params->mode;
+ else
+ return 0;
+}
+
+uint8_t mc_new_controller(motor_controller_params_t *cntr_params, command_generator_t *generator, uint8_t mode) {
uint8_t motor_ind;
control_params_t *params;
float tau, T;
@@ -180,20 +250,20 @@ uint8_t mc_new_controller(motor_controller_params_t *cntr_params, command_genera
motor_ind = get_motor_index(cntr_params->motor);
params = &(controllers[motor_ind]);
- if (params->enable == 0) {
+ if (params->enable == 0 && params->running == 0) {
// define user parameters
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->T = cntr_params->T;
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_estimate[0] = get_output_value(generator); params->last_estimate[1] = 0;
params->last_output = params->last_estimate[0];
params->last_encoder_pos = getEncoderPosition(cntr_params->encoder);
params->reference = generator;
@@ -209,11 +279,17 @@ uint8_t mc_new_controller(motor_controller_params_t *cntr_params, command_genera
// enable the controller
params->enable = 1;
- enableMotor(cntr_params->motor);
- motorSetSpeed(cntr_params->motor, 0);
- // start the controller
- proc_new(motorController_process, params, KERN_MINSTACKSIZE * 16, NULL);
+ // start the correct controller depending on the mode
+ params->mode = mode;
+ if (mode == CONTROLLER_MODE_NORMAL) {
+ proc_new(motorController_process, params, KERN_MINSTACKSIZE * 16, NULL);
+ enableMotor(cntr_params->motor);
+ motorSetSpeed(cntr_params->motor, 0);
+ }
+ else {
+ proc_new(motorController_HIL_process, params, KERN_MINSTACKSIZE * 16, NULL);
+ }
return CONTROLLER_OK;
} else {
@@ -236,6 +312,40 @@ void mc_delete_controller(uint8_t motor) {
// Disable the controller and the power output
if (params->enable != 0) {
params->enable = 0;
- disableMotor(motor);
+ if (params->mode == CONTROLLER_MODE_NORMAL)
+ disableMotor(motor);
+ }
+}
+
+void mc_change_mode(uint8_t motor, uint8_t new_mode) {
+ control_params_t *params;
+
+ params = &(controllers[get_motor_index(motor)]);
+
+ // Do something only if the controller is enabled and in a different mode
+ if (params->enable && params->mode != new_mode) {
+ // Delete the old controller
+ mc_delete_controller(motor);
+ // Wait for the controller process to stop
+ while (params->running)
+ cpu_relax();
+ // Recreate the controller in the correct mode
+ params->last_command = 0;
+ params->last_estimate[0] = get_output_value(params->reference); params->last_estimate[1] = 0;
+ params->last_output = params->last_estimate[0];
+ params->last_encoder_pos = getEncoderPosition(params->encoder);
+ params->mode = new_mode;
+ // enable the controller
+ params->enable = 1;
+
+ // start the correct controller depending on the mode
+ if (new_mode == CONTROLLER_MODE_NORMAL) {
+ proc_new(motorController_process, params, KERN_MINSTACKSIZE * 16, NULL);
+ enableMotor(params->motor);
+ motorSetSpeed(params->motor, 0);
+ }
+ else {
+ proc_new(motorController_HIL_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 3b4df3f..edf226c 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,6 +13,13 @@
#define CONTROLLER_OK 0
#define CONTROLLER_ALREADY_USED 1
+#define CONTROLLER_MODE_NORMAL 1
+#define CONTROLLER_MODE_HIL 2
+
+/*
+ * The motor_controller_params_t structure represents all the parameters needed
+ * by the motor controller. They have to be set according to the motor being controlled
+ */
typedef struct {
uint8_t motor; // Motor ID to control
uint8_t encoder; // Encoder ID to measure motor position from
@@ -25,12 +32,62 @@ typedef struct {
float T; // Sampling period of the controller in seconds
} motor_controller_params_t;
+/*
+ * Initializes the motor controller subsystem. This won't start any controller but has to
+ * be called prior to anything else.
+ */
void motorControllerInit(void);
+
+/*
+ * Returns 'motor' 's last estimated speed.
+ */
float mc_getSpeed(uint8_t motor);
+
+/*
+ * Returns 'motor' 's last position obtained from encoders.
+ */
float mc_getPosition(uint8_t motor);
+
+/*
+ * Change the generator providing its position reference to 'motor' 's controller.
+ */
void mc_setReference(uint8_t motor, command_generator_t *generator);
+/*
+ * Creates a new motor controller.
+ * - cntr_params : pointer to a structure containing controller parameters
+ * - generator : pointer to the generator providing position reference to the controller
+ * - mode : controller mode :
+ * x CONTROLLER_MODE_NORMAL : Normal, state space controller with state estimation
+ * x CONTROLLER_MODE_HIL : Hardware in the loop controller, to use in conjonction with a simulator
+ */
+uint8_t mc_new_controller(motor_controller_params_t *cntr_params, command_generator_t *generator, uint8_t mode);
-uint8_t mc_new_controller(motor_controller_params_t *cntr_params, command_generator_t *generator);
+/*
+ * Asks for the deletion of an existing controller. This will stop the H-bridge
+ * command output, so the control will stop immediately (but the controlling
+ * thread will stop at its next sampling period).
+ */
void mc_delete_controller(uint8_t motor);
+
+/*
+ * Changes the mode of an existing controller. This will create a new controller with the same parameters
+ * as the old one but with the specifyed new mode.
+ */
+void mc_change_mode(uint8_t motor, uint8_t new_mode);
+
+/*
+ * Returns 1 if 'motor' 's controller is enabled.
+ */
+uint8_t mc_is_controller_enabled(uint8_t motor);
+/*
+ * Returns 1 if 'motor' 's controller's process is currently running
+ */
+uint8_t mc_is_controller_running(uint8_t motor);
+/*
+ * Returns 'motor' 's controller mode
+ */
+uint8_t mc_controller_mode(uint8_t motor);
+
+
#endif
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 28d2038..208c342 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
@@ -16,6 +16,7 @@ typedef struct {
float wheel_radius, shaft_width, encoder_gain;
float Ts;
uint8_t enable;
+ uint8_t running;
} odometry_state_t;
odometry_state_t state;
@@ -44,6 +45,14 @@ void odo_disable(void) {
state.enable = 0;
}
+void odo_restart(void) {
+ // Start odometry process
+ if (state.enable == 0 && state.running == 0) {
+ state.enable = 1;
+ proc_new(odometry_process, NULL, sizeof(stack_odometry), stack_odometry);
+ }
+}
+
static void NORETURN odometry_process(void) {
float pos_l, pos_r, last_pos_l, last_pos_r, delta_l, delta_r;
uint8_t dir_l, dir_r;
@@ -53,12 +62,16 @@ static void NORETURN odometry_process(void) {
timer_setDelay(&timer, ms_to_ticks(state.Ts));
timer_setEvent(&timer);
+ // Indicate we are running
+ state.running = 1;
+
// State initialization
last_pos_l = (float)getEncoderPosition(ENCODER3);
last_pos_r = (float)getEncoderPosition(ENCODER4);
while (1) {
if (state.enable == 0) {
+ state.running = 0;
proc_exit();
} else {
timer_add(&timer);
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/odometry.h b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/odometry.h
index b36caa9..0e780a1 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/odometry.h
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/odometry.h
@@ -22,6 +22,7 @@ typedef struct {
void odometryInit(float Ts, float wheel_radius, float shaft_width, float encoder_gain);
void odo_disable(void);
+void odo_restart(void);
void odo_setState(robot_state_t *new_state);
hooks/post-receive
--
krobot
|
