Menu
▾
▴
[Krobot-commits] [SCM] krobot branch, master, updated. 680647577e28cc060a5c859db28d900d1d4305b6
|
From: Xavier L. <Ba...@us...> - 2011-05-27 20:23:42
|
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 680647577e28cc060a5c859db28d900d1d4305b6 (commit)
via eac9c56632f80565e97b7770b945c9ac0f3fff38 (commit)
from ac58fcf9777f7ccd645c408224c34dc2c5f3ce85 (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 680647577e28cc060a5c859db28d900d1d4305b6
Author: Xavier Lagorce <Xav...@cr...>
Date: Sat May 21 18:13:38 2011 +0200
[Controller_Motor_STM32] Modifications for 2011 Krobot and more odometry params.
commit eac9c56632f80565e97b7770b945c9ac0f3fff38
Author: Xavier Lagorce <Xav...@cr...>
Date: Mon May 2 16:25:53 2011 +0200
[Controller_Motor_STM32] Added an holonomic drive generator.
+ refactored differential drive generator.
-----------------------------------------------------------------------
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 bc54d89..0d1145a 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
@@ -61,9 +61,49 @@ command_generator_t* new_dd_generator(command_generator_t *generator,
return generator;
}
+command_generator_t* new_hd_generator(command_generator_t *generator,
+ command_generator_t *linear_pos_x,
+ command_generator_t *linear_speed_x,
+ command_generator_t *linear_pos_y,
+ command_generator_t *linear_speed_y,
+ command_generator_t *rotational_pos,
+ command_generator_t *rotational_speed,
+ float wheel_radius, float struct_radius, float max_speed,
+ uint8_t type) {
+ switch (type) {
+ case 1: // Back wheel
+ generator->type.t = GEN_HD_B;
+ break;
+ case 2: // Right-front wheel
+ generator->type.t = GEN_HD_RF;
+ break;
+ case 3: // Left-front wheel
+ generator->type.t = GEN_HD_LF;
+ break;
+ default:
+ return NULL;
+ break;
+ }
+ generator->type.callback.type = GEN_CALLBACK_NONE;
+ generator->hd.linear_pos_x = linear_pos_x;
+ generator->hd.linear_speed_x = linear_speed_x;
+ generator->hd.linear_pos_x = linear_pos_y;
+ generator->hd.linear_speed_x = linear_speed_y;
+ generator->hd.rotational_pos = rotational_pos;
+ generator->hd.rotational_speed = rotational_speed;
+ generator->hd.wheel_radius = wheel_radius;
+ generator->hd.struct_radius = struct_radius;
+ generator->hd.max_speed = max_speed;
+
+ return generator;
+}
+
command_generator_t* adjust_value(command_generator_t *generator, float value) {
- if (generator->type.t != GEN_DD_RIGHT && generator->type.t != GEN_DD_LEFT) {
+ uint8_t type = generator->type.t;
+
+ if (type != GEN_DD_RIGHT && type != GEN_DD_LEFT
+ && type != GEN_HD_B && type != GEN_HD_RF && type != GEN_HD_LF) {
generator->type.last_output = value;
return generator;
} else {
@@ -120,7 +160,7 @@ command_generator_t* remove_callback(command_generator_t *generator) {
float get_output_value(command_generator_t *generator) {
int32_t cur_time;
- float speed, dt, u1, u2;
+ float speed, dt, u1, u2, u_x, u_y, w;
if (generator->type.state != GEN_STATE_RUNNING)
return generator->type.last_output;
@@ -143,31 +183,56 @@ float get_output_value(command_generator_t *generator) {
generator->ramp2.last_time = cur_time;
break;
case GEN_DD_RIGHT:
+ case GEN_DD_LEFT:
// Update position generators to allow callbacks
get_output_value(generator->dd.linear_pos);
get_output_value(generator->dd.rotational_pos);
// Compute output
u1 = get_output_value(generator->dd.linear_speed);
u2 = get_output_value(generator->dd.rotational_speed);
- generator->type.last_output = (2.0*u1+u2*generator->dd.shaft_width) / (2.0 * generator->dd.wheel_radius);
+ switch (generator->type.t) {
+ case GEN_DD_RIGHT:
+ generator->type.last_output = (2.0*u1+u2*generator->dd.shaft_width) / (2.0 * generator->dd.wheel_radius);
+ break;
+ case GEN_DD_LEFT:
+ generator->type.last_output = (2.0*u1-u2*generator->dd.shaft_width) / (2.0 * generator->dd.wheel_radius);
+ break;
+ }
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:
+ case GEN_HD_B:
+ case GEN_HD_RF:
+ case GEN_HD_LF:
// Update position generators to allow callbacks
- get_output_value(generator->dd.linear_pos);
- get_output_value(generator->dd.rotational_pos);
+ get_output_value(generator->hd.linear_pos_x);
+ get_output_value(generator->hd.linear_pos_y);
+ get_output_value(generator->hd.rotational_pos);
// Compute output
- u1 = get_output_value(generator->dd.linear_speed);
- u2 = get_output_value(generator->dd.rotational_speed);
- generator->type.last_output = (2.0*u1-u2*generator->dd.shaft_width) / (2.0 * generator->dd.wheel_radius);
+ u_x = get_output_value(generator->hd.linear_speed_x);
+ u_y = get_output_value(generator->hd.linear_speed_y);
+ w = get_output_value(generator->hd.rotational_speed);
+ switch (generator->type.t) {
+ case GEN_HD_B:
+ generator->type.last_output =
+ (u_x + w*generator->hd.struct_radius) / generator->hd.wheel_radius;
+ break;
+ case GEN_HD_RF:
+ generator->type.last_output =
+ (-u_x/2.0 + u_y*sqrt(3.0)/2.0 + w*generator->hd.struct_radius) / generator->hd.wheel_radius;
+ break;
+ case GEN_HD_LF:
+ generator->type.last_output =
+ (-u_x/2.0 - u_y*sqrt(3.0)/2.0 + w*generator->hd.struct_radius) / generator->hd.wheel_radius;
+ break;
+ }
if (generator->type.last_output >= 0) {
- generator->type.last_output = MIN(generator->type.last_output, generator->dd.max_speed);
+ generator->type.last_output = MIN(generator->type.last_output, generator->hd.max_speed);
} else {
- generator->type.last_output = MAX(generator->type.last_output, -generator->dd.max_speed);
+ generator->type.last_output = MAX(generator->type.last_output, -generator->hd.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 28aa810..3fa2b6a 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
@@ -26,6 +26,9 @@
// drive.
#define GEN_DD_RIGHT 5 // Outputs the right wheel speed for a differential
// drive.
+#define GEN_HD_B 6 // Outpus the back wheel speed for an holonomic drive.
+#define GEN_HD_RF 7 // Outpus the right-front wheel speed for an holonomic drive.
+#define GEN_HD_LF 8 // Outpus the left-front wheel speed for an holonomic drive.
// Generator states
#define GEN_STATE_PAUSE 0 // The output is freezed.
@@ -80,6 +83,19 @@ typedef struct {
float max_speed;
} dd_generator_t;
+typedef struct {
+ placeholder_generator_t gen;
+ command_generator_t *linear_pos_x;
+ command_generator_t *linear_speed_x;
+ command_generator_t *linear_pos_y;
+ command_generator_t *linear_speed_y;
+ command_generator_t *rotational_pos;
+ command_generator_t *rotational_speed;
+ float wheel_radius;
+ float struct_radius;
+ float max_speed;
+} hd_generator_t;
+
// Usable generator meta-type
union _command_generator_t {
placeholder_generator_t type;
@@ -87,6 +103,7 @@ union _command_generator_t {
ramp_generator_t ramp;
ramp2_generator_t ramp2;
dd_generator_t dd;
+ hd_generator_t hd;
};
/* Initializes a new Constant Generator.
@@ -136,6 +153,40 @@ command_generator_t* new_dd_generator(command_generator_t *generator,
float wheel_radius, float shaft_width, float max_speed,
uint8_t type);
+/* Initializes a new Holonomic Drive generator.
+ * - generator : pointer to the generator to initialize
+ * - linear_pos_x : pointer to the generator giving the integrates of linear_speed_x. This
+ * generator will be called at each computation to allow update in parallel
+ * with linear_speed.
+ * - linear_speed_x : pointer to the generator giving the linear speed along the x axis of
+ * the drive.
+ * - linear_pos_y : pointer to the generator giving the integrates of linear_speed_y. This
+ * generator will be called at each computation to allow update in parallel
+ * with linear_speed.
+ * - linear_speed_y : pointer to the generator giving the linear speed along the y axis of
+ * the drive.
+ * - rotational_pos : pointer to the generator giving the integrates of rotational_speed. This
+ * generator will be called at each computation to allow update in parallel
+ * with rotational_speed.
+ * - rotational_speed : pointer to the generator giving the rotational speed of the drive.
+ * - wheel_radius : radius of the wheels.
+ * - struct_radius : radius of the holonomic drive.
+ * - max_speed : maximum wheel speed (in rad/s).
+ * - type :
+ * o 1 is the back wheel
+ * o 2 is the right-front wheel
+ * o 3 is the left-front wheel
+ */
+command_generator_t* new_hd_generator(command_generator_t *generator,
+ command_generator_t *linear_pos_x,
+ command_generator_t *linear_speed_x,
+ command_generator_t *linear_pos_y,
+ command_generator_t *linear_speed_y,
+ command_generator_t *rotational_pos,
+ command_generator_t *rotational_speed,
+ float wheel_radius, float struct_radius, float max_speed,
+ uint8_t type);
+
/*
* Adjusts the current output value of 'generator' to 'value'.
*/
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 4b10660..5ec825e 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,7 +18,7 @@
#include "differential_drive.h"
#define WHEEL_RADIUS 0.049245
-#define SHAFT_WIDTH 0.259
+#define SHAFT_WIDTH 0.150
PROC_DEFINE_STACK(stack_ind, KERN_MINSTACKSIZE * 2);
@@ -64,17 +64,18 @@ static void init(void)
params.l0[0] = 0.0236;
params.l0[1] = 3.9715;
params.T = 0.005;
- // Initialize left motor
- params.motor = MOTOR3;
- params.encoder = ENCODER3;
- 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(), CONTROLLER_MODE_NORMAL);
+ // Initialize left motor
+ params.motor = MOTOR3;
+ params.encoder = ENCODER3;
+ params.encoder_gain = 2.0*M_PI/2000.0/15.0; // Left motor is reversed
+ mc_new_controller(¶ms, dd_get_left_wheel_generator(), CONTROLLER_MODE_NORMAL);
// Start odometry
- odometryInit(1e-3, WHEEL_RADIUS, SHAFT_WIDTH, -2.0*M_PI/2000.0/15.0);
+ odometryInit(1e-3, WHEEL_RADIUS, SHAFT_WIDTH, 2.0*M_PI/2000.0/15.0, -2.0*M_PI/2000.0/15.0);
// Blink to say we are ready
for (uint8_t i=0; i < 5; i++) {
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 ab0f7eb..3c26212 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
@@ -13,7 +13,7 @@ PROC_DEFINE_STACK(stack_odometry, KERN_MINSTACKSIZE * 8);
typedef struct {
robot_state_t robot_state;
- float wheel_radius, shaft_width, encoder_gain;
+ float wheel_radius, shaft_width, left_encoder_gain, right_encoder_gain;
float Ts;
uint8_t enable;
uint8_t running;
@@ -23,7 +23,7 @@ odometry_state_t state;
static void NORETURN odometry_process(void);
-void odometryInit(float Ts, float wheel_radius, float shaft_width, float encoder_gain) {
+void odometryInit(float Ts, float wheel_radius, float shaft_width, float left_encoder_gain, float right_encoder_gain) {
// Initialize initial state
state.robot_state.x = 0.;
@@ -33,7 +33,8 @@ void odometryInit(float Ts, float wheel_radius, float shaft_width, float encoder
// Initialize robot parameters
state.wheel_radius = wheel_radius;
state.shaft_width = shaft_width;
- state.encoder_gain = encoder_gain;
+ state.left_encoder_gain = left_encoder_gain;
+ state.right_encoder_gain = right_encoder_gain;
state.Ts = Ts;
// Start odometry process
@@ -95,8 +96,8 @@ static void NORETURN odometry_process(void) {
if (delta_r > 0)
delta_r = delta_r - 65535;
}
- delta_l *= state.encoder_gain;
- delta_r *= state.encoder_gain;
+ delta_l *= state.left_encoder_gain;
+ delta_r *= state.right_encoder_gain;
last_pos_l = pos_l;
last_pos_r = pos_r;
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 0e780a1..2d378a0 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
@@ -20,7 +20,7 @@ typedef struct {
float theta;
} robot_state_t;
-void odometryInit(float Ts, float wheel_radius, float shaft_width, float encoder_gain);
+void odometryInit(float Ts, float wheel_radius, float shaft_width, float left_encoder_gain, float right_encoder_gain);
void odo_disable(void);
void odo_restart(void);
hooks/post-receive
--
krobot
|
