Menu
▾
▴
[Krobot-commits] [SCM] krobot branch, master, updated. edd099fd4078acee928803c9e63934c83b783f19
|
From: Xavier L. <Ba...@us...> - 2011-04-21 13:25:18
|
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 edd099fd4078acee928803c9e63934c83b783f19 (commit)
via ad9957749e6fc00a602866bcc19928c9c3393d28 (commit)
from 362646a87e0f22370a259e4d0ce0bf13c2f707bb (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 edd099fd4078acee928803c9e63934c83b783f19
Author: Xavier Lagorce <Xav...@cr...>
Date: Thu Apr 21 15:23:00 2011 +0200
[Controller_Motor_STM32] Added a function to stop following the current trajectory.
This function can be used to stop following the current trajectory and delete
other queued trajectories.
The code has been added in the control program to use this ability when the
stop button is pressed in the viewer.
(note that the real robot will brake with a certain acceleration but that the
simulator will instantly stop.)
commit ad9957749e6fc00a602866bcc19928c9c3393d28
Author: Xavier Lagorce <Xav...@cr...>
Date: Thu Apr 21 14:48:03 2011 +0200
[Controller_Motor_STM32] Store current linear velocity in trajectory structure for futur use.
-----------------------------------------------------------------------
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 e7d2cd6..5760216 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
@@ -96,9 +96,10 @@ typedef struct {
uint16_t v_end:11 __attribute__((__packed__)); // final speed in mm/s
} bezier_msg_t;
-// Emergency stop
+// Stop Bezier Spline following and brakes
typedef struct {
- uint8_t stop; // stop everything
+ float lin_acc __attribute__((__packed__)); // Linear acceleration for braking
+ float rot_acc __attribute__((__packed__)); // Rotational acceleration for braking
} stop_msg_t;
// Select robot mode (normal or HIL)
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 5f60fcb..22fa8c1 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
@@ -193,10 +193,7 @@ static void NORETURN canMonitorListen_process(void) {
case CAN_MSG_STOP:
stop_msg.data32[0] = frame.data32[0];
stop_msg.data32[1] = frame.data32[1];
- if (stop_msg.data.stop == 1) {
- mc_delete_controller(MOTOR3);
- mc_delete_controller(MOTOR4);
- }
+ dd_interrupt_trajectory(stop_msg.data.rot_acc, stop_msg.data.lin_acc);
break;
case CAN_MSG_ODOMETRY_SET:
odometry_msg.data32[0] = frame.data32[0];
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 578cbdd..7883f9a 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
@@ -19,7 +19,7 @@ typedef struct {
uint8_t initialized, enabled;
float params[2][4], dparams[2][4], ddparams[2][4];
float du, v_tab[101];
- float goal[2], v_end, theta_end;
+ float goal[2], v_end, theta_end, v_lin;
float start[2], theta_ini;
} dd_bezier_traj_t;
@@ -44,7 +44,7 @@ static void NORETURN traj_following_process(void) {
Timer timer;
uint8_t next_traj, ui;
robot_state_t rs;
- float cr, v_lin, v_ratio, v_max, v_rot, dxu, dyu;
+ float cr, 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;
@@ -100,17 +100,17 @@ static void NORETURN traj_following_process(void) {
if (ui*traj->du > 1.0)
ui--;
if (ui > 0) {
- v_lin = traj->v_tab[ui-1] +
+ traj->v_lin = traj->v_tab[ui-1] +
(traj->v_tab[ui]-traj->v_tab[ui-1]) * (params.u - traj->du*(ui-1))/traj->du;
} else {
- v_lin = traj->v_tab[0];
+ traj->v_lin = traj->v_tab[0];
}
if (isnan(cr) || isinf(cr)) {
v_ratio = 1.0;
} else {
v_ratio = (cr + params.shaft_width/2.0) / (cr - params.shaft_width/2.0);
}
- v_max = 2/(1+MIN(fabsf(v_ratio), fabsf(1.0/v_ratio)))*v_lin;
+ v_max = 2/(1+MIN(fabsf(v_ratio), fabsf(1.0/v_ratio)))*traj->v_lin;
if (cr >= 0) {
v_rot = v_max * (1 - 1/v_ratio) / params.shaft_width;
} else {
@@ -122,9 +122,9 @@ static void NORETURN traj_following_process(void) {
dt = (cur_time - last_time) * 1e-6;
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;
- params.ghost_state.x += v_lin*cosf(params.ghost_state.theta)*dt;
- params.ghost_state.y += v_lin*sinf(params.ghost_state.theta)*dt;
+ 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);
if (params.ghost_state.theta > M_PI) {
params.ghost_state.theta -= 2*M_PI;
@@ -137,10 +137,10 @@ static void NORETURN traj_following_process(void) {
z2=-(rs.x-params.ghost_state.x)*sinf(params.ghost_state.theta)+(rs.y-params.ghost_state.y)*cosf(params.ghost_state.theta);
z3=tanf(rs.theta-params.ghost_state.theta);
- w1=-params.k1*fabsf(v_lin)*(z1+z2*z3);
- w2=-params.k2*v_lin*z2-params.k3*fabsf(v_lin)*z3;
+ w1=-params.k1*fabsf(traj->v_lin)*(z1+z2*z3);
+ w2=-params.k2*traj->v_lin*z2-params.k3*fabsf(traj->v_lin)*z3;
- u1=(w1+v_lin)/cosf(rs.theta-params.ghost_state.theta);
+ u1=(w1+traj->v_lin)/cosf(rs.theta-params.ghost_state.theta);
u2=w2*powf(cosf(rs.theta-params.ghost_state.theta),2)+v_rot;
// Apply command
@@ -342,6 +342,21 @@ 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) {
+
+ // Prevent the controller from following the current trajectory
+ params.working = 0;
+
+ // Disable all the trajectories
+ params.trajs[0].initialized = 0;
+ params.trajs[0].enabled = 0;
+ params.trajs[1].initialized = 0;
+ params.trajs[1].enabled = 0;
+ // Brake !
+ dd_set_rotational_speed(0., rot_acc);
+ dd_set_linear_speed(0., lin_acc);
+}
+
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/Firmware/controller_motor_stm32/differential_drive.h b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/differential_drive.h
index 73d6944..0a0a0f9 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
@@ -96,6 +96,15 @@ void dd_set_rotational_speed(float speed, float acceleration);
uint8_t dd_add_bezier(float x_end, float y_end, float d1, float d2, float end_angle, float end_speed);
/*
+ * Stops the current trajectory and removes the queued ones.
+ * - rot_acc : acceleration to stop the rotational motion of the drive.
+ * - lin_acc : acceleration to stop the linear motion of the drive.
+ *
+ * If one of the accelerations if zero, the corresponding movement will be stopped abruptly
+ */
+void dd_interrupt_trajectory(float rot_acc, float lin_acc);
+
+/*
* 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
diff --git a/info/control2011/src/lib/krobot_message.ml b/info/control2011/src/lib/krobot_message.ml
index 93ee3f4..82df8a3 100644
--- a/info/control2011/src/lib/krobot_message.ml
+++ b/info/control2011/src/lib/krobot_message.ml
@@ -30,7 +30,7 @@ type t =
| Motor_move of float * float * float
| Motor_turn of float * float * float
| Motor_bezier of float * float * float * float * float * float
- | Motor_stop
+ | Motor_stop of float * float
| Odometry of float * float * float
| Odometry_ghost of float * float * float * int * bool
| Set_odometry of float * float * float
@@ -102,8 +102,10 @@ let to_string = function
sprintf
"Motor_bezier(%f, %f, %f, %f, %f, %f)"
x y d1 d2 theta v
- | Motor_stop ->
- "Motor_stop"
+ | Motor_stop(lin_acc, rot_acc) ->
+ sprintf
+ "Motor_stop(%f, %f)"
+ lin_acc rot_acc
| Odometry(x, y, theta) ->
sprintf
"Odometry(%f, %f, %f)"
@@ -296,13 +298,16 @@ let encode = function
~remote:false
~format:F29bits
~data
- | Motor_stop ->
+ | Motor_stop(lin_acc, rot_acc) ->
+ let data = String.create 8 in
+ put_float32 data 0 lin_acc;
+ put_float32 data 4 rot_acc;
frame
~identifier:204
~kind:Data
~remote:false
~format:F29bits
- ~data:"\x01"
+ ~data
| Set_controller_mode b ->
frame
~identifier:205
@@ -395,6 +400,8 @@ let decode frame =
float (get_sint16 frame.data 4) /. 10000.)
| 204 ->
Motor_stop
+ (get_float32 frame.data 0,
+ get_float32 frame.data 4)
| 205 ->
Set_controller_mode
(get_uint8 frame.data 0 <> 0)
diff --git a/info/control2011/src/lib/krobot_message.mli b/info/control2011/src/lib/krobot_message.mli
index c17cb0b..05f9e49 100644
--- a/info/control2011/src/lib/krobot_message.mli
+++ b/info/control2011/src/lib/krobot_message.mli
@@ -46,8 +46,12 @@ type t =
*)
| Motor_bezier of float * float * float * float * float * float
(** [Motor_bezier(x_end, y_end, d1, d2, theta_end, v_end)] *)
- | Motor_stop
- (** Stops the motors. *)
+ | Motor_stop of float * float
+ (** [Motor_stop(lin_acc, rot_acc)] command to stop following the
+ current Bezier Spline and the queued ones.
+ - [lin_acc] in m/s^2
+ - [rot_acc] in rad/s^2
+ *)
| Odometry of float * float * float
(** [Odometry(x, y, theta)] the position of the robot on the
table. *)
diff --git a/info/control2011/src/tools/krobot_planner.ml b/info/control2011/src/tools/krobot_planner.ml
index 176826e..5f872e5 100644
--- a/info/control2011/src/tools/krobot_planner.ml
+++ b/info/control2011/src/tools/krobot_planner.ml
@@ -336,7 +336,7 @@ let handle_message planner (timestamp, message) =
cancel planner.mover;
set_moving planner false;
set_vertices planner [];
- ignore (Krobot_message.send planner.bus (Unix.gettimeofday (), Motor_stop))
+ ignore (Krobot_message.send planner.bus (Unix.gettimeofday (), Motor_stop(1.0,0.0)))
| _ ->
()
diff --git a/info/control2011/src/tools/krobot_simulator.ml b/info/control2011/src/tools/krobot_simulator.ml
index ad0bf78..4e52d9b 100644
--- a/info/control2011/src/tools/krobot_simulator.ml
+++ b/info/control2011/src/tools/krobot_simulator.ml
@@ -210,7 +210,7 @@ lwt () =
lwt () = Lwt_log.info_f "received: turn(%f, %f, %f)" angle speed acc in
turn sim angle speed acc;
return ()
- | Motor_stop ->
+ | Motor_stop(lin_acc, rot_acc) ->
sim.command <- Idle;
return ()
| Req_motor_status ->
hooks/post-receive
--
krobot
|
