Menu
▾
▴
[Krobot-commits] [SCM] krobot branch, master, updated. 9e5759cec52e0fc9ec2c9aeaed62ef57bfdf6163
|
From: Xavier L. <Ba...@us...> - 2012-05-15 12:22:40
|
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 9e5759cec52e0fc9ec2c9aeaed62ef57bfdf6163 (commit)
via 5bde23dedda3f4fb95d49065eabc207807928e41 (commit)
via 5831326246b4e1e880010f94cc920c4f13bf0b68 (commit)
from 7e52949ac9a86441f90a1066ffa97cab74d46bc8 (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 9e5759cec52e0fc9ec2c9aeaed62ef57bfdf6163
Author: Xavier Lagorce <Xav...@cr...>
Date: Tue May 15 14:21:10 2012 +0200
[Controller_Motor_STM32] Keep the repo up to date :
* Added CAN messages to limit speed and acceleration of bezier following
* Corrected a bug in encoder wrapping detection
* Allow more than one odometry process
* New control parameters for 2012 robot
commit 5bde23dedda3f4fb95d49065eabc207807928e41
Author: Xavier Lagorce <Xav...@cr...>
Date: Tue May 15 14:11:46 2012 +0200
[Controller_Motor_STM32] Update jtag flash script
commit 5831326246b4e1e880010f94cc920c4f13bf0b68
Author: Xavier Lagorce <Xav...@cr...>
Date: Tue May 15 14:10:58 2012 +0200
Add ocaml bytecode to .gitignore
-----------------------------------------------------------------------
Changes:
diff --git a/.gitignore b/.gitignore
index 263aa3c..10f9f01 100644
--- a/.gitignore
+++ b/.gitignore
@@ -14,6 +14,7 @@
*.dmp
*.hex
*.o.d
+*.byte
setup.data
setup.log
_build
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/can_messages.h b/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/can_messages.h
index 81a44c2..1226a2b 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/can_messages.h
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/can_messages.h
@@ -31,6 +31,7 @@
#define CAN_MSG_STOP 204 // stop_can_msg_t
#define CAN_MSG_CONTROLLER_MODE 205 // controller_mode_can_msg_t
#define CAN_MSG_BEZIER_ADD 206 // bezier_can_msg_t
+#define CAN_MSG_BEZIER_LIMITS 207 // bezier_limits_can_msg_t
/* +-----------------------------------------------------------------+
| CAN messages data structures |
@@ -96,6 +97,13 @@ typedef struct {
uint16_t v_end:11 __attribute__((__packed__)); // final speed in mm/s
} bezier_msg_t;
+// Modify Bezier Spline trajectory generation limits
+typedef struct {
+ uint16_t v_max __attribute__((__packed__)); // max linear speed in mm/s
+ uint16_t at_max __attribute__((__packed__)); // max linear acceleration in mm/s/s
+ uint16_t ar_max __attribute__((__packed__)); // max radial acceleration in mm/s/s
+} bezier_limits_msg_t;
+
// Stop Bezier Spline following and brakes
typedef struct {
float lin_acc __attribute__((__packed__)); // Linear acceleration for braking
@@ -153,6 +161,11 @@ typedef union {
} bezier_can_msg_t;
typedef union {
+ bezier_limits_msg_t data;
+ uint32_t data32[2];
+} bezier_limits_can_msg_t;
+
+typedef union {
stop_msg_t data;
uint32_t data32[2];
} stop_can_msg_t;
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/can_monitor.c b/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/can_monitor.c
index 8c3793d..f41d686 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/can_monitor.c
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/can_monitor.c
@@ -13,6 +13,8 @@
#define ROBOT_MODE_NORMAL 0
#define ROBOT_MODE_HIL 1
+#define CONTROL_ODOMETRY 0
+
// Processes stacks
PROC_DEFINE_STACK(stack_can_send, KERN_MINSTACKSIZE * 8);
PROC_DEFINE_STACK(stack_can_receive, KERN_MINSTACKSIZE * 8);
@@ -79,7 +81,7 @@ static void NORETURN canMonitor_process(void) {
// Sending odometry data if not in HIL mode or motor commands if in HIL mode
if (mode != ROBOT_MODE_HIL) {
- odo_getState(&odometry);
+ odo_getState(CONTROL_ODOMETRY, &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);
@@ -152,6 +154,7 @@ static void NORETURN canMonitorListen_process(void) {
stop_can_msg_t stop_msg;
controller_mode_can_msg_t controller_mode_msg;
bezier_can_msg_t bezier_msg;
+ bezier_limits_can_msg_t bezier_limits_msg;
// Initialize constant parameters of TX frame
txm.dlc = 8;
@@ -190,6 +193,13 @@ static void NORETURN canMonitorListen_process(void) {
bezier_msg.data.d1/100.0, bezier_msg.data.d2/100.0,
bezier_msg.data.theta_end/100.0, bezier_msg.data.v_end/1000.0);
break;
+ case CAN_MSG_BEZIER_LIMITS:
+ bezier_limits_msg.data32[0] = frame.data32[0];
+ bezier_limits_msg.data32[1] = frame.data32[1];
+ dd_adjust_limits(bezier_limits_msg.data.v_max/1000.0,
+ bezier_limits_msg.data.at_max/1000.0,
+ bezier_limits_msg.data.ar_max/1000.0);
+ break;
case CAN_MSG_STOP:
stop_msg.data32[0] = frame.data32[0];
stop_msg.data32[1] = frame.data32[1];
@@ -201,7 +211,7 @@ static void NORETURN canMonitorListen_process(void) {
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);
+ odo_setState(CONTROL_ODOMETRY, &odometry);
break;
case CAN_MSG_ODOMETRY:
// We should only receive such message in HIL mode
@@ -211,7 +221,7 @@ static void NORETURN canMonitorListen_process(void) {
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);
+ odo_setState(CONTROL_ODOMETRY, &odometry);
}
break;
case CAN_MSG_CONTROLLER_MODE:
@@ -220,12 +230,12 @@ static void NORETURN canMonitorListen_process(void) {
if (controller_mode_msg.data.mode == 1) {
mc_change_mode(MOTOR3, CONTROLLER_MODE_HIL);
mc_change_mode(MOTOR4, CONTROLLER_MODE_HIL);
- odo_disable();
+ odo_disable(CONTROL_ODOMETRY);
mode = ROBOT_MODE_HIL;
} else {
mc_change_mode(MOTOR3, CONTROLLER_MODE_NORMAL);
mc_change_mode(MOTOR4, CONTROLLER_MODE_NORMAL);
- odo_restart();
+ odo_restart(CONTROL_ODOMETRY);
mode = ROBOT_MODE_NORMAL;
}
break;
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/main.c b/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/main.c
index b52b689..0f69fc0 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/main.c
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/main.c
@@ -18,10 +18,9 @@
#include "differential_drive.h"
#define WHEEL_RADIUS 0.049245
-#define SHAFT_WIDTH 0.14320
+#define SHAFT_WIDTH 0.224
-//#define WHEEL_RADIUS 0.0325
-//#define SHAFT_WIDTH 0.255
+#define CONTROL_ODOMETRY 0
PROC_DEFINE_STACK(stack_ind, KERN_MINSTACKSIZE * 2);
@@ -50,7 +49,8 @@ static void init(void)
// Start control of drive motors
tc_init();
- dd_start(WHEEL_RADIUS, SHAFT_WIDTH, // Structural parameters
+ dd_start(CONTROL_ODOMETRY, // Use odometry CONTROL_ODOMETRY for control
+ WHEEL_RADIUS, SHAFT_WIDTH, // Structural parameters
8*2*M_PI, // Absolute wheel speed limitation
0.5, // Linear velocity limitation
1.0, // Linear acceleration limitation
@@ -58,34 +58,30 @@ static void init(void)
0.4, 0.7, 1.0, // Controller gains
0.005); // Sample period
// Common parameters
- params.encoder_gain = -2.0*M_PI/2000.0/15;
+ params.encoder_gain = 2.0*M_PI/2000.0/15;
params.T = 0.005;
- // Initialize right motor
- params.G0 = 0.0146;
- params.tau = 0.120;
- params.k[0] = -2139.6;
- params.k[1] = -193.9178;
- params.l = -params.k[0];
- params.l0[0] = 0.0408;
- params.l0[1] = 0;
- params.motor = MOTOR4;
- params.encoder = ENCODER4;
- mc_new_controller(¶ms, dd_get_right_wheel_generator(), CONTROLLER_MODE_NORMAL);
// Initialize left motor
- params.G0 = 0.0146;
- params.tau = 0.266;
- params.k[0] = -4689.1;
- params.k[1] = -506.4099;
+ params.G0 = 0.011686;
+ params.tau = 0.118;
+ params.k[0] = -3735.7;
+ params.k[1] = -297.5867;
params.l = -params.k[0];
- params.l0[0] = 0.0630;
- params.l0[1] = 0.0994;
+ params.l0[0] = 0.0561;
+ params.l0[1] = 0.0108;
params.motor = MOTOR3;
params.encoder = ENCODER3;
- params.encoder_gain = 2.0*M_PI/2000.0/15; // Left motor is reversed
mc_new_controller(¶ms, dd_get_left_wheel_generator(), CONTROLLER_MODE_NORMAL);
+ // Initialize right motor
+ params.motor = MOTOR4;
+ params.encoder = ENCODER4;
+ params.encoder_gain = -2.0*M_PI/2000.0/15; // Left motor is reversed
+ 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, -2.0*M_PI/2000.0/15);
+ odometryInit(CONTROL_ODOMETRY, 1e-3, WHEEL_RADIUS, SHAFT_WIDTH, 2.0*M_PI/2000.0/15, -2.0*M_PI/2000.0/15);
+
+ // Init beacon motor
+ enableMotor(MOTOR2);
// Blink to say we are ready
for (uint8_t i=0; i < 5; i++) {
@@ -100,6 +96,9 @@ static void init(void)
LED4_OFF();
timer_delay(100);
}
+
+ // Start Beacon motor
+ //motorSetSpeed(MOTOR2, 1100);
}
static void NORETURN ind_process(void)
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/jtag/flash.cfg b/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/jtag/flash.cfg
index 2721d3e..b1c2352 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/jtag/flash.cfg
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/jtag/flash.cfg
@@ -1,7 +1,7 @@
init
reset halt
-stm32x mass_erase 0
+stm32f1x mass_erase 0
flash write_bank 0 fw.bin 0
reset init
reset run
-shutdown
\ No newline at end of file
+shutdown
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/jtag/openocd.cfg b/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/jtag/openocd.cfg
index d6a58e9..0e09b2b 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/jtag/openocd.cfg
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/jtag/openocd.cfg
@@ -77,7 +77,7 @@ target create $_TARGETNAME cortex_m3 -endian $_ENDIAN -chain-position $_TARGETNA
$_TARGETNAME configure -work-area-phys 0x20000000 -work-area-size $_WORKAREASIZE -work-area-backup 0
set _FLASHNAME $_CHIPNAME.flash
-flash bank $_FLASHNAME stm32x 0 0 0 0 $_TARGETNAME
+flash bank $_FLASHNAME stm32f1x 0 0 0 0 $_TARGETNAME
# For more information about the configuration files, take a look at:
# openocd.texi
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/lib/differential_drive.c b/elec/boards/Controller_Motor_STM32/Firmwares/lib/differential_drive.c
index b566a84..01fa1d2 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/lib/differential_drive.c
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/lib/differential_drive.c
@@ -25,6 +25,7 @@ typedef struct {
typedef struct {
uint8_t initialized, enabled, running, working;
+ uint8_t odometry_process;
float wheel_radius, shaft_width;
float last_lin_acceleration, last_rot_acceleration;
float u, v_max, at_max, ar_max;
@@ -80,7 +81,7 @@ static void NORETURN traj_following_process(void) {
}
timer_add(&timer);
if (params.working) {
- odo_getState(&rs);
+ odo_getState(params.odometry_process, &rs);
// Stop following the trajectory if we are close enough to our goal
if (params.u >= 1.0 || ((rs.x-traj->goal[0]) * (rs.x-traj->goal[0]) + (rs.y-traj->goal[1]) * (rs.y-traj->goal[1])) <= (0.01*0.01)) {
@@ -123,12 +124,15 @@ static void NORETURN traj_following_process(void) {
dxu = bezier_apply(traj->dparams[0], params.u);
dyu = bezier_apply(traj->dparams[1], params.u);
params.u += traj->v_lin/sqrtf(dxu*dxu+dyu*dyu)*dt;
- params.ghost_state.x += traj->v_lin*cosf(params.ghost_state.theta)*dt;
- params.ghost_state.y += traj->v_lin*sinf(params.ghost_state.theta)*dt;
- params.ghost_state.theta = fmodf(params.ghost_state.theta + v_rot*dt, 2*M_PI);
+ //params.ghost_state.x += traj->v_lin*cosf(params.ghost_state.theta)*dt;
+ //params.ghost_state.y += traj->v_lin*sinf(params.ghost_state.theta)*dt;
+ //params.ghost_state.theta = fmodf(params.ghost_state.theta + v_rot*dt, 2*M_PI);
+ params.ghost_state.x = bezier_apply(traj->params[0],params.u);
+ params.ghost_state.y = bezier_apply(traj->params[1],params.u);
+ params.ghost_state.theta = atan2f(dyu,dxu);
if (params.ghost_state.theta > M_PI) {
params.ghost_state.theta -= 2*M_PI;
- } else if (params.ghost_state.theta <= M_PI) {
+ } else if (params.ghost_state.theta <= -M_PI) {
params.ghost_state.theta += 2*M_PI;
}
@@ -156,9 +160,11 @@ static void NORETURN traj_following_process(void) {
}
}
-void dd_start(float wheel_radius, float shaft_width, float max_wheel_speed,
+void dd_start(uint8_t odometry_process,
+ float wheel_radius, float shaft_width, float max_wheel_speed,
float v_max, float at_max, float ar_max,
float k1, float k2, float k3, float Ts) {
+ params.odometry_process = odometry_process;
params.wheel_radius = wheel_radius;
params.shaft_width = shaft_width;
params.last_lin_acceleration = 0.0;
@@ -307,7 +313,7 @@ uint8_t dd_add_bezier(float x_end, float y_end, float d1, float d2, float end_an
y_ini = params.trajs[params.current_traj].goal[1];
theta_ini = params.trajs[params.current_traj].theta_end;
} else {
- odo_getState(&rs);
+ odo_getState(params.odometry_process, &rs);
x_ini = rs.x;
y_ini = rs.y;
theta_ini = rs.theta;
@@ -357,6 +363,12 @@ void dd_interrupt_trajectory(float rot_acc, float lin_acc) {
dd_set_linear_speed(0., lin_acc);
}
+void dd_adjust_limits(float v_max, float at_max, float ar_max) {
+ params.v_max = v_max;
+ params.at_max = at_max;
+ params.ar_max = ar_max;
+}
+
uint8_t dd_get_ghost_state(robot_state_t *state, float *u) {
if (state != NULL) {
state->x = params.ghost_state.x;
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/lib/differential_drive.h b/elec/boards/Controller_Motor_STM32/Firmwares/lib/differential_drive.h
index 0a0a0f9..df33321 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/lib/differential_drive.h
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/lib/differential_drive.h
@@ -31,6 +31,7 @@
#define DD_GHOST_MOVING 1
/* Initializes the differential drive
+ * - odometry_process : ID of the odometry process to base the control on
* - wheel_radius : radius of the wheels (in meters)
* - shaft_width : propulsion shaft width (in meters)
* - max_wheel_speed : maximum wheel speed (in rad/s)
@@ -43,7 +44,8 @@
* 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, float max_wheel_speed,
+void dd_start(uint8_t odometry_process,
+ float wheel_radius, float shaft_width, float max_wheel_speed,
float v_max, float at_max, float ar_max,
float k1, float k2, float k3, float Ts);
@@ -104,6 +106,13 @@ uint8_t dd_add_bezier(float x_end, float y_end, float d1, float d2, float end_an
*/
void dd_interrupt_trajectory(float rot_acc, float lin_acc);
+/* Change limitations of the trajectory follower
+ * - v_max : maximum linear speed (in m/s)
+ * - at_max : maximum tangential acceleration (in m/s/s)
+ * - ar_max : maximum radial acceleration (in m/s/s)
+ */
+void dd_adjust_limits(float v_max, float at_max, float ar_max);
+
/*
* Return the current state of the followed ghost robot
* - state : pointer to a robot_state_t structure where the ghost state will be written
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/lib/motor_controller.c b/elec/boards/Controller_Motor_STM32/Firmwares/lib/motor_controller.c
index 3bc54e0..6caa48f 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/lib/motor_controller.c
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/lib/motor_controller.c
@@ -178,12 +178,17 @@ static void NORETURN motorController_process(void) {
// Measure motor rotation
encoder_pos = (float)getEncoderPosition(params->encoder);
delta = encoder_pos - params->last_encoder_pos;
- if (getEncoderDirection(params->encoder) == ENCODER_DIR_UP) {
+ /*if (getEncoderDirection(params->encoder) == ENCODER_DIR_UP) {
if (delta < 0)
delta = delta + 65535;
} else {
if (delta > 0)
delta = delta - 65535;
+ }*/
+ if (delta > 32767) {
+ delta = delta - 65535;
+ } else if (delta < - 32767) {
+ delta = delta + 65535;
}
// New state vector estimation
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/lib/odometry.c b/elec/boards/Controller_Motor_STM32/Firmwares/lib/odometry.c
index 3c26212..6b33924 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/lib/odometry.c
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/lib/odometry.c
@@ -9,7 +9,8 @@
#include "odometry.h"
-PROC_DEFINE_STACK(stack_odometry, KERN_MINSTACKSIZE * 8);
+#define ODOMETRY_STACK_SIZE (KERN_MINSTACKSIZE * 8)
+static cpu_stack_t stack_odometry[MAX_ODOMETRY_PROCESSES][(ODOMETRY_STACK_SIZE + sizeof(cpu_stack_t) - 1) / sizeof(cpu_stack_t)];
typedef struct {
robot_state_t robot_state;
@@ -19,38 +20,38 @@ typedef struct {
uint8_t running;
} odometry_state_t;
-odometry_state_t state;
+odometry_state_t state[MAX_ODOMETRY_PROCESSES];
static void NORETURN odometry_process(void);
-void odometryInit(float Ts, float wheel_radius, float shaft_width, float left_encoder_gain, float right_encoder_gain) {
+void odometryInit(uint8_t process_num, float Ts, float wheel_radius, float shaft_width, float left_encoder_gain, float right_encoder_gain) {
// Initialize initial state
- state.robot_state.x = 0.;
- state.robot_state.y = 0;
- state.robot_state.theta = 0;
+ state[process_num].robot_state.x = 0.;
+ state[process_num].robot_state.y = 0;
+ state[process_num].robot_state.theta = 0;
// Initialize robot parameters
- state.wheel_radius = wheel_radius;
- state.shaft_width = shaft_width;
- state.left_encoder_gain = left_encoder_gain;
- state.right_encoder_gain = right_encoder_gain;
- state.Ts = Ts;
+ state[process_num].wheel_radius = wheel_radius;
+ state[process_num].shaft_width = shaft_width;
+ state[process_num].left_encoder_gain = left_encoder_gain;
+ state[process_num].right_encoder_gain = right_encoder_gain;
+ state[process_num].Ts = Ts;
// Start odometry process
- state.enable = 1;
- proc_new(odometry_process, NULL, sizeof(stack_odometry), stack_odometry);
+ state[process_num].enable = 1;
+ proc_new(odometry_process, (&state[process_num]), sizeof(stack_odometry[process_num]), stack_odometry[process_num]);
}
-void odo_disable(void) {
- state.enable = 0;
+void odo_disable(uint8_t process_num) {
+ state[process_num].enable = 0;
}
-void odo_restart(void) {
+void odo_restart(uint8_t process_num) {
// Start odometry process
- if (state.enable == 0 && state.running == 0) {
- state.enable = 1;
- proc_new(odometry_process, NULL, sizeof(stack_odometry), stack_odometry);
+ if (state[process_num].enable == 0 && state[process_num].running == 0) {
+ state[process_num].enable = 1;
+ proc_new(odometry_process, (&state[process_num]), sizeof(stack_odometry[process_num]), stack_odometry[process_num]);
}
}
@@ -58,21 +59,25 @@ 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;
Timer timer;
+ odometry_state_t *state;
+
+ // get data
+ state = (odometry_state_t *) proc_currentUserData();
// configure timer
- timer_setDelay(&timer, ms_to_ticks(1000 * state.Ts));
+ timer_setDelay(&timer, ms_to_ticks(1000 * state->Ts));
timer_setEvent(&timer);
// Indicate we are running
- state.running = 1;
+ 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;
+ if (state->enable == 0) {
+ state->running = 0;
proc_exit();
} else {
timer_add(&timer);
@@ -82,7 +87,17 @@ static void NORETURN odometry_process(void) {
pos_r = (float)getEncoderPosition(ENCODER4); dir_r = getEncoderDirection(ENCODER4);
delta_l = pos_l - last_pos_l;
delta_r = pos_r - last_pos_r;
- if (dir_l == ENCODER_DIR_UP) {
+ if (delta_l > 32767) {
+ delta_l = delta_l - 65535;
+ } else if (delta_l < - 32767) {
+ delta_l = delta_l + 65535;
+ }
+ if (delta_r > 32767) {
+ delta_r = delta_r - 65535;
+ } else if (delta_r < - 32767) {
+ delta_r = delta_r + 65535;
+ }
+ /*if (dir_l == ENCODER_DIR_UP) {
if (delta_l < 0)
delta_l = delta_l + 65535;
} else {
@@ -95,37 +110,37 @@ static void NORETURN odometry_process(void) {
} else {
if (delta_r > 0)
delta_r = delta_r - 65535;
- }
- delta_l *= state.left_encoder_gain;
- delta_r *= state.right_encoder_gain;
+ }*/
+ delta_l *= state->left_encoder_gain;
+ delta_r *= state->right_encoder_gain;
last_pos_l = pos_l;
last_pos_r = pos_r;
// New state computation
- 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);
+ 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;
+ 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
}
}
-void odo_setState(robot_state_t *new_state) {
- state.robot_state.x = new_state->x;
- state.robot_state.y = new_state->y;
- state.robot_state.theta = new_state->theta;
+void odo_setState(uint8_t process_num, robot_state_t *new_state) {
+ state[process_num].robot_state.x = new_state->x;
+ state[process_num].robot_state.y = new_state->y;
+ state[process_num].robot_state.theta = new_state->theta;
}
-void odo_getState(robot_state_t *robot_state) {
- robot_state->x = state.robot_state.x;
- robot_state->y = state.robot_state.y;
- robot_state->theta = state.robot_state.theta;
+void odo_getState(uint8_t process_num, robot_state_t *robot_state) {
+ robot_state->x = state[process_num].robot_state.x;
+ robot_state->y = state[process_num].robot_state.y;
+ robot_state->theta = state[process_num].robot_state.theta;
}
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/lib/odometry.h b/elec/boards/Controller_Motor_STM32/Firmwares/lib/odometry.h
index 2d378a0..cac8c83 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/lib/odometry.h
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/lib/odometry.h
@@ -14,18 +14,20 @@
#include <drv/timer.h>
#include "encoder.h"
+#define MAX_ODOMETRY_PROCESSES 2
+
typedef struct {
float x;
float y;
float theta;
} robot_state_t;
-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);
+void odometryInit(uint8_t process_num, float Ts, float wheel_radius, float shaft_width, float left_encoder_gain, float right_encoder_gain);
+void odo_disable(uint8_t process_num);
+void odo_restart(uint8_t process_num);
-void odo_setState(robot_state_t *new_state);
+void odo_setState(uint8_t process_num, robot_state_t *new_state);
-void odo_getState(robot_state_t *robot_state);
+void odo_getState(uint8_t process_num, robot_state_t *robot_state);
#endif /* __ODOMETRY_H */
hooks/post-receive
--
krobot
|
