Menu
▾
▴
[Krobot-commits] [SCM] krobot branch, master, updated. a23abb45064d154e262a1e82a3112976f082950e
|
From: Xavier L. <Ba...@us...> - 2011-04-20 19:01:41
|
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 a23abb45064d154e262a1e82a3112976f082950e (commit)
from 5f3e029aaab36e94eb061e83d3a810f3c6c3adce (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 a23abb45064d154e262a1e82a3112976f082950e
Author: Xavier Lagorce <Xav...@cr...>
Date: Wed Apr 20 21:00:18 2011 +0200
[Controller_Motor_STM32] Send current position on the spline when moving
-----------------------------------------------------------------------
Changes:
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/can_messages.h b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/can_messages.h
index 9980947..e7d2cd6 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/can_messages.h
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/can_messages.h
@@ -68,6 +68,7 @@ typedef struct {
int16_t x __attribute__((__packed__)); // X position in mm (fixed point representation...)
int16_t y __attribute__((__packed__)); // Y position in mm
int16_t theta __attribute__((__packed__)); // angle in 1/10000 radians
+ uint8_t u; // Parameter on the spline between 0 and 255
uint8_t state; // 1 if trajectory in progress, 0 else
} ghost_msg_t;
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 9a86272..5f60fcb 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
@@ -51,6 +51,7 @@ static void NORETURN canMonitor_process(void) {
status_can_msg_t status_msg;
can_tx_frame txm;
robot_state_t odometry;
+ float u;
Timer timer_can;
// Initialize constant parameters of TX frame
@@ -117,10 +118,11 @@ static void NORETURN canMonitor_process(void) {
timer_add(&timer_can);
// Sending ghost state
- msg_ghost.data.state = dd_get_ghost_state(&odometry);
+ msg_ghost.data.state = dd_get_ghost_state(&odometry, &u);
msg_ghost.data.x = (int16_t)(odometry.x * 1000.0);
msg_ghost.data.y = (int16_t)(odometry.y * 1000.0);
msg_ghost.data.theta = (int16_t)(odometry.theta * 1000.0);
+ msg_ghost.data.u = (uint8_t)(u * 255);
txm.data32[0] = msg_ghost.data32[0];
txm.data32[1] = msg_ghost.data32[1];
txm.eid = CAN_MSG_GHOST;
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/differential_drive.c b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/differential_drive.c
index c22d8c8..2a4b2c5 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/differential_drive.c
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/differential_drive.c
@@ -27,7 +27,7 @@ typedef struct {
uint8_t initialized, enabled, running, working;
float wheel_radius, shaft_width;
float last_lin_acceleration, last_rot_acceleration;
- float v_max, at_max, ar_max;
+ float u, v_max, at_max, ar_max;
command_generator_t left_wheel_speed, right_wheel_speed;
command_generator_t left_wheel, right_wheel;
uint8_t current_traj;
@@ -44,7 +44,7 @@ static void NORETURN traj_following_process(void) {
Timer timer;
uint8_t next_traj, ui;
robot_state_t rs;
- float cr, u, v_lin, v_ratio, v_max, v_rot, dxu, dyu;
+ float cr, v_lin, v_ratio, v_max, v_rot, dxu, dyu;
float z1, z2, z3, w1, w2, u1, u2, dt;
dd_bezier_traj_t *traj;
int32_t last_time, cur_time;
@@ -68,7 +68,7 @@ static void NORETURN traj_following_process(void) {
if (!params.working && params.trajs[next_traj].initialized) {
LED2_ON();
params.working = 1;
- u = 0;
+ params.u = 0;
ui = 0;
params.current_traj = next_traj;
traj = ¶ms.trajs[params.current_traj];
@@ -84,7 +84,7 @@ static void NORETURN traj_following_process(void) {
odo_getState(&rs);
// Stop following the trajectory if we are close enough to our goal
- if (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)) {
+ 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)) {
//if (((rs.x-traj->goal[0]) * (rs.x-traj->goal[0]) + (rs.y-traj->goal[1]) * (rs.y-traj->goal[1])) <= (0.01*0.01)) {
LED2_OFF();
params.working = 0;
@@ -98,13 +98,13 @@ static void NORETURN traj_following_process(void) {
} else {
// We are following a trajectory, let's do it
// Compute ghost vehicule parameters
- cr = bezier_cr(u, traj->dparams, traj->ddparams);
- for (; ui*traj->du <= u; ui++);
+ cr = bezier_cr(params.u, traj->dparams, traj->ddparams);
+ for (; ui*traj->du <= params.u; ui++);
if (ui*traj->du > 1.0)
ui--;
if (ui > 0) {
v_lin = traj->v_tab[ui-1] +
- (traj->v_tab[ui]-traj->v_tab[ui-1]) * (u - traj->du*(ui-1))/traj->du;
+ (traj->v_tab[ui]-traj->v_tab[ui-1]) * (params.u - traj->du*(ui-1))/traj->du;
} else {
v_lin = traj->v_tab[0];
}
@@ -123,9 +123,9 @@ static void NORETURN traj_following_process(void) {
// Evolution of the ghost vehicule state
cur_time = ticks_to_us(timer_clock());
dt = (cur_time - last_time) * 1e-6;
- dxu = bezier_apply(traj->dparams[0], u);
- dyu = bezier_apply(traj->dparams[1], u);
- u += v_lin/sqrtf(dxu*dxu+dyu*dyu)*dt;
+ dxu = bezier_apply(traj->dparams[0], params.u);
+ dyu = bezier_apply(traj->dparams[1], params.u);
+ params.u += v_lin/sqrtf(dxu*dxu+dyu*dyu)*dt;
//if (u >= 1.0) {
// u = 1.0;
//} else {
@@ -349,10 +349,11 @@ uint8_t dd_add_bezier(float x_end, float y_end, float d1, float d2, float end_an
}
}
-uint8_t dd_get_ghost_state(robot_state_t *state) {
+uint8_t dd_get_ghost_state(robot_state_t *state, float *u) {
state->x = params.ghost_state.x;
state->y = params.ghost_state.y;
state->theta = params.ghost_state.theta;
+ *u = params.u;
if (params.working == 1)
return DD_GHOST_MOVING;
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/differential_drive.h b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/differential_drive.h
index 93fc4ef..96a8241 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/differential_drive.h
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/differential_drive.h
@@ -98,11 +98,12 @@ uint8_t dd_add_bezier(float x_end, float y_end, float d1, float d2, float end_an
/*
* Return the current state of the followed ghost robot
* - state : pointer to a robot_state_t structure where the ghost state will be written
+ * - u : pointer to a float in which to write the current value of the parameter on the spline
*
* return value :
* - DD_GHOST_MOVING : if a trajectory is currently followed
* - DD_GHOST_STOPPED : if the ghost robot is stopped
*/
-uint8_t dd_get_ghost_state(robot_state_t *state);
+uint8_t dd_get_ghost_state(robot_state_t *state, float *u);
#endif /* __DIFFERENTIAL_DRIVE_H */
hooks/post-receive
--
krobot
|
