Menu
▾
▴
[Krobot-commits] [SCM] krobot branch, master, updated. 04302e1607bd38ec213b2012d1af04fdc1e50433
|
From: Xavier L. <Ba...@us...> - 2013-04-25 21:48: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 04302e1607bd38ec213b2012d1af04fdc1e50433 (commit)
via b67c7679c92f48a88ed479022112fffcf3695bdc (commit)
from 4ea1cf29d38266053fb64d3a0279213d3f6e02d3 (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 04302e1607bd38ec213b2012d1af04fdc1e50433
Author: Xavier Lagorce <Xav...@cr...>
Date: Thu Apr 25 23:43:56 2013 +0200
[ControllerMotorSTM32] Use new bezier limitations with saturation of rotational speed
Also changed the way the rotational speed is computed (geometrically -> cleaner)
commit b67c7679c92f48a88ed479022112fffcf3695bdc
Author: Xavier Lagorce <Xav...@cr...>
Date: Thu Apr 25 18:23:08 2013 +0200
[control2011] Added limitation of rotational speed in bezier limits CAN message and associated commands
* Added support of the new rotational speed in vm
* Added support of the new rotational speed in viewer
* Added support of the new rotational speed in simulator
-----------------------------------------------------------------------
Changes:
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 0b2cc5d..b125c63 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
@@ -145,6 +145,7 @@ typedef struct {
// Modify Bezier Spline trajectory generation limits
typedef struct {
uint16_t v_max __attribute__((__packed__)); // max linear speed in mm/s
+ uint16_t omega_max __attribute__((__packed__)); // max rotational speed in mrad/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;
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 f9c90c2..4429200 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
@@ -277,6 +277,7 @@ static void NORETURN canMonitorListen_process(void) {
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.omega_max/1000.0,
bezier_limits_msg.data.at_max/1000.0,
bezier_limits_msg.data.ar_max/1000.0);
break;
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/lib/bezier_utils.c b/elec/boards/Controller_Motor_STM32/Firmwares/lib/bezier_utils.c
index 83fe983..0b14d94 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/lib/bezier_utils.c
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/lib/bezier_utils.c
@@ -34,7 +34,8 @@ void bezier_generate(float params[2][4],
}
void bezier_velocity_profile(float dparams[2][4], float ddparams[2][4],
- float v_max, float at_max, float ar_max,
+ float v_max, float omega_max,
+ float at_max, float ar_max,
float v_ini, float v_end,
float du, float* v_tab) {
float vmins[4], cr, cr_p, cr_pp, u, vm, dt, nv, dx, dy, dsu;
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/lib/bezier_utils.h b/elec/boards/Controller_Motor_STM32/Firmwares/lib/bezier_utils.h
index 2aa45fd..dcf67e1 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/lib/bezier_utils.h
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/lib/bezier_utils.h
@@ -34,7 +34,8 @@ void bezier_generate(float params[2][4],
* contraints
*/
void bezier_velocity_profile(float dparams[2][4], float ddparams[2][4],
- float v_max, float at_max, float ar_max,
+ float v_max, float omega_max,
+ float at_max, float ar_max,
float v_ini, float v_end,
float du, float* v_tab);
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 f2b0444..a9fe843 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/lib/differential_drive.c
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/lib/differential_drive.c
@@ -28,7 +28,7 @@ typedef struct {
uint8_t odometry_process;
float left_wheel_radius, right_wheel_radius, shaft_width;
float last_lin_acceleration, last_rot_acceleration;
- float u, v_max, at_max, ar_max;
+ float u, v_max, omega_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;
@@ -45,7 +45,7 @@ static void NORETURN traj_following_process(void) {
Timer timer;
uint8_t next_traj, ui;
robot_state_t rs;
- float cr, v_ratio, v_max, v_rot, dxu, dyu;
+ float v_rot, dxu, dyu, ddxu, ddyu, dsu2, dsu, dthetau;
float z1, z2, z3, w1, w2, u1, u2, dt;
dd_bezier_traj_t *traj;
int32_t last_time, cur_time;
@@ -96,7 +96,7 @@ static void NORETURN traj_following_process(void) {
} else {
// We are following a trajectory, let's do it
// Compute ghost vehicule parameters
- cr = bezier_cr(params.u, traj->dparams, traj->ddparams);
+ //cr = bezier_cr(params.u, traj->dparams, traj->ddparams);
for (; ui*traj->du <= params.u; ui++);
if (ui*traj->du > 1.0)
ui--;
@@ -106,24 +106,26 @@ static void NORETURN traj_following_process(void) {
} else {
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)))*traj->v_lin;
- if (cr >= 0) {
- v_rot = v_max * (1 - 1/v_ratio) / params.shaft_width;
- } else {
- v_rot = v_max * (v_ratio - 1) / params.shaft_width;
+ dxu = bezier_apply(traj->dparams[0], params.u);
+ dyu = bezier_apply(traj->dparams[1], params.u);
+ ddxu = bezier_apply(traj->ddparams[0], params.u);
+ ddyu = bezier_apply(traj->ddparams[1], params.u);
+ dsu2 = dxu*dxu+dyu*dyu;
+ dsu = sqrtf(dsu2);
+ dthetau = (ddyu*dxu - ddxu*dyu) / dsu2;
+ v_rot = dthetau * traj->v_lin / dsu;
+ if (v_rot > params.omega_max) {
+ v_rot = params.omega_max;
+ traj->v_lin = v_rot * dsu / dthetau;
+ } else if (v_rot < -params.omega_max) {
+ v_rot = -params.omega_max;
+ traj->v_lin = v_rot * dsu / dthetau;
}
// 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], params.u);
- dyu = bezier_apply(traj->dparams[1], params.u);
- params.u += traj->v_lin/sqrtf(dxu*dxu+dyu*dyu)*dt;
+ params.u += traj->v_lin/dsu*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);
@@ -346,7 +348,8 @@ uint8_t dd_add_bezier(float x_end, float y_end, float d1, float d2, float end_an
bezier_diff(traj->dparams, traj->ddparams);
// Compute velocity profile
bezier_velocity_profile(traj->dparams, traj->ddparams,
- params.v_max, params.at_max, params.ar_max,
+ params.v_max, params.omega_max,
+ params.at_max, params.ar_max,
v_ini, end_speed,
0.01, traj->v_tab);
traj->du = 0.01;
@@ -371,8 +374,9 @@ 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) {
+void dd_adjust_limits(float v_max, float omega_max, float at_max, float ar_max) {
params.v_max = v_max;
+ params.omega_max = omega_max;
params.at_max = at_max;
params.ar_max = ar_max;
}
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 35d2388..8be953d 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/lib/differential_drive.h
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/lib/differential_drive.h
@@ -111,10 +111,11 @@ void dd_interrupt_trajectory(float rot_acc, float lin_acc);
/* Change limitations of the trajectory follower
* - v_max : maximum linear speed (in m/s)
+ * - omega_max : maximim rotational speed (in rad/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);
+void dd_adjust_limits(float v_max, float omega_max, float at_max, float ar_max);
/*
* Return the current state of the followed ghost robot
diff --git a/info/control2011/src/lib/krobot_action.ml b/info/control2011/src/lib/krobot_action.ml
index 8507d79..58bbfd6 100644
--- a/info/control2011/src/lib/krobot_action.ml
+++ b/info/control2011/src/lib/krobot_action.ml
@@ -15,7 +15,7 @@ type t =
| Stop
| Think
| Goto of vertice * vector option
- | Set_limits of float * float * float
+ | Set_limits of float * float * float * float
| Follow_path of vertice list * vector option * bool
| Bezier of float * vertice * vertice * vertice * vertice * float
| Set_curve of Bezier.curve option
@@ -66,8 +66,8 @@ let rec to_string = function
"Think"
| Goto (v,vect) ->
sprintf "Goto %s %s" (string_of_vertice v) (string_of_option string_of_vector vect)
- | Set_limits (vmax,atan_max, arad_max) ->
- sprintf "Set_limits(%f, %f, %f)" vmax atan_max arad_max
+ | Set_limits (vmax,omega_max,atan_max, arad_max) ->
+ sprintf "Set_limits(%f, %f, %f, %f)" vmax omega_max atan_max arad_max
| Set_led (_,_) -> "Set_led"
| Follow_path (l,vect, correct) ->
sprintf "Follow_path [%s, %s, %b]"
diff --git a/info/control2011/src/lib/krobot_action.mli b/info/control2011/src/lib/krobot_action.mli
index e5c9f04..3722d9f 100644
--- a/info/control2011/src/lib/krobot_action.mli
+++ b/info/control2011/src/lib/krobot_action.mli
@@ -25,7 +25,7 @@ type t =
if the bool parameter is true, the path is inverted according
to the robot team *)
- | Set_limits of float * float * float
+ | Set_limits of float * float * float * float
(** limit the speed *)
(* TODO: en faire un node pour pouvoir revenir a des limites normales en sortant
diff --git a/info/control2011/src/lib/krobot_message.ml b/info/control2011/src/lib/krobot_message.ml
index 358c399..95b224b 100644
--- a/info/control2011/src/lib/krobot_message.ml
+++ b/info/control2011/src/lib/krobot_message.ml
@@ -50,7 +50,7 @@ type t =
| Motor_command of int * int
| Motor_activation of int * bool
| Motor_stop of float * float
- | Motor_bezier_limits of float * float * float
+ | Motor_bezier_limits of float * float * float * float
| Odometry of float * float * float
| Odometry_indep of float * float * float
| Odometry_ghost of float * float * float * int * bool
@@ -197,10 +197,10 @@ let to_string = function
sprintf
"Motor_stop(%f, %f)"
lin_acc rot_acc
- | Motor_bezier_limits(v_max, a_tan_max, a_rad_max) ->
+ | Motor_bezier_limits(v_max, omega_max, a_tan_max, a_rad_max) ->
sprintf
- "Motor_bezier_limits(%f, %f, %f)"
- v_max a_tan_max a_rad_max
+ "Motor_bezier_limits(%f, %f, %f, %f)"
+ v_max omega_max a_tan_max a_rad_max
| Motor_command(motor_id, speed) ->
sprintf
"Motor_command(%d, %d)"
@@ -634,14 +634,16 @@ let encode = function
~remote:false
~format:F29bits
~data
- | Motor_bezier_limits(v_max, a_tan_max, a_rad_max) ->
+ | Motor_bezier_limits(v_max, omega_max, a_tan_max, a_rad_max) ->
let v_max = v_max *. 1000. in
+ let omega_max = omega_max *. 1000. in
let a_tan_max = a_tan_max *. 1000. in
let a_rad_max = a_rad_max *. 1000. in
- let data = String.create 6 in
+ let data = String.create 8 in
put_uint16 data 0 (int_of_float v_max);
- put_uint16 data 2 (int_of_float a_tan_max);
- put_uint16 data 4 (int_of_float a_rad_max);
+ put_uint16 data 2 (int_of_float omega_max);
+ put_uint16 data 4 (int_of_float a_tan_max);
+ put_uint16 data 6 (int_of_float a_rad_max);
frame
~identifier:207
~kind:Data
@@ -786,7 +788,8 @@ let decode frame =
Motor_bezier_limits
(float (get_uint16 frame.data 0) /. 1000.,
float (get_uint16 frame.data 2) /. 1000.,
- float (get_uint16 frame.data 4) /. 1000.)
+ float (get_uint16 frame.data 4) /. 1000.,
+ float (get_uint16 frame.data 6) /. 1000.)
| 208 ->
Motor_command
(get_uint8 frame.data 0,
diff --git a/info/control2011/src/lib/krobot_message.mli b/info/control2011/src/lib/krobot_message.mli
index 5486768..3327b98 100644
--- a/info/control2011/src/lib/krobot_message.mli
+++ b/info/control2011/src/lib/krobot_message.mli
@@ -96,9 +96,10 @@ type t =
- [lin_acc] in m/s^2
- [rot_acc] in rad/s^2
*)
- | Motor_bezier_limits of float * float * float
- (** [Motor_bezier_limits(v_max, a_tan_max, a_rad_max)]
+ | Motor_bezier_limits of float * float * float * float
+ (** [Motor_bezier_limits(v_max, omega_max, a_tan_max, a_rad_max)]
- [v_max] in m/s
+ - [omega_max] in rad/s
- [a_tan_max] in m/s^2
- [a_rad_max] in m/s^2 *)
| Odometry of float * float * float
diff --git a/info/control2011/src/tools/krobot_homologation.ml b/info/control2011/src/tools/krobot_homologation.ml
index c0d7dcf..2f5c4e7 100644
--- a/info/control2011/src/tools/krobot_homologation.ml
+++ b/info/control2011/src/tools/krobot_homologation.ml
@@ -62,7 +62,7 @@ let strat_base =
Set_led(`Green,false);
Reset_odometry `Auto;
Wait_for_odometry_reset `Auto;
- Set_limits (0.3,1.0,1.0);
+ Set_limits (0.3,3.14,1.0,1.0);
End;
]
diff --git a/info/control2011/src/tools/krobot_simulator.ml b/info/control2011/src/tools/krobot_simulator.ml
index f32d5cf..2857a59 100644
--- a/info/control2011/src/tools/krobot_simulator.ml
+++ b/info/control2011/src/tools/krobot_simulator.ml
@@ -71,8 +71,8 @@ type command =
(* [Turn(t_acc, velocity)] *)
| Move of float * float
(* [Move(t_acc, velocity)] *)
- | Bezier_cmd of (float * float * float) * bool
- (** [Motor_bezier_limits(v_max, a_tan_max, a_rad_max)] *)
+ | Bezier_cmd of (float * float * float * float) * bool
+ (** [Motor_bezier_limits(v_max, omega_max, a_tan_max, a_rad_max)] *)
(* Type of simulators. *)
type simulator = {
@@ -102,7 +102,7 @@ type simulator = {
(* The start time of the current command. *)
mutable command_end : float;
(* The end time of the current command. *)
- mutable bezier_limits : float * float * float;
+ mutable bezier_limits : float * float * float * float;
}
(* +-----------------------------------------------------------------+
@@ -177,13 +177,22 @@ let velocities sim dt =
sim.command <- Idle;
(0., 0.)
| Some curve ->
- let (v_max,_,a_r_max) = limits in
+ let (v_max,omega_max,_,a_r_max) = limits in
let s' = norm (Bezier.dt curve sim.bezier_u) in
let { vx = x'; vy = y' } = Bezier.dt curve sim.bezier_u in
let { vx = x'';vy = y''} = Bezier.ddt curve sim.bezier_u in
let theta' = ( y'' *. x' -. x'' *. y' ) /. ( x' *. x' +. y' *. y' ) in
let cr = (x'*.x'+.y'*.y') ** 1.5 /. (x'*.y''-.y'*.x'') in
let vel = min v_max (sqrt (a_r_max *. (abs_float cr))) in
+ let omega = theta' *. vel /. s' in
+ let vel, omega =
+ if omega > omega_max then
+ omega_max *. s' /. theta', omega_max
+ else if omega < -. omega_max then
+ -. omega_max *. s' /. theta', -. omega_max
+ else
+ vel, omega
+ in
sim.bezier_u <- sim.bezier_u +. vel /. s' *. dt;
if dir then
(vel, theta' *. vel /. s')
@@ -397,8 +406,8 @@ let handle_message bus (timestamp, message) =
sim.state_indep <- { x; y; theta }
| Set_odometry_indep(x, y, theta) ->
sim.state <- { x; y; theta }
- | Motor_bezier_limits(v_max, a_tan_max, a_rad_max) ->
- sim.bezier_limits <- (v_max, a_tan_max, a_rad_max)
+ | Motor_bezier_limits(v_max, omega_max, a_tan_max, a_rad_max) ->
+ sim.bezier_limits <- (v_max, omega_max, a_tan_max, a_rad_max)
| Motor_bezier(x_end, y_end, d1, d2, theta_end, v_end) ->
Lwt_unix.run (Lwt_log.info_f "received: bezier(%f, %f, %f, %f, %f, %f)" x_end y_end d1 d2 theta_end v_end);
bezier sim (x_end, y_end, d1, d2, theta_end, v_end)
@@ -581,7 +590,7 @@ lwt () =
command_start = 0.;
command_end = 0.;
time = Unix.gettimeofday ();
- bezier_limits = 1., 1., 1.;
+ bezier_limits = 0.5, 3.14, 1., 1.;
} in
sim := Some local_sim;
diff --git a/info/control2011/src/tools/krobot_viewer.ml b/info/control2011/src/tools/krobot_viewer.ml
index 900d9b3..897b658 100644
--- a/info/control2011/src/tools/krobot_viewer.ml
+++ b/info/control2011/src/tools/krobot_viewer.ml
@@ -854,13 +854,14 @@ lwt () =
let send_motor_limit () =
let v_max = ui#v_max#adjustment#value in
+ let omega_max = ui#omega_max#adjustment#value in
let a_tan_max = ui#a_tan_max#adjustment#value in
let a_rad_max = ui#a_rad_max#adjustment#value in
ignore (Krobot_bus.send viewer.bus
(Unix.gettimeofday (),
CAN (Info,
Krobot_message.encode
- (Motor_bezier_limits (v_max, a_tan_max, a_rad_max))))) in
+ (Motor_bezier_limits (v_max, omega_max, a_tan_max, a_rad_max))))) in
ignore
(ui#v_max#connect#value_changed
diff --git a/info/control2011/src/tools/krobot_viewer_ui.glade b/info/control2011/src/tools/krobot_viewer_ui.glade
index 2dc2953..c7f5eaf 100644
--- a/info/control2011/src/tools/krobot_viewer_ui.glade
+++ b/info/control2011/src/tools/krobot_viewer_ui.glade
@@ -1180,6 +1180,48 @@
</packing>
</child>
<child>
+ <widget class="GtkVBox" id="omega_max_box">
+ <property name="visible">True</property>
+ <property name="can_focus">False</property>
+ <child>
+ <widget class="GtkLabel" id="omega_max_label">
+ <property name="visible">True</property>
+ <property name="can_focus">False</property>
+ <property name="label" translatable="yes">max omega</property>
+ </widget>
+ <packing>
+ <property name="expand">True</property>
+ <property name="fill">True</property>
+ <property name="position">0</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkSpinButton" id="omega_max">
+ <property name="visible">True</property>
+ <property name="can_focus">True</property>
+ <property name="invisible_char">●</property>
+ <property name="primary_icon_activatable">False</property>
+ <property name="secondary_icon_activatable">False</property>
+ <property name="primary_icon_sensitive">True</property>
+ <property name="secondary_icon_sensitive">True</property>
+ <property name="adjustment">3.14 0.01 6.28 0.10000000000000001 1 0</property>
+ <property name="digits">2</property>
+ <property name="numeric">True</property>
+ </widget>
+ <packing>
+ <property name="expand">True</property>
+ <property name="fill">True</property>
+ <property name="position">1</property>
+ </packing>
+ </child>
+ </widget>
+ <packing>
+ <property name="expand">True</property>
+ <property name="fill">True</property>
+ <property name="position">1</property>
+ </packing>
+ </child>
+ <child>
<widget class="GtkVBox" id="a_tan_max_box">
<property name="visible">True</property>
<property name="can_focus">False</property>
@@ -1218,7 +1260,7 @@
<packing>
<property name="expand">True</property>
<property name="fill">True</property>
- <property name="position">1</property>
+ <property name="position">2</property>
</packing>
</child>
<child>
@@ -1260,7 +1302,7 @@
<packing>
<property name="expand">True</property>
<property name="fill">True</property>
- <property name="position">2</property>
+ <property name="position">3</property>
</packing>
</child>
</widget>
diff --git a/info/control2011/src/tools/krobot_vm.ml b/info/control2011/src/tools/krobot_vm.ml
index 015573d..2ee0244 100644
--- a/info/control2011/src/tools/krobot_vm.ml
+++ b/info/control2011/src/tools/krobot_vm.ml
@@ -438,9 +438,9 @@ let rec exec robot actions =
| Can c ::rest ->
ignore (Lwt_log.info_f "Can");
(rest, Send_frame[c])
- | Set_limits(vmax,atan_max,arad_max) :: rest ->
+ | Set_limits(vmax,omega_max,atan_max,arad_max) :: rest ->
ignore (Lwt_log.info_f "Set_limit");
- (rest, Send[Motor_bezier_limits(vmax,atan_max,arad_max)])
+ (rest, Send[Motor_bezier_limits(vmax,omega_max,atan_max,arad_max)])
| Set_led(led,value) :: rest ->
ignore (Lwt_log.info_f "Set_led");
let led = match led with
hooks/post-receive
--
krobot
|
