Menu
▾
▴
[Krobot-commits] [SCM] krobot branch, master, updated. 8359d44c2c097a6a9851638a1f67524dd4d3bd2c
|
From: Xavier L. <Ba...@us...> - 2011-04-19 19:09:47
|
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 8359d44c2c097a6a9851638a1f67524dd4d3bd2c (commit)
via b4c4f022e4d1ac4d4fc0935340e488090892cdb0 (commit)
via 98a0d66534cc7697ad707c6d418496183fcc5606 (commit)
from 4f1a9587a1dba18c3bd3e3b962dacaffab58c2db (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 8359d44c2c097a6a9851638a1f67524dd4d3bd2c
Author: Xavier Lagorce <Xav...@cr...>
Date: Tue Apr 19 20:52:04 2011 +0200
[Controller_Motor_STM32] One thread to send them all.
commit b4c4f022e4d1ac4d4fc0935340e488090892cdb0
Author: Xavier Lagorce <Xav...@cr...>
Date: Tue Apr 19 16:48:12 2011 +0200
[Controller_Motor_STM32] Extended differential drive controller with Bezier Splines.
The differential drive controller can now follow some Bezier Splines. The
splines are directly generated on the ARM and only global parameters are
required.
This currently support to chain splines but not to interrupt them.
commit 98a0d66534cc7697ad707c6d418496183fcc5606
Author: Xavier Lagorce <Xav...@cr...>
Date: Tue Apr 19 16:43:00 2011 +0200
[Controller_Motor_STM32] Added a function to trajectory_controller to allow direct action on speed.
-----------------------------------------------------------------------
Changes:
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/bezier_utils.c b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/bezier_utils.c
new file mode 100644
index 0000000..0b74fde
--- /dev/null
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/bezier_utils.c
@@ -0,0 +1,122 @@
+/*
+ * bezier_utils.c
+ * --------------
+ * Copyright : (c) 2011, Xavier Lagorce <Xav...@cr...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ */
+
+#include "bezier_utils.h"
+
+float bezier_apply(float params[4], float u) {
+ return (params[0] + params[1]*u + params[2]*u*u + params[3]*u*u*u);
+}
+
+void bezier_generate(float params[2][4],
+ float p_x, float p_y,
+ float p1_x, float p1_y,
+ float p2_x, float p2_y,
+ float s_x, float s_y) {
+
+ params[0][0] = p_x;
+ params[1][0] = p_y;
+ params[0][1] = -3*p_x+3*p1_x;
+ params[1][1] = -3*p_y+3*p1_y;
+ params[0][2] = 3*p_x-6*p1_x+3*p2_x;
+ params[1][2] = 3*p_y-6*p1_y+3*p2_y;
+ params[0][3] = -p_x+3*p1_x-3*p2_x+s_x;
+ params[1][3] = -p_y+3*p1_y-3*p2_y+s_y;
+}
+
+void bezier_velocity_profile(float dparams[2][4], float ddparams[2][4],
+ float v_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;
+ int ind = 0, j, nb_pts, mins[4], im, i;
+
+ for (i=0, u=0.0; u <= 1.0; i++, u+=du) {
+ v_tab[i] = v_max;
+ }
+ nb_pts = i;
+
+ // Looking for curvature radius minima to limit speed at this positions
+ mins[ind] = 0;
+ vmins[ind] = v_ini;
+ ind++;
+
+ cr_pp = fabsf(bezier_cr(0, dparams, ddparams));
+ cr_p = fabsf(bezier_cr(du, dparams, ddparams));
+ for (i=2, u=2*du; u <= 1.0; i++, u+=du) {
+ cr = fabsf(bezier_cr(u, dparams, ddparams));
+ if ((cr >= cr_p) && (cr_p < cr_pp)) {
+ mins[ind] = i;
+ vmins[ind] = sqrtf(ar_max*cr);
+ ind++;
+ }
+ cr_pp = cr_p;
+ cr_p = cr;
+ }
+ mins[ind] = nb_pts-1;
+ vmins[ind] = v_end;
+ ind++;
+
+ // Compute speed limitations
+ for (j=0; j < ind; j++) {
+ im = mins[j];
+ vm = vmins[j];
+ if (vm < v_max) {
+ v_tab[im] = vm;
+
+ // Profile for preceding velocity
+ for (i = im-1; i >= 0; i--) {
+ dx = bezier_apply(dparams[0], du*(i+1));
+ dy = bezier_apply(dparams[1], du*(i+1));
+ dsu = sqrtf(dx*dx + dy*dy);
+ dt = (-v_tab[i+1]+sqrtf(v_tab[i+1]*v_tab[i+1]+2*at_max*du*dsu))/at_max;
+ nv = v_tab[i+1]+at_max*dt;
+ if (nv < v_tab[i]) {
+ v_tab[i] = nv;
+ } else {
+ break;
+ }
+ }
+ // Profile for following sector
+ for (i = im+1; i < nb_pts; i++) {
+ dx = bezier_apply(dparams[0], du*(i-1));
+ dy = bezier_apply(dparams[1], du*(i-1));
+ dsu = sqrtf(dx*dx + dy*dy);
+ dt = (-v_tab[i-1]+sqrtf(v_tab[i-1]*v_tab[i-1]+2*at_max*du*dsu))/at_max;
+ nv = v_tab[i-1]+at_max*dt;
+ if (nv < v_tab[i]) {
+ v_tab[i] = nv;
+ } else {
+ break;
+ }
+ }
+ } // end if (vm < v_max)
+ } // for mins
+}
+
+float bezier_cr(float u, float dparams[2][4], float ddparams[2][4]) {
+
+ float dx, dy, ddx, ddy;
+
+ dx = bezier_apply(dparams[0], u);
+ dy = bezier_apply(dparams[1], u);
+ ddx = bezier_apply(ddparams[0], u);
+ ddy = bezier_apply(ddparams[1], u);
+
+ return powf(dx*dx+dy*dy, 1.5) / (dx*ddy - dy*ddx);
+}
+
+void bezier_diff(float params[2][4], float dparams[2][4]) {
+ int i, j;
+ for (i=0; i<=1; i++) {
+ for (j=0; j<=2; j++) {
+ dparams[i][j] = (j+1)*params[i][j+1];
+ }
+ dparams[i][3] = 0.;
+ }
+}
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/bezier_utils.h b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/bezier_utils.h
new file mode 100644
index 0000000..c36fd62
--- /dev/null
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/bezier_utils.h
@@ -0,0 +1,50 @@
+/*
+ * bezier_utils.h
+ * --------------
+ * Copyright : (c) 2011, Xavier Lagorce <Xav...@cr...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ */
+
+#ifndef __BEZIER_UTILS_H
+#define __BEZIER_UTILS_H
+
+#include <math.h>
+#include <stdint.h>
+
+/*
+ * Apply a polynomial form of a Bezier Spline to a value of the parameter
+ */
+float bezier_apply(float params[4], float u);
+
+/*
+ * Computes the parameters of the polynomial from of a Bezier Spline
+ */
+void bezier_generate(float params[2][4],
+ float p_x, float p_y,
+ float p1_x, float p1_y,
+ float p2_x, float p2_y,
+ float s_x, float s_y);
+
+/*
+ * Computes the velocity profile along a Bezier Spline and according to physical
+ * contraints
+ */
+void bezier_velocity_profile(float dparams[2][4], float ddparams[2][4],
+ float v_max, float at_max, float ar_max,
+ float v_ini, float v_end,
+ float du, float* v_tab);
+
+/*
+ * Computes the curvature radius of a Bezier Spline for a given value of the
+ * parameter
+ */
+float bezier_cr(float u, float dparams[2][4], float ddparams[2][4]);
+
+/*
+ * Differentiates the polynomial form of a Bezier Spline
+ */
+void bezier_diff(float params[2][4], float dparams[2][4]);
+
+#endif /* __BEZIER_UTILS_H */
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
new file mode 100644
index 0000000..9980947
--- /dev/null
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/can_messages.h
@@ -0,0 +1,164 @@
+/*
+ * can_messages.h
+ * --------------
+ * Copyright : (c) 2011, Xavier Lagorce <Xav...@cr...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ */
+
+#ifndef __CAN_MESSAGES_H
+#define __CAN_MESSAGES_H
+
+#include <stdint.h>
+
+/* +-----------------------------------------------------------------+
+ | CAN messages IDs |
+ +-----------------------------------------------------------------+ */
+
+// Data sent by the card
+#define CAN_MSG_ENCODERS34 100 // encoder_can_msg_t
+#define CAN_MSG_MOTOR3 101 // motor_can_msg_t
+#define CAN_MSG_MOTOR4 102 // motor_can_msg_t
+#define CAN_MSG_STATUS 103 // status_can_msg_t
+#define CAN_MSG_ODOMETRY 104 // odometry_can_msg_t
+#define CAN_MSG_GHOST 105 // ghost_can_msg_t
+
+// Received commands
+#define CAN_MSG_MOVE 201 // move_can_msg_t
+#define CAN_MSG_TURN 202 // turn_can_msg_t
+#define CAN_MSG_ODOMETRY_SET 203 // odometry_can_msg_t
+#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
+
+/* +-----------------------------------------------------------------+
+ | CAN messages data structures |
+ +-----------------------------------------------------------------+ */
+
+// Information about two encoders
+typedef struct {
+ uint16_t encoder1_pos __attribute__((__packed__));
+ uint16_t encoder2_pos __attribute__((__packed__));
+ uint8_t encoder1_dir;
+ uint8_t encoder2_dir;
+ uint16_t padding __attribute__((__packed__));
+} encoder_msg_t;
+
+// Position and speed of one controller motor
+typedef struct {
+ float position __attribute__((__packed__)); // angle in radians
+ float speed __attribute__((__packed__)); // speed in rad/s (estimation)
+} motor_msg_t;
+
+// Trajectory controller states
+typedef struct {
+ uint8_t is_moving;
+} status_msg_t;
+
+// Robot state
+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
+} odometry_msg_t;
+
+// Ghost state for differential drive system
+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 state; // 1 if trajectory in progress, 0 else
+} ghost_msg_t;
+
+// Move command
+typedef struct {
+ int32_t distance __attribute__((__packed__)); // Distance in mm (fixed point representation...)
+ uint16_t speed __attribute__((__packed__)); // Speed in mm/s
+ uint16_t acceleration __attribute__((__packed__)); // Acceleration in mm/s^2
+} move_msg_t;
+
+// Turn command
+typedef struct {
+ int32_t angle __attribute__((__packed__)); // angle in 1/10000 radians (fixed point representation...)
+ uint16_t speed __attribute__((__packed__)); // Speed in 1/1000 rad/s
+ uint16_t acceleration __attribute__((__packed__)); // Acceleration in 1/1000 radians/s^2
+} turn_msg_t;
+
+// Add a new Bezier Spline to the wait queue
+typedef struct {
+ uint16_t x_end:12 __attribute__((__packed__)); // end point x coordinate in mm
+ uint16_t y_end:12 __attribute__((__packed__)); // end point y coordinate in mm
+ uint8_t d1; // first branch length in cm
+ uint8_t d2; // last branch length in cm
+ int16_t theta_end:13 __attribute__((__packed__)); // end angle in 1/1000 radians
+ uint16_t v_end:11 __attribute__((__packed__)); // final speed in mm/s
+} bezier_msg_t;
+
+// Emergency stop
+typedef struct {
+ uint8_t stop; // stop everything
+} stop_msg_t;
+
+// Select robot mode (normal or HIL)
+typedef struct {
+ uint8_t mode;
+} controller_mode_msg_t;
+
+
+/* +-----------------------------------------------------------------+
+ | CAN messages unions for data representation |
+ +-----------------------------------------------------------------+ */
+
+typedef union {
+ encoder_msg_t data;
+ uint32_t data32[2];
+} encoder_can_msg_t;
+
+typedef union {
+ motor_msg_t data;
+ uint32_t data32[2];
+} motor_can_msg_t;
+
+typedef union {
+ status_msg_t data;
+ uint32_t data32[2];
+} status_can_msg_t;
+
+typedef union {
+ odometry_msg_t data;
+ uint32_t data32[2];
+} odometry_can_msg_t;
+
+typedef union {
+ ghost_msg_t data;
+ uint32_t data32[2];
+} ghost_can_msg_t;
+
+typedef union {
+ move_msg_t data;
+ uint32_t data32[2];
+} move_can_msg_t;
+
+typedef union {
+ turn_msg_t data;
+ uint32_t data32[2];
+} turn_can_msg_t;
+
+typedef union {
+ bezier_msg_t data;
+ uint32_t data32[2];
+} bezier_can_msg_t;
+
+typedef union {
+ stop_msg_t data;
+ uint32_t data32[2];
+} stop_can_msg_t;
+
+typedef union {
+ controller_mode_msg_t data;
+ uint32_t data32[2];
+} controller_mode_can_msg_t;
+
+
+#endif /* __CAN_MESSAGES_H */
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 2c0e6da..9a86272 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
@@ -19,103 +19,6 @@ PROC_DEFINE_STACK(stack_can_receive, KERN_MINSTACKSIZE * 8);
// globals
volatile uint8_t mode;
-// Structures to represent data into CAN messages
-typedef struct {
- uint16_t encoder1_pos __attribute__((__packed__));
- uint16_t encoder2_pos __attribute__((__packed__));
- uint8_t encoder1_dir;
- uint8_t encoder2_dir;
- uint16_t padding __attribute__((__packed__));
-} encoder_msg_t;
-
-typedef struct {
- float position __attribute__((__packed__));
- float speed __attribute__((__packed__));
-} motor_msg_t;
-
-typedef struct {
- uint8_t is_moving; // 1 if a movement action is in progress, 0 if it's finished
-} status_msg_t;
-
-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
-} odometry_msg_t;
-
-typedef struct {
- int32_t distance __attribute__((__packed__)); // Distance in mm (fixed point representation...)
- uint16_t speed __attribute__((__packed__)); // Speed in mm/s
- uint16_t acceleration __attribute__((__packed__)); // Acceleration in mm/s^2
-} move_msg_t;
-
-typedef struct {
- int32_t angle __attribute__((__packed__)); // angle in 1/10000 radians (fixed point representation...)
- uint16_t speed __attribute__((__packed__)); // Speed in 1/1000 rad/s
- uint16_t acceleration __attribute__((__packed__)); // Acceleration in 1/1000 radians/s^2
-} turn_msg_t;
-
-typedef struct {
- uint8_t stop; // stop everything
-} stop_msg_t;
-
-typedef struct {
- uint8_t mode;
-} controller_mode_msg_t;
-
-// Union to manipulate CAN messages' data easily
-typedef union {
- encoder_msg_t data;
- uint32_t data32[2];
-} encoder_can_msg_t;
-
-typedef union {
- motor_msg_t data;
- uint32_t data32[2];
-} motor_can_msg_t;
-
-typedef union {
- status_msg_t data;
- uint32_t data32[2];
-} status_can_msg_t;
-
-typedef union {
- odometry_msg_t data;
- uint32_t data32[2];
-} odometry_can_msg_t;
-
-typedef union {
- move_msg_t data;
- uint32_t data32[2];
-} move_can_msg_t;
-
-typedef union {
- turn_msg_t data;
- uint32_t data32[2];
-} turn_can_msg_t;
-
-typedef union {
- stop_msg_t data;
- uint32_t data32[2];
-} stop_can_msg_t;
-
-typedef union {
- controller_mode_msg_t data;
- uint32_t data32[2];
-} controller_mode_can_msg_t;
-
-// Can messages IDs
-#define CAN_MSG_ENCODERS34 100 // encoder_can_msg_t
-#define CAN_MSG_MOTOR3 101 // motor_can_msg_t
-#define CAN_MSG_MOTOR4 102 // motor_can_msg_t
-#define CAN_MSG_STATUS 103 // status_can_msg_t
-#define CAN_MSG_ODOMETRY 104 // odometry_can_msg_t
-#define CAN_MSG_MOVE 201 // move_can_msg_t
-#define CAN_MSG_TURN 202 // turn_can_msg_t
-#define CAN_MSG_ODOMETRY_SET 203 // odometry_can_msg_t
-#define CAN_MSG_STOP 204 // stop_can_msg_t
-#define CAN_MSG_CONTROLLER_MODE 205 // controller_mode_msg_t
-
// Process for communication
static void NORETURN canMonitor_process(void);
static void NORETURN canMonitorListen_process(void);
@@ -144,6 +47,8 @@ static void NORETURN canMonitor_process(void) {
//encoder_can_msg_t msg_enc;
motor_can_msg_t msg_mot;
odometry_can_msg_t msg_odo;
+ ghost_can_msg_t msg_ghost;
+ status_can_msg_t status_msg;
can_tx_frame txm;
robot_state_t odometry;
Timer timer_can;
@@ -154,7 +59,7 @@ static void NORETURN canMonitor_process(void) {
txm.ide = 1;
txm.sid = 0;
- timer_setDelay(&timer_can, ms_to_ticks(10));
+ timer_setDelay(&timer_can, ms_to_ticks(5));
timer_setEvent(&timer_can);
while(1) {
@@ -171,25 +76,7 @@ static void NORETURN canMonitor_process(void) {
txm.eid = CAN_MSG_ENCODERS34;
can_transmit(CAND1, &txm, ms_to_ticks(10));*/
- // Sending MOTOR3 data
- msg_mot.data.position = mc_getPosition(MOTOR3);
- msg_mot.data.speed = mc_getSpeed(MOTOR3);
-
- txm.data32[0] = msg_mot.data32[0];
- txm.data32[1] = msg_mot.data32[1];
- txm.eid = CAN_MSG_MOTOR3;
- can_transmit(CAND1, &txm, ms_to_ticks(10));
-
- // Sending MOTOR4 data
- msg_mot.data.position = mc_getPosition(MOTOR4);
- msg_mot.data.speed = mc_getSpeed(MOTOR4);
-
- txm.data32[0] = msg_mot.data32[0];
- txm.data32[1] = msg_mot.data32[1];
- txm.eid = CAN_MSG_MOTOR4;
- can_transmit(CAND1, &txm, ms_to_ticks(10));
-
- // Sending odometry data if not in HIL mode
+ // Sending odometry data if not in HIL mode or motor commands if in HIL mode
if (mode != ROBOT_MODE_HIL) {
odo_getState(&odometry);
msg_odo.data.x = (int16_t)(odometry.x * 1000.0);
@@ -205,8 +92,46 @@ static void NORETURN canMonitor_process(void) {
txm.data32[1] = msg_odo.data32[1];
txm.eid = CAN_MSG_ODOMETRY;
can_transmit(CAND1, &txm, ms_to_ticks(10));
+ } else {
+ // Sending MOTOR3 data
+ msg_mot.data.position = mc_getPosition(MOTOR3);
+ msg_mot.data.speed = mc_getSpeed(MOTOR3);
+
+ txm.data32[0] = msg_mot.data32[0];
+ txm.data32[1] = msg_mot.data32[1];
+ txm.eid = CAN_MSG_MOTOR3;
+ can_transmit(CAND1, &txm, ms_to_ticks(10));
+
+ // Sending MOTOR4 data
+ msg_mot.data.position = mc_getPosition(MOTOR4);
+ msg_mot.data.speed = mc_getSpeed(MOTOR4);
+
+ txm.data32[0] = msg_mot.data32[0];
+ txm.data32[1] = msg_mot.data32[1];
+ txm.eid = CAN_MSG_MOTOR4;
+ can_transmit(CAND1, &txm, ms_to_ticks(10));
}
+ // Wait before sending the other packets
+ timer_waitEvent(&timer_can);
+ timer_add(&timer_can);
+
+ // Sending ghost state
+ msg_ghost.data.state = dd_get_ghost_state(&odometry);
+ 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);
+ txm.data32[0] = msg_ghost.data32[0];
+ txm.data32[1] = msg_ghost.data32[1];
+ txm.eid = CAN_MSG_GHOST;
+ can_transmit(CAND1, &txm, ms_to_ticks(10));
+
+ status_msg.data.is_moving = tc_is_working(MOTOR1 | MOTOR2 | MOTOR3 | MOTOR4);
+ txm.data32[0] = status_msg.data32[0];
+ txm.data32[1] = status_msg.data32[1];
+ txm.eid = CAN_MSG_STATUS;
+ can_transmit(CAND1, &txm, ms_to_ticks(10));
+
// Wait for the next transmission timer
timer_waitEvent(&timer_can);
}
@@ -219,12 +144,12 @@ static void NORETURN canMonitorListen_process(void) {
can_tx_frame txm;
robot_state_t odometry;
- status_can_msg_t status_msg;
move_can_msg_t move_msg;
turn_can_msg_t turn_msg;
odometry_can_msg_t odometry_msg;
stop_can_msg_t stop_msg;
controller_mode_can_msg_t controller_mode_msg;
+ bezier_can_msg_t bezier_msg;
// Initialize constant parameters of TX frame
txm.dlc = 8;
@@ -238,12 +163,7 @@ static void NORETURN canMonitorListen_process(void) {
if (frame.rtr == 1) {
// Handle requests
switch (frame.eid) {
- case CAN_MSG_STATUS:
- status_msg.data.is_moving = tc_is_working(MOTOR1 | MOTOR2 | MOTOR3 | MOTOR4);
- txm.data32[0] = status_msg.data32[0];
- txm.data32[1] = status_msg.data32[1];
- txm.eid = CAN_MSG_STATUS;
- can_transmit(CAND1, &txm, ms_to_ticks(10));
+ default:
break;
}
} else {
@@ -261,6 +181,13 @@ static void NORETURN canMonitorListen_process(void) {
if (!tc_is_working(TC_MASK(DD_LINEAR_SPEED_TC) | TC_MASK(DD_ROTATIONAL_SPEED_TC)))
dd_turn(turn_msg.data.angle / 10000.0, turn_msg.data.speed / 1000.0, turn_msg.data.acceleration / 1000.0);
break;
+ case CAN_MSG_BEZIER_ADD:
+ bezier_msg.data32[0] = frame.data32[0];
+ bezier_msg.data32[1] = frame.data32[1];
+ dd_add_bezier(bezier_msg.data.x_end/1000.0, bezier_msg.data.y_end/1000.0,
+ bezier_msg.data.d1/100.0, bezier_msg.data.d2/100.0,
+ bezier_msg.data.theta_end/1000.0, bezier_msg.data.v_end/1000.0);
+ break;
case CAN_MSG_STOP:
stop_msg.data32[0] = frame.data32[0];
stop_msg.data32[1] = frame.data32[1];
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/can_monitor.h b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/can_monitor.h
index 2c83df0..da6ad7f 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/can_monitor.h
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/can_monitor.h
@@ -18,6 +18,7 @@
#include "trajectory_controller.h"
#include "odometry.h"
#include "differential_drive.h"
+#include "can_messages.h"
void canMonitorInit(void);
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/controller_motor_stm32_user.mk b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/controller_motor_stm32_user.mk
index 6f52c1d..bd55ecc 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/controller_motor_stm32_user.mk
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/controller_motor_stm32_user.mk
@@ -17,6 +17,7 @@ controller_motor_stm32_USER_CSRC = \
$(controller_motor_stm32_SRC_PATH)/motor_controller.c \
$(controller_motor_stm32_SRC_PATH)/command_generator.c \
$(controller_motor_stm32_SRC_PATH)/trajectory_controller.c \
+ $(controller_motor_stm32_SRC_PATH)/bezier_utils.c \
$(controller_motor_stm32_SRC_PATH)/differential_drive.c \
$(controller_motor_stm32_SRC_PATH)/can_monitor.c \
$(controller_motor_stm32_SRC_PATH)/main.c \
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 85b643d..c22d8c8 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
@@ -9,22 +9,188 @@
#include "differential_drive.h"
+#ifndef MIN
+#define MIN(a,b) ((a) < (b) ? (a) : (b))
+#endif
+
+PROC_DEFINE_STACK(stack_traj_following, KERN_MINSTACKSIZE * 32);
+
typedef struct {
uint8_t initialized, enabled;
- float wheel_radius, shaft_radius;
+ float params[2][4], dparams[2][4], ddparams[2][4];
+ float du, v_tab[101];
+ float goal[2], v_end, theta_end;
+ float start[2], theta_ini;
+} dd_bezier_traj_t;
+
+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;
command_generator_t left_wheel_speed, right_wheel_speed;
command_generator_t left_wheel, right_wheel;
+ uint8_t current_traj;
+ dd_bezier_traj_t trajs[2];
+ float Ts, k1, k2, k3;
+ robot_state_t ghost_state;
} dd_params_t;
static dd_params_t params;
-void dd_start(float wheel_radius, float shaft_width, float max_wheel_speed) {
+static void NORETURN traj_following_process(void);
+
+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 z1, z2, z3, w1, w2, u1, u2, dt;
+ dd_bezier_traj_t *traj;
+ int32_t last_time, cur_time;
+
+ // configure timer
+ timer_setDelay(&timer, ms_to_ticks(params.Ts * 1000));
+ timer_setEvent(&timer);
+
+ // Indicate we are running
+ params.running = 1;
+
+ // Init
+ params.working = 0;
+ next_traj = (params.current_traj + 1) % 2;
+
+ while (1) {
+ if (params.enabled == 0) {
+ params.running = 0;
+ proc_exit();
+ } else {
+ if (!params.working && params.trajs[next_traj].initialized) {
+ LED2_ON();
+ params.working = 1;
+ u = 0;
+ ui = 0;
+ params.current_traj = next_traj;
+ traj = ¶ms.trajs[params.current_traj];
+ next_traj = (params.current_traj + 1) % 2;
+ params.ghost_state.x = traj->start[0];
+ params.ghost_state.y = traj->start[1];
+ params.ghost_state.theta = traj->theta_ini;
+ traj->enabled = 1;
+ last_time = ticks_to_us(timer_clock());
+ }
+ timer_add(&timer);
+ if (params.working) {
+ 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 (((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;
+ traj->initialized = 0;
+ traj->enabled = 0;
+ if (!params.trajs[next_traj].initialized) {
+ // We have no other trajectory to follow, let's brake
+ dd_set_rotational_speed(0., params.at_max);
+ dd_set_linear_speed(0., params.at_max);
+ }
+ } 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++);
+ 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;
+ } else {
+ 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;
+ 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;
+ }
+
+ // 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;
+ //if (u >= 1.0) {
+ // u = 1.0;
+ //} else {
+ 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.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;
+ } else if (params.ghost_state.theta <= M_PI) {
+ params.ghost_state.theta += 2*M_PI;
+ }
+ //}
+
+ // Compute command
+ z1=(rs.x-params.ghost_state.x)*cosf(params.ghost_state.theta)+(rs.y-params.ghost_state.y)*sinf(params.ghost_state.theta);
+ 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;
+
+ u1=(w1+v_lin)/cosf(rs.theta-params.ghost_state.theta);
+ u2=w2*powf(cosf(rs.theta-params.ghost_state.theta),2)+v_rot;
+
+ // Apply command
+ dd_set_linear_speed(u1, 0.);
+ dd_set_rotational_speed(u2, 0.);
+
+ // Keep current time
+ last_time = cur_time;
+ }
+ }
+ }
+ timer_waitEvent(&timer); // Wait for the remaining of the sample period
+ }
+}
+
+void dd_start(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.wheel_radius = wheel_radius;
- params.shaft_radius = shaft_width;
+ params.shaft_width = shaft_width;
params.last_lin_acceleration = 0.0;
params.last_rot_acceleration = 0.0;
+ params.ghost_state.x = 0;
+ params.ghost_state.y = 0;
+ params.ghost_state.theta = 0;
+
+ params.running = 0;
+ params.working = 0;
+ params.current_traj = 0;
+ params.trajs[0].initialized = 0;
+ params.trajs[1].initialized = 0;
+ params.trajs[0].enabled = 0;
+ params.trajs[1].enabled = 0;
+ params.v_max = v_max;
+ params.at_max = at_max;
+ params.ar_max = ar_max;
+
+ params.k1 = k1;
+ params.k2 = k2;
+ params.k3 = k3;
+ params.Ts = Ts;
+
tc_new_controller(DD_LINEAR_SPEED_TC);
tc_new_controller(DD_ROTATIONAL_SPEED_TC);
new_dd_generator(¶ms.left_wheel_speed,
@@ -51,6 +217,8 @@ void dd_start(float wheel_radius, float shaft_width, float max_wheel_speed) {
params.initialized = 1;
params.enabled = 1;
+
+ proc_new(traj_following_process, NULL, sizeof(stack_traj_following), stack_traj_following);
}
void dd_pause(void) {
@@ -104,15 +272,90 @@ void dd_turn(float angle, float speed, float acceleration) {
void dd_set_linear_speed(float speed, float acceleration) {
if (params.enabled) {
- params.last_lin_acceleration = acceleration;
- tc_goto_speed(DD_LINEAR_SPEED_TC, speed, params.last_lin_acceleration);
+ if (acceleration != 0.) {
+ params.last_lin_acceleration = acceleration;
+ tc_goto_speed(DD_LINEAR_SPEED_TC, speed, params.last_lin_acceleration);
+ } else {
+ tc_set_speed(DD_LINEAR_SPEED_TC, speed);
+ }
}
}
void dd_set_rotational_speed(float speed, float acceleration) {
if (params.enabled) {
- params.last_rot_acceleration = acceleration;
- tc_goto_speed(DD_ROTATIONAL_SPEED_TC, speed, params.last_rot_acceleration);
+ if (acceleration != 0.) {
+ params.last_rot_acceleration = acceleration;
+ tc_goto_speed(DD_ROTATIONAL_SPEED_TC, speed, params.last_rot_acceleration);
+ } else {
+ tc_set_speed(DD_ROTATIONAL_SPEED_TC, speed);
+ }
}
}
+uint8_t dd_add_bezier(float x_end, float y_end, float d1, float d2, float end_angle, float end_speed) {
+ uint8_t t_ind;
+ robot_state_t rs;
+ dd_bezier_traj_t *traj;
+ float v_ini, x_ini, y_ini, theta_ini;
+
+ t_ind = (params.current_traj + 1) % 2;
+
+ if (params.trajs[t_ind].initialized == 1) {
+ return DD_TRAJECTORY_ALREADY_USED;
+ } else {
+ traj = ¶ms.trajs[t_ind];
+ // New trajectory is not enabled
+ traj->enabled = 0;
+
+ // Get starting parameters
+ if (params.trajs[params.current_traj].enabled) {
+ v_ini = params.trajs[params.current_traj].v_end;
+ x_ini = params.trajs[params.current_traj].goal[0];
+ y_ini = params.trajs[params.current_traj].goal[1];
+ theta_ini = params.trajs[params.current_traj].theta_end;
+ } else {
+ odo_getState(&rs);
+ x_ini = rs.x;
+ y_ini = rs.y;
+ theta_ini = rs.theta;
+ v_ini = 0.01; // non null low velocity to allow the system to start
+ }
+
+ // Compute Bezier Spline parameters
+ bezier_generate(traj->params,
+ x_ini, y_ini,
+ x_ini + d1*cosf(theta_ini), y_ini + d1*sinf(theta_ini),
+ x_end - d2*cosf(end_angle), y_end - d2*sinf(end_angle),
+ x_end, y_end);
+ traj->goal[0] = x_end;
+ traj->goal[1] = y_end;
+ traj->v_end = end_speed;
+ traj->theta_end = end_angle;
+ traj->start[0] = x_ini;
+ traj->start[1] = y_ini;
+ traj->theta_ini = theta_ini;
+ // Differentiate parameters
+ bezier_diff(traj->params, traj->dparams);
+ 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,
+ v_ini, end_speed,
+ 0.01, traj->v_tab);
+ traj->du = 0.01;
+
+ params.trajs[t_ind].initialized = 1;
+ return DD_NO_ERROR;
+ }
+}
+
+uint8_t dd_get_ghost_state(robot_state_t *state) {
+ state->x = params.ghost_state.x;
+ state->y = params.ghost_state.y;
+ state->theta = params.ghost_state.theta;
+
+ if (params.working == 1)
+ return DD_GHOST_MOVING;
+ else
+ return DD_GHOST_STOPPED;
+}
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 61684ce..93fc4ef 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
@@ -11,8 +11,11 @@
#define __DIFFERENTIAL_DRIVE_H
#include <math.h>
+#include <drv/timer.h>
#include "trajectory_controller.h"
#include "command_generator.h"
+#include "odometry.h"
+#include "bezier_utils.h"
#ifndef DD_LINEAR_SPEED_TC
#define DD_LINEAR_SPEED_TC 0
@@ -21,15 +24,28 @@
#define DD_ROTATIONAL_SPEED_TC 1
#endif
+#define DD_NO_ERROR 0
+#define DD_TRAJECTORY_ALREADY_USED 1
+
+#define DD_GHOST_STOPPED 0
+#define DD_GHOST_MOVING 1
+
/* Initializes the differential drive
* - wheel_radius : radius of the wheels (in meters)
- * - shaft_width : propulsion shaft radius (in meters)
- * - max_speed : maximum wheel speed (in rad/s)
+ * - shaft_width : propulsion shaft width (in meters)
+ * - max_wheel_speed : maximum wheel speed (in rad/s)
+ * - 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)
+ * - k1, k2, k3 : control loop gain for trajectory following
+ * - Ts : sample time for control loop in seconds
*
* 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_speed);
+void dd_start(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);
/* Pauses or Resumes the differential drive system.
* In pause mode, the drive will accept no further command and actions will be
@@ -74,4 +90,19 @@ void dd_turn(float angle, float speed, float acceleration);
void dd_set_linear_speed(float speed, float acceleration);
void dd_set_rotational_speed(float speed, float acceleration);
+/*
+ * Add a Bezier Spline to the trajectory follower
+ */
+uint8_t dd_add_bezier(float x_end, float y_end, float d1, float d2, float end_angle, float end_speed);
+
+/*
+ * Return the current state of the followed ghost robot
+ * - state : pointer to a robot_state_t structure where the ghost state will be written
+ *
+ * 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);
+
#endif /* __DIFFERENTIAL_DRIVE_H */
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 926c6ad..6b607e9 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
@@ -47,7 +47,7 @@ static void init(void)
// Start control of drive motors
tc_init();
- dd_start(WHEEL_RADIUS, SHAFT_WIDTH, 4.8*2*M_PI); // limit wheel speed to 1.5 m/s
+ dd_start(WHEEL_RADIUS, SHAFT_WIDTH, 8*2*M_PI, 0.5, 1.0, 1.0, 0.7, 0.7, 1.0, 0.01);
// Common parameters
params.encoder_gain = -2.0*M_PI/2000.0/15.0;
params.G0 = 0.0145;
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/trajectory_controller.c b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/trajectory_controller.c
index 5b01741..2ae2d4f 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/trajectory_controller.c
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/trajectory_controller.c
@@ -299,6 +299,30 @@ void tc_goto_speed(uint8_t cntr_index, float speed, float acceleration) {
cont->working = 1;
}
+void tc_set_speed(uint8_t cntr_index, float speed) {
+ trajectory_controller_t *cont;
+
+ // Get the controller and verifies it is enabled
+ if (cntr_index >= NUM_TC_MAX)
+ return;
+ cont = &controllers[cntr_index];
+ if (!cont->enabled)
+ return;
+
+ // Disable a possibly running profile
+ cont->aut.state = TRAPEZOID_STATE_STOP;
+ remove_callback(&cont->position);
+ remove_callback(&cont->speed);
+
+ // Set speed and acceleration
+ adjust_value(&cont->speed, speed);
+ adjust_speed(&cont->speed, 0.0);
+
+ cont->working = 0;
+}
+
+
+
void tc_move(tc_robot_t *robot, float distance, float speed, float acceleration) {
float dis_s, spe_s, acc_s;
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/trajectory_controller.h b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/trajectory_controller.h
index a4901a3..cf8452d 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/trajectory_controller.h
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/trajectory_controller.h
@@ -76,6 +76,11 @@ void tc_goto(uint8_t cntr_index, float angle, float speed, float acceleration);
*/
void tc_goto_speed(uint8_t cntr_index, float speed, float acceleration);
+/*
+ * Change the speed command of a controller without any concern about acceleration
+ */
+void tc_set_speed(uint8_t cntr_index, float speed);
+
/* Moves along a line
* - robot : pointer to a structure describing the robot configuration
* - distance : distance to move of (in meters), can be positive (forward)
hooks/post-receive
--
krobot
|
