Menu
▾
▴
krobot-commits — code commits
You can subscribe to this list here.
| 2009 |
Jan
|
Feb
|
Mar
|
Apr
|
May
|
Jun
|
Jul
|
Aug
|
Sep
|
Oct
|
Nov
|
Dec
(13) |
|---|---|---|---|---|---|---|---|---|---|---|---|---|
| 2010 |
Jan
(50) |
Feb
(137) |
Mar
(84) |
Apr
(36) |
May
(100) |
Jun
(5) |
Jul
|
Aug
(4) |
Sep
(13) |
Oct
(1) |
Nov
(4) |
Dec
(22) |
| 2011 |
Jan
(4) |
Feb
(9) |
Mar
(113) |
Apr
(76) |
May
(31) |
Jun
(19) |
Jul
|
Aug
|
Sep
|
Oct
|
Nov
|
Dec
|
| 2012 |
Jan
(4) |
Feb
|
Mar
(2) |
Apr
(6) |
May
(19) |
Jun
|
Jul
|
Aug
|
Sep
|
Oct
(4) |
Nov
|
Dec
|
| 2013 |
Jan
|
Feb
|
Mar
(2) |
Apr
(22) |
May
(6) |
Jun
|
Jul
|
Aug
|
Sep
|
Oct
(1) |
Nov
|
Dec
|
| 2014 |
Jan
|
Feb
|
Mar
|
Apr
|
May
(1) |
Jun
|
Jul
|
Aug
|
Sep
|
Oct
|
Nov
|
Dec
|
|
From: Xavier L. <Ba...@us...> - 2014-05-12 00:23:50
|
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 3fd27c9f52ebfe4588a658f93d641636e39bdc1b (commit)
via 1cea11f47cb9ceb0701bb017264f4d8492754773 (commit)
via 0c63a2e2a45bfe4a14222541a555befa5bb55437 (commit)
via 5f3876a31395d1f29dfaf44c4445995d7ab24722 (commit)
via 994929ba5bce7ee3387ea81fc0332104f6b3ff9d (commit)
via 5a83dc24890f992491b5ef7582ff8e76a3120a08 (commit)
via 315d201eac4a75dec3795a0f8cd5e1e168c74d79 (commit)
via 5a4a54b8904d0781dd7fe6d37b8123e158a16996 (commit)
via 474b5f30be79772343e5073ae5cd7d31b4f1f773 (commit)
via 66cbb39e243aee04550be39bbc86f333523e46c9 (commit)
via 92b82b451da8b0bc99264986f5a431c2ef8f5f89 (commit)
via 95f83897c7d2531942b06e5d56bac9220e364856 (commit)
via d37942788c1bfd5edde52e2dd975534e66ec80c6 (commit)
via ecc660db1c451278ffd064ec1cfc8a0385e090e9 (commit)
via eb03d55164987db98e4b345d7b928e511607ba05 (commit)
via 76c179c01a444c72d7d58a2fbfbf0960423c4385 (commit)
via 41e6e5c280a84237c99a1713b414fc5ef286c27a (commit)
via ca92842c71d8b988cc21e896212d0a1960ebd339 (commit)
via 685980f1ed1e10de6d368434feac3947b5a1b910 (commit)
from 233667799df4892865de6877584ef3c9ac89dc6c (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 3fd27c9f52ebfe4588a658f93d641636e39bdc1b
Author: Xavier Lagorce <Xav...@cr...>
Date: Mon May 12 02:25:06 2014 +0200
[Controller_Motor_STM32/Propulsion_Drive] Change motor direction (added functionnality in motor library)
commit 1cea11f47cb9ceb0701bb017264f4d8492754773
Author: Xavier Lagorce <Xav...@cr...>
Date: Mon May 12 00:53:36 2014 +0200
[Krobot_config] Added new size of the robot (without arms) and adapted viewer
commit 0c63a2e2a45bfe4a14222541a555befa5bb55437
Author: Xavier Lagorce <Xav...@cr...>
Date: Sun May 11 23:03:53 2014 +0200
[info] Added new CAN messages for lifts and pumps
commit 5f3876a31395d1f29dfaf44c4445995d7ab24722
Author: Xavier Lagorce <Xav...@cr...>
Date: Sun May 11 22:32:34 2014 +0200
[Controller_Motor_STM32/Effectors] Send correct CAN id for lift states
commit 994929ba5bce7ee3387ea81fc0332104f6b3ff9d
Author: Xavier Lagorce <Xav...@cr...>
Date: Sun May 11 22:18:39 2014 +0200
[Controller_Motor_STM32/Effectors] Update for 2014's configuration
commit 5a83dc24890f992491b5ef7582ff8e76a3120a08
Author: Xavier Lagorce <Xav...@cr...>
Date: Sun May 11 22:17:33 2014 +0200
[Controller_Motor_STM32/lib] Added a function to get current command applyed to a MOTOR
commit 315d201eac4a75dec3795a0f8cd5e1e168c74d79
Author: Xavier Lagorce <Xav...@cr...>
Date: Sun May 11 22:17:06 2014 +0200
[Controller_Motor_STM32/Effectors] Updated openocd configuration
commit 5a4a54b8904d0781dd7fe6d37b8123e158a16996
Author: Xavier Lagorce <Xav...@cr...>
Date: Sun May 11 22:16:23 2014 +0200
[Controller_Motor_STM32] Update lifts configuration for 2014's robot.
commit 474b5f30be79772343e5073ae5cd7d31b4f1f773
Author: Xavier Lagorce <Xav...@cr...>
Date: Sun May 11 22:15:42 2014 +0200
[Controller_Motor_STM32] No more motor on output MOTOR2
commit 66cbb39e243aee04550be39bbc86f333523e46c9
Author: Xavier Lagorce <Xav...@cr...>
Date: Sun May 11 22:13:55 2014 +0200
[Controller_Motor_STM32] Moved can messages description to a common location for different firmwares
commit 92b82b451da8b0bc99264986f5a431c2ef8f5f89
Author: Pierre Chambart <pie...@cr...>
Date: Sun May 11 22:30:04 2014 +0200
[info/urg] Add config to limit visible points
commit 95f83897c7d2531942b06e5d56bac9220e364856
Author: Xavier Lagorce <Xav...@cr...>
Date: Sun May 11 17:02:58 2014 +0200
[info/viewer] Replaced Blue by Yellow in Viewer UI
commit d37942788c1bfd5edde52e2dd975534e66ec80c6
Author: Xavier Lagorce <Xav...@cr...>
Date: Fri May 9 19:25:17 2014 +0200
[elec/Controller_Motor_STM32] Lift FRONT/BACK are now LEFT/RIGHT
commit ecc660db1c451278ffd064ec1cfc8a0385e090e9
Author: Xavier Lagorce <Xav...@cr...>
Date: Fri May 9 19:24:06 2014 +0200
[info/control2011] New table drawing, configuration, obstacles and objects
commit eb03d55164987db98e4b345d7b928e511607ba05
Author: Xavier Lagorce <Xav...@cr...>
Date: Fri May 9 16:49:17 2014 +0200
[elec/boards] Added motherboard documentation
commit 76c179c01a444c72d7d58a2fbfbf0960423c4385
Author: Pierre Chambart <pie...@cr...>
Date: Fri May 9 00:10:48 2014 +0200
Krobot_example: loop
commit 41e6e5c280a84237c99a1713b414fc5ef286c27a
Author: Pierre Chambart <pie...@cr...>
Date: Fri May 9 00:10:30 2014 +0200
Hackish help for the pathfinder
commit ca92842c71d8b988cc21e896212d0a1960ebd339
Author: Pierre Chambart <pie...@cr...>
Date: Fri May 9 00:08:00 2014 +0200
Typo in krobot_action doc
commit 685980f1ed1e10de6d368434feac3947b5a1b910
Author: Pierre Chambart <pie...@cr...>
Date: Thu May 8 18:37:03 2014 +0200
[info/src/tools] simple startegy launcher example
-----------------------------------------------------------------------
Changes:
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/can_messages.h b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/can_messages.h
deleted file mode 100644
index 3fe2b50..0000000
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/can_messages.h
+++ /dev/null
@@ -1,176 +0,0 @@
-/*
- * 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
-
-#define CAN_MSG_LIFT_CMD 231 // lift_cmd_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 u; // Parameter on the spline between 0 and 255
- 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
- int16_t d1:9 __attribute__((__packed__)); // first branch length in cm
- uint8_t d2:8 __attribute__((__packed__)); // last branch length in cm
- int16_t theta_end:12 __attribute__((__packed__)); // end angle in 1/100 radians
- uint16_t v_end:11 __attribute__((__packed__)); // final speed in mm/s
-} bezier_msg_t;
-
-// Stop Bezier Spline following and brakes
-typedef struct {
- 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)
-typedef struct {
- uint8_t mode;
-} controller_mode_msg_t;
-
-// Lift command position, a value is ignored if < 0
-typedef struct {
- float front_lift __attribute__((__packed__));
- float back_lift __attribute__((__packed__));
-} lift_cmd_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;
-
-typedef union {
- lift_cmd_msg_t data;
- uint32_t data32[2];
-} lift_cmd_can_msg_t;
-
-#endif /* __CAN_MESSAGES_H */
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/can_messages_sensors.h b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/can_messages_sensors.h
deleted file mode 100644
index 512077e..0000000
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/can_messages_sensors.h
+++ /dev/null
@@ -1,154 +0,0 @@
-/**
- * Sensor-Actuator board messages
- *
- * Header file for the Sensor and Actuator CAN messages
- *
- * Copyright © 2011 Nicolas Dandrimont <ol...@cr...>
- * Authors: Nicolas Dandrimont <ol...@cr...>
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- */
-
-#ifndef CAN_MESSAGES_H__
-#define CAN_MESSAGES_H__
-
-/**
- * Message IDs
- */
-
-// Beacon
-
-#define CAN_BEACON_POSITION 301 // beacon_position
-#define CAN_BEACON_LOWLEVEL_POSITION 302 // beacon_lowlevel_position
-#define CAN_BEACON_CALIBRATION 303 // beacon_calibration
-
-// Switches
-
-#define CAN_SWITCH_STATUS_1 311 // switch_status
-#define CAN_SWITCH_STATUS_2 312 // switch_status
-#define CAN_SWITCH_SET 313 // switch_request
-
-
-// ADC
-
-#define CAN_ADC_VALUES_1 321 // adc_values
-#define CAN_ADC_VALUES_2 322 // adc_values
-
-
-/****************************************************************************/
-
-/**
- * Message Packets
- */
-
-/**
- * Beacon messages
- */
-
-// Position of the opponent
-struct beacon_position_pkt {
- uint16_t angle; // in 1/10000th of radians [0; 2*Pi[
- uint16_t distance; // in mm [0; 65536[
- uint16_t period; // in 1/10000th of seconds [0; 1[
-} __attribute__((packed));
-
-
-
-// Beacon low-level position
-struct beacon_lowlevel_position_pkt {
- uint16_t angle; // in 1/10000th of radians [0; 2*Pi[
- uint16_t width; // in 1/100000th of radians [0; Pi/5[
- uint32_t period; // in timer ticks
-} __attribute__((packed));
-
-
-
-// Beacon calibration
-struct beacon_calibration_pkt {
- uint16_t width; // in 1/100000th of radians [0; Pi/5[
- uint16_t distance; // in mm [0; 65536[
-} __attribute__((packed));
-
-
-/**
- * Switch messages
- */
-
-// Switch status
-struct switch_status_pkt {
- uint8_t sw1;
- uint8_t sw2;
- uint8_t sw3;
- uint8_t sw4;
- uint8_t sw5;
- uint8_t sw6;
- uint8_t sw7;
- uint8_t sw8;
-};
-
-// Switch request
-struct switch_request_pkt {
- uint8_t num;
- uint8_t state;
-};
-
-/**
- * ADC messages
- */
-// ADC Values
-struct adc_values_pkt {
- uint16_t val1;
- uint16_t val2;
- uint16_t val3;
- uint16_t val4;
-} __attribute__((packed));
-
-/****************************************************************************/
-
-/**
- * Typedefs
- */
-
-typedef union {
- struct beacon_position_pkt p;
- uint32_t d[2];
-} beacon_position;
-
-typedef union {
- struct beacon_lowlevel_position_pkt p;
- uint32_t d[2];
-} beacon_lowlevel_position;
-
-typedef union {
- struct beacon_calibration_pkt p;
- uint32_t d[2];
-} beacon_calibration;
-
-typedef union {
- struct switch_status_pkt p;
- uint32_t d[2];
-} switch_status;
-
-typedef union {
- struct switch_request_pkt p;
- uint32_t d[2];
-} switch_request;
-
-typedef union {
- struct adc_values_pkt p;
- uint32_t d[2];
-} adc_values;
-
-#endif
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/can_monitor.c b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/can_monitor.c
index 43913c8..bd48102 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/can_monitor.c
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/can_monitor.c
@@ -45,6 +45,7 @@ void canMonitorInit(void) {
static void NORETURN canMonitor_process(void) {
encoder_can_msg_t msg_enc;
+ pump_state_can_msg_t msg_pumps;
can_tx_frame txm;
Timer timer_can;
@@ -60,15 +61,24 @@ static void NORETURN canMonitor_process(void) {
timer_add(&timer_can);
- // Sending ENCODER3 and ENCODER4 data
- msg_enc.data.encoder1_pos = getEncoderPosition(ENCODER3);
+ // Sending ENCODER2 and ENCODER4 data
+ msg_enc.data.encoder1_pos = getEncoderPosition(ENCODER2);
msg_enc.data.encoder2_pos = getEncoderPosition(ENCODER4);
- msg_enc.data.encoder1_dir = getEncoderDirection(ENCODER3);
+ msg_enc.data.encoder1_dir = getEncoderDirection(ENCODER2);
msg_enc.data.encoder2_dir = getEncoderDirection(ENCODER4);
txm.data32[0] = msg_enc.data32[0];
txm.data32[1] = msg_enc.data32[1];
- txm.eid = CAN_MSG_ENCODERS34;
+ txm.eid = CAN_MSG_LIFT_ENCODERS;
+ can_transmit(CAND1, &txm, ms_to_ticks(10));
+
+ // Sending pump states
+ msg_pumps.data.left_pump = motorGetSpeed(MOTOR1);
+ msg_pumps.data.right_pump = motorGetSpeed(MOTOR3);
+
+ txm.data32[0] = msg_pumps.data32[0];
+ txm.data32[1] = msg_pumps.data32[1];
+ txm.eid = CAN_MSG_PUMP_STATE;
can_transmit(CAND1, &txm, ms_to_ticks(10));
// Wait for the next transmission timer
@@ -87,6 +97,7 @@ static void NORETURN canMonitorListen_process(void) {
switch_status end_courses_msg;
lift_cmd_can_msg_t lift_cmd_msg;
+ pump_cmd_can_msg_t pump_cmd_msg;
// Initialize constant parameters of TX frame
txm.dlc = 8;
@@ -126,21 +137,29 @@ static void NORETURN canMonitorListen_process(void) {
end_courses_msg.d[1] = frame.data32[1];
end_stops = 0;
if (!end_courses_msg.p.sw3)
- end_stops |= LC_BACK_BOTTOM;
+ end_stops |= LC_RIGHT_BOTTOM;
if (!end_courses_msg.p.sw4)
- end_stops |= LC_BACK_UP;
+ end_stops |= LC_RIGHT_UP;
lc_end_stop_reached(end_stops);
//if (end_courses_msg.p.sw1) {
// lc_release();
// motorSetSpeed(MOTOR2,0);
break;
case CAN_MSG_LIFT_CMD:
- lift_cmd_msg.data32[0] = frame.data32[0];
- lift_cmd_msg.data32[1] = frame.data32[1];
- if (lift_cmd_msg.data.front_lift >= 0)
- lc_goto_position(LC_FRONT_LIFT, lift_cmd_msg.data.front_lift);
- if (lift_cmd_msg.data.back_lift >= 0)
- lc_goto_position(LC_BACK_LIFT, lift_cmd_msg.data.back_lift);
+ //lift_cmd_msg.data32[0] = frame.data32[0];
+ //lift_cmd_msg.data32[1] = frame.data32[1];
+ //if (lift_cmd_msg.data.front_lift >= 0)
+ // lc_goto_position(LC_LEFT_LIFT, lift_cmd_msg.data.front_lift);
+ //if (lift_cmd_msg.data.back_lift >= 0)
+ // lc_goto_position(LC_RIGHT_LIFT, lift_cmd_msg.data.back_lift);
+ break;
+ case CAN_MSG_PUMP_CMD:
+ pump_cmd_msg.data32[0] = frame.data32[0];
+ pump_cmd_msg.data32[1] = frame.data32[1];
+ if (pump_cmd_msg.data.left_pump >= 0)
+ motorSetSpeed(MOTOR1, pump_cmd_msg.data.left_pump);
+ if (pump_cmd_msg.data.right_pump >= 0)
+ motorSetSpeed(MOTOR3, pump_cmd_msg.data.right_pump);
break;
}
}
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/can_monitor.h b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/can_monitor.h
index c55f124..e01de3b 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/can_monitor.h
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/can_monitor.h
@@ -18,8 +18,8 @@
#include "trajectory_controller.h"
#include "odometry.h"
#include "differential_drive.h"
-#include "can_messages.h"
-#include "can_messages_sensors.h"
+#include "can_messages/can_messages.h"
+#include "can_messages/can_messages_sensors.h"
#include "lift_controller.h"
void canMonitorInit(void);
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/main.c b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/main.c
index f906450..15f8b65 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/main.c
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/main.c
@@ -15,7 +15,7 @@
#include "motor_controller.h"
#include "can_monitor.h"
#include "command_generator.h"
-#include "differential_drive.h"
+#include "lift_controller.h"
PROC_DEFINE_STACK(stack_ind, KERN_MINSTACKSIZE * 2);
@@ -42,17 +42,21 @@ static void init(void)
// Initialize Command generator
tc_init();
// Initialize Lift Controller
- lc_init();
+ //lc_init();
// Init lifts
- lc_homing(LC_BACK_LIFT);
- lc_goto_position(LC_BACK_LIFT, 0.5);
- while(tc_is_working(TC_MASK(LC_TC_BACK)))
- timer_delay(100);
+ //lc_homing(LC_RIGHT_LIFT);
+ //lc_goto_position(LC_RIGHT_LIFT, 0.5);
+ //while(tc_is_working(TC_MASK(LC_TC_RIGHT)))
+ // timer_delay(100);
+
+ // Setup Left pump motor
+ motorSetMaxPWM(MOTOR1, 1800); // Limit to 12V
+ enableMotor(MOTOR1);
- // Setup Beacon's motor
- motorSetMaxPWM(MOTOR2, 1080);
- enableMotor(MOTOR2);
+ // Setup Right pump motor
+ motorSetMaxPWM(MOTOR3, 1800); // Limit to 12V
+ enableMotor(MOTOR3);
// Blink to say we are ready
for (uint8_t i=0; i < 5; i++) {
@@ -73,13 +77,13 @@ static void NORETURN ind_process(void)
{
while(1) {
LED1_ON();
- lc_goto_position(LC_BACK_LIFT, 1.);
- while(tc_is_working(TC_MASK(LC_TC_BACK)))
+ lc_goto_position(LC_RIGHT_LIFT, 1.);
+ while(tc_is_working(TC_MASK(LC_TC_RIGHT)))
timer_delay(100);
timer_delay(1000);
LED1_OFF();
- lc_goto_position(LC_BACK_LIFT, 0);
- while(tc_is_working(TC_MASK(LC_TC_BACK)))
+ lc_goto_position(LC_RIGHT_LIFT, 0);
+ while(tc_is_working(TC_MASK(LC_TC_RIGHT)))
timer_delay(100);
timer_delay(1000);
}
@@ -92,9 +96,6 @@ int main(void)
/* Create a new child process */
//proc_new(ind_process, NULL, sizeof(stack_ind), stack_ind);
- // Tests
- motorSetSpeed(MOTOR2, 1000);
-
/*
* The main process is kept to periodically report the stack
* utilization of all the processes (1 probe per second).
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/jtag/flash.cfg b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/jtag/flash.cfg
index 2721d3e..b1c2352 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/jtag/flash.cfg
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/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/Effectors/jtag/openocd.cfg b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/jtag/openocd.cfg
index d6a58e9..0e09b2b 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/jtag/openocd.cfg
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/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/Propulsion_Drive/controller_motor_stm32/can_messages.h b/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/can_messages.h
deleted file mode 100644
index b125c63..0000000
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/can_messages.h
+++ /dev/null
@@ -1,259 +0,0 @@
-/*
- * 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
-#define CAN_MSG_CONTROL_ERROR 106 // error_can_msg_t
-#define CAN_MSG_ODOMETRY_INDEP 107 // odometry_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_SIMULATION_MODE 205 // simulation_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
-#define CAN_MSG_MOTOR_COMMAND 208 // motor_command_can_msg_t
-#define CAN_MSG_ODOMETRY_INDEP_SET 209 // odometry_can_msg_t
-#define CAN_MSG_CONTROLLER_ACTIVATION 210 // controller_activation_can_msg_t
-#define CAN_MSG_DRIVE_ACTIVATION 211 // controller_activation_can_msg_t
-#define CAN_MSG_TORQUE_LIMIT 212 // torque_limit_can_msg_t
-#define CAN_MSG_DRIVE_TORQUE_LIMIT 213 // drive_torque_limit_can_msg_t
-
-/* +-----------------------------------------------------------------+
- | Constants for messages |
- +-----------------------------------------------------------------+ */
-#define SIMULATION_MODE_NO 0
-#define SIMULATION_MODE_NORMAL 1
-#define SIMULATION_MODE_HIL 2
-
-/* +-----------------------------------------------------------------+
- | 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;
-
-// Error packet
-typedef struct {
- uint8_t err1;
- uint8_t err2;
-} error_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 u; // Parameter on the spline between 0 and 255
- uint8_t state; // 1 if trajectory in progress, 0 else
-} ghost_msg_t;
-
-// Torque limitation on some motors
-typedef struct {
- uint8_t motor __attribute__((__packed__)); // Logic OR of bits corresponding to motors
- uint16_t limit __attribute__((__packed__)); // Torque limit (between 0 and 3600)
-} torque_limit_msg_t;
-
-// Torque limitation on propulsion drive
-typedef struct {
- uint16_t limit __attribute__((__packed__)); // Torque limit (between 0 and 3600)
-} drive_torque_limit_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;
-
-// Controller activation command
-typedef struct {
- uint8_t motor __attribute__((__packed__)); // Motor ID of the motor to act on
- uint8_t activate __attribute__((__packed__)); // Activate the controller if non zero
- // else release the control.
-} controller_activation_msg_t;
-
-// Propulsion Drive activation command
-typedef struct {
- uint8_t activate __attribute__((__packed__)); // Activate the propulsion drive if non zero
- // else release the control.
-} drive_activation_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
- int16_t d1:9 __attribute__((__packed__)); // first branch length in cm
- uint8_t d2:8 __attribute__((__packed__)); // last branch length in cm
- int16_t theta_end:12 __attribute__((__packed__)); // end angle in 1/100 radians
- 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 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;
-
-// Stop Bezier Spline following and brakes
-typedef struct {
- 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, simulation or HIL)
-typedef struct {
- uint8_t mode;
-} simulation_mode_msg_t;
-
-// Command the speed of a particular motor
-typedef struct {
- uint8_t motor_id __attribute__((__packed__));
- int32_t speed __attribute__((__packed__));
-} motor_command_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 {
- error_msg_t data;
- uint32_t data32[2];
-} error_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 {
- torque_limit_msg_t data;
- uint32_t data32[2];
-} torque_limit_can_msg_t;
-
-typedef union {
- drive_torque_limit_msg_t data;
- uint32_t data32[2];
-} drive_torque_limit_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 {
- controller_activation_msg_t data;
- uint32_t data32[2];
-} controller_activation_can_msg_t;
-
-typedef union {
- drive_activation_msg_t data;
- uint32_t data32[2];
-} drive_activation_can_msg_t;
-
-typedef union {
- bezier_msg_t data;
- uint32_t data32[2];
-} 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;
-
-typedef union {
- simulation_mode_msg_t data;
- uint32_t data32[2];
-} simulation_mode_can_msg_t;
-
-typedef union {
- motor_command_msg_t data;
- uint32_t data32[2];
-} motor_command_can_msg_t;
-
-#endif /* __CAN_MESSAGES_H */
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/can_messages_sensors.h b/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/can_messages_sensors.h
deleted file mode 100644
index 48c3b90..0000000
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/can_messages_sensors.h
+++ /dev/null
@@ -1,223 +0,0 @@
-/**
- * Sensor-Actuator board messages
- *
- * Header file for the Sensor and Actuator CAN messages
- *
- * Copyright © 2011 Nicolas Dandrimont <ol...@cr...>
- * Authors: Nicolas Dandrimont <ol...@cr...>
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- */
-
-#ifndef CAN_MESSAGES_H__
-#define CAN_MESSAGES_H__
-
-/**
- * Message IDs
- */
-
-// Beacon
-
-#define CAN_BEACON_POSITION 301 // beacon_position
-#define CAN_BEACON_LOWLEVEL_POSITION 302 // beacon_lowlevel_position
-#define CAN_BEACON_CALIBRATION 303 // beacon_calibration
-
-// Switches
-
-#define CAN_SWITCH_STATUS_1 311 // switch_status
-#define CAN_SWITCH_STATUS_2 312 // switch_status
-#define CAN_SWITCH_SET 313 // switch_request
-
-
-// ADC
-
-#define CAN_ADC_VALUES_1 321 // adc_values
-#define CAN_ADC_VALUES_2 322 // adc_values
-
-// Battery monitoring
-
-#define CAN_BATTERY_STATUS_1 331 // battery_status
-#define CAN_BATTERY_STATUS_2 332 // battery_status
-
-// AX12
-
-#define CAN_AX12_STATE 341 // ax12_state
-#define CAN_AX12_REQUEST_STATE 342 // ax12_request_state
-#define CAN_AX12_GOTO 343 // ax12_goto
-//#define CAN_AX12_RESET 344 // Not used anymore
-#define CAN_AX12_SET_TORQUE_ENABLE 345 // ax12_set_torque_enable
-
-/****************************************************************************/
-
-/**
- * Message Packets
- */
-
-/**
- * Beacon messages
- */
-
-// Position of the opponent
-struct beacon_position_pkt {
- uint16_t angle[2]; // in 1/10000th of radians [0; 2*Pi[
- uint16_t distance[2]; // in mm [0; 65536[
-} __attribute__((packed));
-
-
-
-// Beacon low-level position
-struct beacon_lowlevel_position_pkt {
- uint16_t angle; // in 1/10000th of radians [0; 2*Pi[
- uint16_t width; // in 1/100000th of radians [0; Pi/5[
- uint32_t period; // in timer ticks
-} __attribute__((packed));
-
-
-
-// Beacon calibration
-struct beacon_calibration_pkt {
- uint16_t width; // in 1/100000th of radians [0; Pi/5[
- uint16_t distance; // in mm [0; 65536[
-} __attribute__((packed));
-
-
-/**
- * Switch messages
- */
-
-// Switch status
-struct switch_status_pkt {
- uint8_t sw1;
- uint8_t sw2;
- uint8_t sw3;
- uint8_t sw4;
- uint8_t sw5;
- uint8_t sw6;
- uint8_t sw7;
- uint8_t sw8;
-};
-
-// Switch request
-struct switch_request_pkt {
- uint8_t num;
- uint8_t state;
-};
-
-/**
- * ADC messages
- */
-// ADC Values
-struct adc_values_pkt {
- uint16_t val1;
- uint16_t val2;
- uint16_t val3;
- uint16_t val4;
-} __attribute__((packed));
-
-/**
- * Battery monitoring
- */
-struct battery_status_pkt {
- uint16_t elem[4]; // in 1/10000th volts [0; 6.5536[
-} __attribute__((packed));
-
-
-/**
- * AX-12
- */
-struct ax12_state_pkt {
- uint8_t address;
- uint16_t position;
- uint16_t speed;
- uint16_t torque;
-} __attribute__((packed));
-
-struct ax12_request_state_pkt {
- uint8_t address;
-} __attribute__((packed));
-
-struct ax12_goto_pkt {
- uint8_t address;
- uint16_t position;
- uint16_t speed;
-} __attribute__((packed));
-
-struct ax12_set_torque_enable_pkt {
- uint8_t address;
- uint8_t enable;
-};
-
-/****************************************************************************/
-
-/**
- * Typedefs
- */
-
-typedef union {
- struct beacon_position_pkt p;
- uint32_t d[2];
-} beacon_position;
-
-typedef union {
- struct beacon_lowlevel_position_pkt p;
- uint32_t d[2];
-} beacon_lowlevel_position;
-
-typedef union {
- struct beacon_calibration_pkt p;
- uint32_t d[2];
-} beacon_calibration;
-
-typedef union {
- struct switch_status_pkt p;
- uint32_t d[2];
-} switch_status;
-
-typedef union {
- struct switch_request_pkt p;
- uint32_t d[2];
-} switch_request;
-
-typedef union {
- struct adc_values_pkt p;
- uint32_t d[2];
-} adc_values;
-
-typedef union {
- struct battery_status_pkt p;
- uint32_t d[2];
-} battery_status;
-
-typedef union {
- struct ax12_state_pkt p;
- uint32_t d[2];
-} ax12_state;
-
-typedef union {
- struct ax12_request_state_pkt p;
- uint32_t d[2];
-} ax12_request_state;
-
-typedef union {
- struct ax12_goto_pkt p;
- uint32_t d[2];
-} ax12_goto;
-
-typedef union {
- struct ax12_set_torque_enable_pkt p;
- uint32_t d[2];
-} ax12_set_torque_enable;
-
-#endif
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/can_monitor.h b/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/can_monitor.h
index c829c10..5ac94cb 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/can_monitor.h
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/can_monitor.h
@@ -18,8 +18,8 @@
#include "trajectory_controller.h"
#include "odometry.h"
#include "differential_drive.h"
-#include "can_messages.h"
-#include "can_messages_sensors.h"
+#include "can_messages/can_messages.h"
+#include "can_messages/can_messages_sensors.h"
void canMonitorInit(void);
void can_send_error(uint8_t err1, uint8_t err2);
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 8955dc1..07f4307 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,8 +18,8 @@
#include "differential_drive.h"
//#define PROP_WHEEL_RADIUS 0.049245
-#define PROP_WHEEL_RADIUS_LEFT 0.049207
-#define PROP_WHEEL_RADIUS_RIGHT 0.049283
+#define PROP_WHEEL_RADIUS_RIGHT 0.049207
+#define PROP_WHEEL_RADIUS_LEFT 0.049283
#define PROP_WHEEL_RADIUS ((PROP_WHEEL_RADIUS_RIGHT+PROP_WHEEL_RADIUS_LEFT)/2)
#define PROP_SHAFT_WIDTH 0.22587
@@ -55,6 +55,9 @@ static void init(void)
// Start control of drive motors
tc_init();
+ // Invert motor direction
+ motorInvertDirection(MOTOR3, 1);
+ motorInvertDirection(MOTOR4, 1);
dd_start(CONTROL_ODOMETRY, // Use odometry CONTROL_ODOMETRY for control
PROP_WHEEL_RADIUS_LEFT, PROP_WHEEL_RADIUS_RIGHT, PROP_SHAFT_WIDTH, // Structural parameters
8*2*M_PI, // Absolute wheel speed limitation
@@ -65,9 +68,9 @@ 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 left motor
+ // Initialize right motor
params.G0 = 0.011686;
params.tau = 0.118;
params.k[0] = -3735.7;
@@ -77,18 +80,18 @@ static void init(void)
params.l0[1] = 0.0108;
params.motor = MOTOR3;
params.encoder = ENCODER3;
- mc_new_controller(¶ms, dd_get_left_wheel_generator(), CONTROLLER_MODE_NORMAL);
- // Initialize right motor
+ mc_new_controller(¶ms, dd_get_right_wheel_generator(), CONTROLLER_MODE_NORMAL);
+ // Initialize left 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);
+ 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);
// Start odometrys
odometryInit(0, 1e-3,
PROP_WHEEL_RADIUS_LEFT, PROP_WHEEL_RADIUS_RIGHT,
PROP_SHAFT_WIDTH,
- ENCODER3, ENCODER4,
+ ENCODER4, ENCODER3,
2.0*M_PI/2000.0/15, -2.0*M_PI/2000.0/15);
odometryInit(1, 1e-3,
INDEP_WHEEL_RADIUS, INDEP_WHEEL_RADIUS,
@@ -111,7 +114,7 @@ static void init(void)
}
// Enable motor pump
- enableMotor(MOTOR2);
+ //enableMotor(MOTOR2);
}
static void NORETURN ind_process(void)
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/lib/can_messages/can_messages.h b/elec/boards/Controller_Motor_STM32/Firmwares/lib/can_messages/can_messages.h
new file mode 100644
index 0000000..660edd3
--- /dev/null
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/lib/can_messages/can_messages.h
@@ -0,0 +1,298 @@
+/*
+ * 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
+#define CAN_MSG_CONTROL_ERROR 106 // error_can_msg_t
+#define CAN_MSG_ODOMETRY_INDEP 107 // odometry_can_msg_t
+
+#define CAN_MSG_LIFT_ENCODERS 131 // encoder_can_msg_t
+#define CAN_MSG_PUMP_STATE 132 // pump_state_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_SIMULATION_MODE 205 // simulation_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
+#define CAN_MSG_MOTOR_COMMAND 208 // motor_command_can_msg_t
+#define CAN_MSG_ODOMETRY_INDEP_SET 209 // odometry_can_msg_t
+#define CAN_MSG_CONTROLLER_ACTIVATION 210 // controller_activation_can_msg_t
+#define CAN_MSG_DRIVE_ACTIVATION 211 // controller_activation_can_msg_t
+#define CAN_MSG_TORQUE_LIMIT 212 // torque_limit_can_msg_t
+#define CAN_MSG_DRIVE_TORQUE_LIMIT 213 // drive_torque_limit_can_msg_t
+
+#define CAN_MSG_LIFT_CMD 231 // lift_cmd_msg_t
+#define CAN_MSG_PUMP_CMD 232 // pump_cmd_msg_t
+
+/* +-----------------------------------------------------------------+
+ | Constants for messages |
+ +-----------------------------------------------------------------+ */
+#define SIMULATION_MODE_NO 0
+#define SIMULATION_MODE_NORMAL 1
+#define SIMULATION_MODE_HIL 2
+
+/* +-----------------------------------------------------------------+
+ | 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;
+
+// Error packet
+typedef struct {
+ uint8_t err1;
+ uint8_t err2;
+} error_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 u; // Parameter on the spline between 0 and 255
+ uint8_t state; // 1 if trajectory in progress, 0 else
+} ghost_msg_t;
+
+// Pumps voltage status
+typedef struct {
+ int16_t left_pump __attribute__((__packed__));
+ int16_t right_pump __attribute__((__packed__));
+} pump_state_msg_t;
+
+// Torque limitation on some motors
+typedef struct {
+ uint8_t motor __attribute__((__packed__)); // Logic OR of bits corresponding to motors
+ uint16_t limit __attribute__((__packed__)); // Torque limit (between 0 and 3600)
+} torque_limit_msg_t;
+
+// Torque limitation on propulsion drive
+typedef struct {
+ uint16_t limit __attribute__((__packed__)); // Torque limit (between 0 and 3600)
+} drive_torque_limit_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;
+
+// Controller activation command
+typedef struct {
+ uint8_t motor __attribute__((__packed__)); // Motor ID of the motor to act on
+ uint8_t activate __attribute__((__packed__)); // Activate the controller if non zero
+ // else release the control.
+} controller_activation_msg_t;
+
+// Propulsion Drive activation command
+typedef struct {
+ uint8_t activate __attribute__((__packed__)); // Activate the propulsion drive if non zero
+ // else release the control.
+} drive_activation_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
+ int16_t d1:9 __attribute__((__packed__)); // first branch length in cm
+ uint8_t d2:8 __attribute__((__packed__)); // last branch length in cm
+ int16_t theta_end:12 __attribute__((__packed__)); // end angle in 1/100 radians
+ 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 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;
+
+// Stop Bezier Spline following and brakes
+typedef struct {
+ 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, simulation or HIL)
+typedef struct {
+ uint8_t mode;
+} simulation_mode_msg_t;
+
+// Command the speed of a particular motor
+typedef struct {
+ uint8_t motor_id __attribute__((__packed__));
+ int32_t speed __attribute__((__packed__));
+} motor_command_msg_t;
+
+// Lifts command position, a value is ignored if < 0
+typedef struct {
+ float left_lift __attribute__((__packed__));
+ float right_lift __attribute__((__packed__));
+} lift_cmd_msg_t;
+
+// Pumps voltage command, a value is ignored if < 0
+typedef struct {
+ int16_t left_pump __attribute__((__packed__));
+ int16_t right_pump __attribute__((__packed__));
+} pump_cmd_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 {
+ error_msg_t data;
+ uint32_t data32[2];
+} error_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 {
+ pump_state_msg_t data;
+ uint32_t data32[2];
+} pump_state_can_msg_t;
+
+typedef union {
+ torque_limit_msg_t data;
+ uint32_t data32[2];
+} torque_limit_can_msg_t;
+
+typedef union {
+ drive_torque_limit_msg_t data;
+ uint32_t data32[2];
+} drive_torque_limit_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 {
+ controller_activation_msg_t data;
+ uint32_t data32[2];
+} controller_activation_can_msg_t;
+
+typedef union {
+ drive_activation_msg_t data;
+ uint32_t data32[2];
+} drive_activation_can_msg_t;
+
+typedef union {
+ bezier_msg_t data;
+ uint32_t data32[2];
+} 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;
+
+typedef union {
+ simulation_mode_msg_t data;
+ uint32_t data32[2];
+} simulation_mode_can_msg_t;
+
+typedef union {
+ motor_command_msg_t data;
+ uint32_t data32[2];
+} motor_command_can_msg_t;
+
+typedef union {
+ lift_cmd_msg_t data;
+ uint32_t data32[2];
+} lift_cmd_can_msg_t;
+
+typedef union {
+ pump_cmd_msg_t data;
+ uint32_t data32[2];
+} pump_cmd_can_msg_t;
+
+#endif /* __CAN_MESSAGES_H */
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/lib/can_messages/can_messages_sensors.h b/elec/boards/Controller_Motor_STM32/Firmwares/lib/can_messages/can_messages_sensors.h
new file mode 100644
index 0000000..057833f
--- /dev/null
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/lib/can_messages/can_messages_sensors.h
@@ -0,0 +1,242 @@
+/**
+ * Sensor-Actuator board messages
+ *
+ * Header file for the Sensor and Actuator CAN messages
+ *
+ * Copyright © 2011 Nicolas Dandrimont <ol...@cr...>
+ * Authors: Nicolas Dandrimont <ol...@cr...>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#ifndef CAN_MESSAGES_H__
+#define CAN_MESSAGES_H__
+
+/**
+ * Message IDs
+ */
+
+// Beacon
+
+#define CAN_BEACON_POSITION 301 // beacon_position
+#define CAN_BEACON_LOWLEVEL_POSITION 302 // beacon_lowlevel_position
+#define CAN_BEACON_CALIBRATION 303 // beacon_calibration
+
+// Switches
+
+#define CAN_SWITCH_STATUS_1 311 // switch_status
+#define CAN_SWITCH_STATUS_2 312 // switch_status
+#define CAN_SWITCH_SET 313 // switch_request
+
+
+// ADC
+
+#define CAN_ADC_VALUES_1 321 // adc_values
+#define CAN_ADC_VALUES_2 322 // adc_values
+
+// Battery monitoring
+
+#define CAN_BATTERY_STATUS_1 331 // battery_status
+#define CAN_BATTERY_STATUS_2 332 // battery_status
+
+// AX12
+
+#define CAN_AX12_STATE 341 // ax12_state
+#define CAN_AX12_REQUEST_STATE 342 // ax12_request_state
+#define CAN_AX12_GOTO 343 // ax12_goto
+//#define CAN_AX12_RESET 344 // Not used anymore
+#define CAN_AX12_SET_TORQUE_ENABLE 345 // ax12_set_torque_enable
+
+// Simulation control
+#define CAN_MSG_SIMULATION_MODE 205 // simulation mode
+#define SIMULATION_MODE_NO 0
+#define SIMULATION_MODE_NORMAL 1
+#define SIMULATION_MODE_HIL 2
+
+
+/****************************************************************************/
+
+/**
+ * Message Packets
+ */
+
+/**
+ * Beacon messages
+ */
+
+// Position of the opponent
+struct beacon_position_pkt {
+ uint16_t angle[2]; // in 1/10000th of radians [0; 2*Pi[
+ uint16_t distance[2]; // in mm [0; 65536[
+} __attribute__((packed));
+
+
+
+// Beacon low-level position
+struct beacon_lowlevel_position_pkt {
+ uint16_t angle; // in 1/10000th of radians [0; 2*Pi[
+ uint16_t width; // in 1/100000th of radians [0; Pi/5[
+ uint32_t period; // in timer ticks
+} __attribute__((packed));
+
+
+
+// Beacon calibration
+struct beacon_calibration_pkt {
+ uint16_t width; // in 1/100000th of radians [0; Pi/5[
+ uint16_t distance; // in mm [0; 65536[
+} __attribute__((packed));
+
+
+/**
+ * Switch messages
+ */
+
+// Switch status
+struct switch_status_pkt {
+ uint8_t sw1;
+ uint8_t sw2;
+ uint8_t sw3;
+ uint8_t sw4;
+ uint8_t sw5;
+ uint8_t sw6;
+ uint8_t sw7;
+ uint8_t sw8;
+};
+
+// Switch request
+struct switch_request_pkt {
+ uint8_t num;
+ uint8_t state;
+};
+
+/**
+ * ADC messages
+ */
+// ADC Values
+struct adc_values_pkt {
+ uint16_t val1;
+ uint16_t val2;
+ uint16_t val3;
+ uint16_t val4;
+} __attribute__((packed));
+
+/**
+ * Battery monitoring
+ */
+struct battery_status_pkt {
+ uint16_t elem[4]; // in 1/10000th volts [0; 6.5536[
+} __attribute__((packed));
+
+
+/**
+ * AX-12
+ */
+struct ax12_state_pkt {
+ uint8_t address;
+ uint16_t position;
+ uint16_t speed;
+ uint16_t torque;
+} __attribute__((packed));
+
+struct ax12_request_state_pkt {
+ uint8_t address;
+} __attribute__((packed));
+
+struct ax12_goto_pkt {
+ uint8_t address;
+ uint16_t position;
+ uint16_t speed;
+} __attribute__((packed));
+
+struct ax12_set_torque_enable_pkt {
+ uint8_t address;
+ uint8_t enable;
+};
+
+/**
+ * Simulation mode control
+ */
+struct simulation_mode_pkt {
+ uint8_t mode;
+} __attribute__((packed));
+
+/****************************************************************************/
+
+/**
+ * Typedefs
+ */
+
+typedef union {
+ struct beacon_position_pkt p;
+ uint32_t d[2];
+} beacon_position;
+
+typedef union {
+ struct beacon_lowlevel_position_pkt p;
+ uint32_t d[2];
+} beacon_lowlevel_position;
+
+typedef union {
+ struct beacon_calibration_pkt p;
+ uint32_t d[2];
+} beacon_calibration;
+
+typedef union {
+ struct switch_status_pkt p;
+ uint32_t d[2];
+} switch_status;
+
+typedef union {
+ struct switch_request_pkt p;
+ uint32_t d[2];
+} switch_request;
+
+typedef union {
+ struct adc_values_pkt p;
+ uint32_t d[2];
+} adc_values;
+
+typedef union {
+ struct battery_status_pkt p;
+ uint32_t d[2];
+} battery_status;
+
+typedef union {
+ struct ax12_state_pkt p;
+ uint32_t d[2];
+} ax12_state;
+
+typedef union {
+ struct ax12_request_state_pkt p;
+ uint32_t d[2];
+} ax12_request_state;
+
+typedef union {
+ struct ax12_goto_pkt p;
+ uint32_t d[2];
+} ax12_goto;
+
+typedef union {
+ struct ax12_set_torque_enable_pkt p;
+ uint32_t d[2];
+} ax12_set_torque_enable;
+
+typedef union {
+ struct simulation_mode_pkt p;
+ uint32_t d[2];
+} simulation_mode;
+
+#endif
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/lib/lift_controller.c b/elec/boards/Controller_Motor_STM32/Firmwares/lib/lift_controller.c
index 56cb686..7a50213 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/lib/lift_controller.c
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/lib/lift_controller.c
@@ -41,15 +41,15 @@ void lc_init(void) {
lc_state[i].extend = 0;
lc_state[i].end_stop_ind = 0;
}
- lc_state[LC_FRONT_LIFT].tc_ind = LC_TC_FRONT;
- lc_state[LC_BACK_LIFT].tc_ind = LC_TC_BACK;
+ lc_state[LC_LEFT_LIFT].tc_ind = LC_TC_LEFT;
+ lc_state[LC_RIGHT_LIFT].tc_ind = LC_TC_RIGHT;
// Init Trajectory controllers
- tc_new_controller(LC_TC_FRONT);
- tc_new_controller(LC_TC_BACK);
+ tc_new_controller(LC_TC_LEFT);
+ tc_new_controller(LC_TC_RIGHT);
// Limit PWM value
- motorSetMaxPWM(MOTOR3, 1600);
- motorSetMaxPWM(MOTOR4, 1600);
+ motorSetMaxPWM(MOTOR2, 1800); // Limit to 12V
+ motorSetMaxPWM(MOTOR4, 1800); // Limit to 12V
// Common parameters
params.encoder_gain = 2.0*M_PI/588.0;
params.G0 = 0.0035;
@@ -60,32 +60,32 @@ void lc_init(void) {
params.l0[0] = 0.0091;
params.l0[1] = 1.6361;
params.T = 0.005;
- // Initialize front lift
+ // Initialize left lift
+ params.motor = MOTOR2;
+ params.encoder = ENCODER2;
+ mc_new_controller(¶ms, tc_get_position_generator(LC_TC_LEFT), CONTROLLER_MODE_NORMAL);
+ // Initialize right lift
params.motor = MOTOR4;
params.encoder = ENCODER4;
- mc_new_controller(¶ms, tc_get_position_generator(LC_TC_FRONT), CONTROLLER_MODE_NORMAL);
- // Initialize back lift
- params.motor = MOTOR3;
- params.encoder = ENCODER3;
- mc_new_controller(¶ms, tc_get_position_generator(LC_TC_BACK), CONTROLLER_MODE_NORMAL);
+ mc_new_controller(¶ms, tc_get_position_generator(LC_TC_RIGHT), CONTROLLER_MODE_NORMAL);
}
void lc_end_stop_reached(uint8_t end_stops) {
// Set the end stop indicators and stop the lift if needed
- if ((end_stops & LC_FRONT_UP) || (end_stops & LC_FRONT_BOTTOM)) {
- if ((end_stops & LC_FRONT_UP) && (lc_state[LC_FRONT_LIFT].direction == 1))
- tc_stop(LC_TC_FRONT);
- if ((end_stops & LC_FRONT_BOTTOM) && (lc_state[LC_FRONT_LIFT].direction == -1))
- tc_s...
[truncated message content] |
|
From: Xavier L. <Ba...@us...> - 2013-10-21 18:17:38
|
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 233667799df4892865de6877584ef3c9ac89dc6c (commit)
from ce758846dbc2dfaf0e1d50ae203a0e955dc33aaf (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 233667799df4892865de6877584ef3c9ac89dc6c
Author: Xavier Lagorce <Xav...@cr...>
Date: Mon Oct 21 20:15:27 2013 +0200
[info/examples] Added a simple top level example to use AX12s
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/examples/krobot_ax12.ml b/info/control2011/examples/krobot_ax12.ml
new file mode 100644
index 0000000..e5aa940
--- /dev/null
+++ b/info/control2011/examples/krobot_ax12.ml
@@ -0,0 +1,73 @@
+#use "topfind";;
+#camlp4o;;
+#require "lwt.syntax";;
+#require "krobot";;
+open Krobot_bus;;
+open Krobot_message;;
+open Krobot_action;;
+open Krobot_geom;;
+
+let ax12_2_base_position = 519;;
+let ax12_2_high_position = 210;;
+let ax12_2_mid_position = 400;;
+let ax12_1_mid_position = 600;;
+let ax12_1_base_position = 518;;
+let ax12_1_high_position = 823;;
+
+let send cmd = ignore(Lwt_unix.run (Krobot_bus.send bus (Unix.gettimeofday (), cmd)))
+let send_strat strat = ignore(Lwt_unix.run (Krobot_bus.send bus (Unix.gettimeofday (), Strategy_set strat)))
+
+let update_team_led team =
+ let m1,m2 =
+ if team = `Red then
+ Switch_request(7,false), Switch_request(6,true)
+ else
+ Switch_request(7,true), Switch_request(6,false)
+ in
+ lwt () = Krobot_message.send bus (Unix.gettimeofday (),m1) in
+ Krobot_message.send bus (Unix.gettimeofday (), m2)
+
+let hit_ax12_id id =
+ let base, high =
+ if id = 1
+ then ax12_1_base_position, ax12_1_high_position
+ else ax12_2_base_position, ax12_2_high_position
+ in
+ Node (Simple,
+ [Can (Krobot_message.encode (Ax12_Set_Torque_Enable (id,true)));
+ Wait_for 0.1;
+ Can (Krobot_message.encode (Ax12_Goto (id, high, 0)));
+ Wait_for 0.3;
+ Can (Krobot_message.encode (Ax12_Goto (id, base, 0)));
+ Wait_for 0.3])
+
+let ax12_activate id status =
+ send_strat ([Node (Simple, [
+ Can (Krobot_message.encode (Ax12_Set_Torque_Enable (id, status)))
+ ])
+ ])
+
+let ax12_goto id angle speed =
+ send_strat ([Node (Simple, [
+ Can (Krobot_message.encode (Ax12_Goto (id, angle, speed)))
+ ])
+ ])
+
+let hit_side side =
+ (if side = 1 then
+ run_lwt (update_team_led `Red)
+ else
+ run_lwt (update_team_led `Blue));
+ let strat = [ hit_ax12_id side ] in
+ run_lwt (
+ Krobot_bus.send bus (Unix.gettimeofday (), Strategy_set strat)
+ )
+
+let () = hit_side 1
+let () = hit_side 2
+
+let () = ax12_activate 1 true
+let () = ax12_goto 1 ax12_1_high_position 300
+let () = ax12_goto 1 ax12_1_mid_position 300
+let () = ax12_goto 1 ax12_1_base_position 0
+let () = ax12_activate 1 false
hooks/post-receive
--
krobot
|
|
From: Xavier L. <Ba...@us...> - 2013-05-22 19:40:00
|
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 ce758846dbc2dfaf0e1d50ae203a0e955dc33aaf (commit)
via 295eeed1694fcd66c866a47adf80d185a5e006ed (commit)
via 1469f324204351a9fed364e7cd255536c1192bc2 (commit)
via 151f042d69b02a6f65a4ece42a5fb3bdba392546 (commit)
via 63aabe085353534abc4dba4d0ef0ffb5d04d1801 (commit)
via 3e283fa22033418ebf6c4d103c7e776301193f62 (commit)
via 4d0245d3a58ad206462f821ec5c3aeacc4b83ecb (commit)
via 31786065d863cf2d4aa07eb5cd7de558db72b4f0 (commit)
via c2c799d2a17fb401b88bc2849cc1f1841759486e (commit)
via c08a114cea56f74ab6b2f05c801ccc51298db979 (commit)
via 3bf0f7ea516321571c2aed4e4e7183643452575b (commit)
via 7dbe121c133f23ab4a6138e3dc96b930bac06d57 (commit)
via eb7dd7a042219ec1367ab1e4c70638abfb967aba (commit)
via 533f1301506c201c4eee01e880188aef30e46795 (commit)
via 290770ae636671de184457766135eac3185507bd (commit)
via d9d6be1512ead188854051362a1c02d3e98b430f (commit)
via 2b85f9f2fb495bdc813093536d4dfb118b317a93 (commit)
via 41de715dce9c59ddd5e01ad2e46cf9ca78cc2199 (commit)
via 4a99bb6f314d8fd380c838aca948c14fa74b3644 (commit)
via 928be7aa366f0f3dc8bddfa795fe8d770dd66d93 (commit)
via e9af8ac8951ba2f07d3c42b2cca4760635c49ca9 (commit)
via 8b3c3a760cfe86119055ab8a4efe0ef32708ba39 (commit)
via 6b10692ae39b4877fcbb3aac0ea13a4f705983e2 (commit)
via 21f4c7294724d3dd47cda68d1449f0c8ac0cc1f7 (commit)
via 7b0b7c21e1651c41b78feefad032dc9a7979ff3a (commit)
via 04b81baf4f9b35178c3ab8be28a188039df38447 (commit)
via 0c2fdf06b1b22b3fad45aed19e5340503d5e7c6b (commit)
via b6e98afc67a9ef75084b799937b41098aefbc941 (commit)
via e585caecbceac260f2445e29cc8a0b749e9e6f29 (commit)
via 5e1b821afc9a263e6d6ca62f147be3bc5f0f9a5c (commit)
via 5969c55d6a482da3c5ca64b297534ddc169d2f6b (commit)
via 4b9de7fd9542e1146be0c2acdf8f25531bb63ffe (commit)
via f6536242e8ccfa1a1be748f4d39f676ffdef4043 (commit)
via 6e85c22cba2f58f6fc3ca3e79531eba7c65c58a6 (commit)
via c03ebbe9394f6e3d6a2c6f259af7784201946857 (commit)
via a664d667f121bc3b28987193ace0c70e7e71b386 (commit)
via a4574aae138c61257232e728da0f70998ca57afa (commit)
via 06174d40f9dde8634a703d6d9e27826c9e91269a (commit)
via 53c2b745afb88b6702b451ca23e40610622fa961 (commit)
via c59ad622b32fe84e054198a44cccbeaabaee29b8 (commit)
via bb9d6760a512f97c12324052b9328668ccffeb07 (commit)
via 98ff61ee2de1ebd6c4a90a4d4d25428b054aa72e (commit)
via 01f7336a3ed8c3eab82b646dfc7d8dc4d723545f (commit)
via d3cf53369367cb157e87a197f8e417ac8f7c6569 (commit)
from bc96bdd7d9d8342fe018fe76ff3bf9b5a6fdc3c7 (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 ce758846dbc2dfaf0e1d50ae203a0e955dc33aaf
Author: Pierre Chambart <pie...@oc...>
Date: Sat May 11 10:05:24 2013 +0200
filter urg
commit 295eeed1694fcd66c866a47adf80d185a5e006ed
Author: Pierre Chambart <pie...@oc...>
Date: Sat May 11 09:34:45 2013 +0200
plop
commit 1469f324204351a9fed364e7cd255536c1192bc2
Author: Pierre Chambart <pie...@oc...>
Date: Sat May 11 09:20:33 2013 +0200
test
commit 151f042d69b02a6f65a4ece42a5fb3bdba392546
Author: Pierre Chambart <pie...@oc...>
Date: Fri May 10 17:17:06 2013 +0200
Move back action, more things in homologation
commit 63aabe085353534abc4dba4d0ef0ffb5d04d1801
Author: Pierre Chambart <pie...@oc...>
Date: Fri May 10 15:51:19 2013 +0200
more on xbee
commit 3e283fa22033418ebf6c4d103c7e776301193f62
Author: Pierre Chambart <pie...@oc...>
Date: Fri May 10 15:51:08 2013 +0200
generate trajectories that does not go backward
commit 4d0245d3a58ad206462f821ec5c3aeacc4b83ecb
Author: Pierre Chambart <pie...@oc...>
Date: Fri May 10 07:42:38 2013 +0200
Homologation: infinite loop
commit 31786065d863cf2d4aa07eb5cd7de558db72b4f0
Author: Pierre Chambart <pie...@oc...>
Date: Fri May 10 07:16:10 2013 +0200
More flexible node, allow n retries/skip/loop
commit c2c799d2a17fb401b88bc2849cc1f1841759486e
Author: Pierre Chambart <pie...@oc...>
Date: Fri May 10 06:22:13 2013 +0200
Finish krobot-xbee
commit c08a114cea56f74ab6b2f05c801ccc51298db979
Author: Pierre Chambart <pie...@oc...>
Date: Fri May 10 02:31:21 2013 +0200
Stuff start
commit 3bf0f7ea516321571c2aed4e4e7183643452575b
Author: Pierre Chambart <pie...@oc...>
Date: Fri May 10 02:30:52 2013 +0200
Start xbee
commit 7dbe121c133f23ab4a6138e3dc96b930bac06d57
Author: Pierre Chambart <pie...@oc...>
Date: Fri May 10 01:23:07 2013 +0200
Little bit smarter
commit eb7dd7a042219ec1367ab1e4c70638abfb967aba
Author: Pierre Chambart <pie...@oc...>
Date: Fri May 10 00:52:17 2013 +0200
Clean some dead code (since 2011)
commit 533f1301506c201c4eee01e880188aef30e46795
Author: Pierre Chambart <pie...@oc...>
Date: Fri May 10 00:45:27 2013 +0200
Many fixes, better startegy
commit 290770ae636671de184457766135eac3185507bd
Author: Pierre Chambart <pie...@oc...>
Date: Thu May 9 20:04:28 2013 +0200
Add cake to the table extraction
commit d9d6be1512ead188854051362a1c02d3e98b430f
Author: Pierre Chambart <pie...@oc...>
Date: Thu May 9 20:04:06 2013 +0200
Urg replay
commit 2b85f9f2fb495bdc813093536d4dfb118b317a93
Author: Pierre Chambart <pie...@oc...>
Date: Thu May 9 17:38:37 2013 +0200
Multiple fix
commit 41de715dce9c59ddd5e01ad2e46cf9ca78cc2199
Author: Pierre Chambart <pie...@oc...>
Date: Thu May 9 13:38:26 2013 +0200
krobot_urg_extract: Filter when only one point
commit 4a99bb6f314d8fd380c838aca948c14fa74b3644
Author: Pierre Chambart <pie...@oc...>
Date: Thu May 9 13:34:17 2013 +0200
Return to nice homologation
commit 928be7aa366f0f3dc8bddfa795fe8d770dd66d93
Author: Pierre Chambart <pie...@oc...>
Date: Thu May 9 13:28:36 2013 +0200
Fix vm explosions
commit e9af8ac8951ba2f07d3c42b2cca4760635c49ca9
Author: Pierre Chambart <pie...@oc...>
Date: Thu May 9 11:45:12 2013 +0200
homo
commit 8b3c3a760cfe86119055ab8a4efe0ef32708ba39
Author: Pierre Chambart <pie...@oc...>
Date: Thu May 9 10:34:29 2013 +0200
Hack homologation
commit 6b10692ae39b4877fcbb3aac0ea13a4f705983e2
Author: Pierre Chambart <pie...@oc...>
Date: Thu May 9 10:09:36 2013 +0200
Fix homologation: changing switch reset timer
commit 21f4c7294724d3dd47cda68d1449f0c8ac0cc1f7
Author: Xavier Lagorce <Xav...@cr...>
Date: Thu May 9 13:26:40 2013 +0200
[krobot-vm] Blink all LEDs if emergency stop button if pressed
commit 7b0b7c21e1651c41b78feefad032dc9a7979ff3a
Author: Xavier Lagorce <Xav...@cr...>
Date: Thu May 9 08:53:49 2013 +0200
[krobot-viewer] Change robot's color for better visibility
commit 04b81baf4f9b35178c3ab8be28a188039df38447
Author: Pierre Chambart <pie...@oc...>
Date: Thu May 9 08:48:30 2013 +0200
Change team led: red on red
commit 0c2fdf06b1b22b3fad45aed19e5340503d5e7c6b
Author: Pierre Chambart <pie...@oc...>
Date: Thu May 9 08:44:57 2013 +0200
Remove safety margin: we start too close from the wall to have one...
commit b6e98afc67a9ef75084b799937b41098aefbc941
Author: Xavier Lagorce <Xav...@cr...>
Date: Thu May 9 08:27:34 2013 +0200
[krobot-vm] Fixed "typo"
commit e585caecbceac260f2445e29cc8a0b749e9e6f29
Author: Pierre Chambart <pie...@oc...>
Date: Thu May 9 08:09:59 2013 +0200
Optimise Icp_utils.far_enougth_filter
commit 5e1b821afc9a263e6d6ca62f147be3bc5f0f9a5c
Author: Xavier Lagorce <Xav...@cr...>
Date: Thu May 9 07:47:41 2013 +0200
[Krobot_vm] Don't start the pump at the beggining of the match
commit 5969c55d6a482da3c5ca64b297534ddc169d2f6b
Author: Pierre Chambart <pie...@oc...>
Date: Thu May 9 00:56:02 2013 +0200
try something is now an action when goto fail
commit 4b9de7fd9542e1146be0c2acdf8f25531bb63ffe
Author: Pierre Chambart <pie...@oc...>
Date: Thu May 9 00:55:39 2013 +0200
Change homologation
commit f6536242e8ccfa1a1be748f4d39f676ffdef4043
Author: Pierre Chambart <pie...@oc...>
Date: Thu May 9 00:55:20 2013 +0200
Fix initial position
commit 6e85c22cba2f58f6fc3ca3e79531eba7c65c58a6
Author: Pierre Chambart <pie...@oc...>
Date: Thu May 9 00:10:01 2013 +0200
Fix ax12 position in homologation
commit c03ebbe9394f6e3d6a2c6f259af7784201946857
Author: Pierre Chambart <pie...@oc...>
Date: Thu May 9 00:09:27 2013 +0200
Update config
commit a664d667f121bc3b28987193ace0c70e7e71b386
Author: Pierre Chambart <pie...@oc...>
Date: Wed May 8 23:30:57 2013 +0200
fix homologation
commit a4574aae138c61257232e728da0f70998ca57afa
Author: Pierre Chambart <pie...@oc...>
Date: Wed May 8 23:28:20 2013 +0200
Fix end of match
commit 06174d40f9dde8634a703d6d9e27826c9e91269a
Author: Pierre Chambart <pie...@oc...>
Date: Wed May 8 23:28:00 2013 +0200
Homologation with baloon
commit 53c2b745afb88b6702b451ca23e40610622fa961
Author: Pierre Chambart <pie...@oc...>
Date: Wed May 8 23:27:25 2013 +0200
filter urg data to avoid spurious points
commit c59ad622b32fe84e054198a44cccbeaabaee29b8
Author: Xavier Lagorce <Xav...@cr...>
Date: Wed May 8 23:24:53 2013 +0200
[Krobot_viewer] Draw 2013 Table (better late than never...)
commit bb9d6760a512f97c12324052b9328668ccffeb07
Author: Pierre Chambart <pie...@oc...>
Date: Wed May 8 21:56:46 2013 +0200
Trivial homologation program
commit 98ff61ee2de1ebd6c4a90a4d4d25428b054aa72e
Author: Pierre Chambart <pie...@oc...>
Date: Wed May 8 19:58:27 2013 +0200
See and avoid objects
commit 01f7336a3ed8c3eab82b646dfc7d8dc4d723545f
Author: Pierre Chambart <pie...@oc...>
Date: Wed May 8 18:17:14 2013 +0200
Fix build
commit d3cf53369367cb157e87a197f8e417ac8f7c6569
Author: Pierre Chambart <pie...@oc...>
Date: Wed May 8 18:10:48 2013 +0200
krobot_urg_extract: extract objects from urg captures
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/_oasis b/info/control2011/_oasis
index 85d5e9e..adcbdf0 100644
--- a/info/control2011/_oasis
+++ b/info/control2011/_oasis
@@ -34,6 +34,10 @@ Flag sdl
Description: enable sdl
Default: true
+Flag lacaml
+ Description: compile icp depending on lacaml
+ Default: true
+
# +-------------------------------------------------------------------+
# | Libraries |
# +-------------------------------------------------------------------+
@@ -77,6 +81,8 @@ Library "krobot-can"
Library "krobot-icp"
FindlibName: icp
FindlibParent: krobot
+ Build$: flag(lacaml)
+ Install$: flag(lacaml)
BuildDepends: lacaml
XMETADescription: 2d Point cloud registration
XMETARequires: lacaml
@@ -317,6 +323,22 @@ Executable "krobot-urg"
MainIs: krobot_urg.ml
BuildDepends: krobot, urg, lwt.syntax, lwt.preemptive, threads
+Executable "krobot-urg-extract"
+ Path: src/tools
+ Build$: flag(lacaml)
+ Install$: flag(lacaml)
+ CompiledObject: best
+ MainIs: krobot_urg_extract.ml
+ BuildDepends: krobot, krobot.icp, lwt.syntax
+
+Executable "krobot-urg-replay"
+ Path: src/tools
+ Build$: flag(lacaml)
+ Install$: flag(lacaml)
+ CompiledObject: best
+ MainIs: krobot_urg_replay.ml
+ BuildDepends: krobot, lwt.syntax, krobot.icp
+
Executable "krobot-beacon-reader"
Path: src/tools
Install: true
@@ -324,6 +346,13 @@ Executable "krobot-beacon-reader"
MainIs: krobot_beacon_reader.ml
BuildDepends: krobot, lwt.syntax, lwt.unix
+Executable "krobot-xbee"
+ Path: src/tools
+ Install: true
+ CompiledObject: best
+ MainIs: krobot_xbee.ml
+ BuildDepends: krobot, lwt.syntax, lwt.unix, str
+
# +-------------------------------------------------------------------+
# | Examples |
# +-------------------------------------------------------------------+
diff --git a/info/control2011/_tags b/info/control2011/_tags
index 4b773d9..7aa1784 100644
--- a/info/control2011/_tags
+++ b/info/control2011/_tags
@@ -1,6 +1,6 @@
<**/*.ml{,i}>: syntax_camlp4o
<src/interfaces/*.ml>: -syntax_camlp4o
-<src/icp/*.ml>: -syntax_camlp4o
+<src/icp/*.ml{,i}>: -syntax_camlp4o
# OASIS_START
# DO NOT EDIT (digest: 9a283fb86531fb8de178da3ad5f872b1)
diff --git a/info/control2011/src/icp/icp_utils.ml b/info/control2011/src/icp/icp_utils.ml
index a961e5e..b85b81b 100644
--- a/info/control2011/src/icp/icp_utils.ml
+++ b/info/control2011/src/icp/icp_utils.ml
@@ -21,6 +21,12 @@ let shuffle l =
let l1,l2 = split ([],[]) l in
merge [] l1 (List.rev l2)
+let circle (dx,dy) r a0 an n =
+ let angle_ramp = linear a0 an n in
+ let x = Array.map (fun theta -> r *. cos theta +. dx) angle_ramp in
+ let y = Array.map (fun theta -> r *. sin theta +. dy) angle_ramp in
+ x,y
+
let table ~width ~length n =
let x1,y1 = line (0.,0.) (length,0.) n in
let x2,y2 = line (length,0.) (length,width) n in
@@ -31,6 +37,24 @@ let table ~width ~length n =
let f a = Array.of_list (shuffle (shuffle (Array.to_list a))) in
{ dx = f x; dy = f y }
+let pi = 3.14159265358979323
+
+let real_table n =
+ let length = 3. in
+ let width = 2. in
+ let x1,y1 = line (0.,0.) (length,0.) n in
+ let x2,y2 = line (length,0.) (length,width) n in
+ let x3,y3 = line (0.,width) (0.,0.) n in
+ let x4,y4 = line (0.,width) (1.,width) (n/2) in
+ let x5,y5 = line (2.,width) (length,width) (n/2) in
+ let x6,y6 = circle (1.5,2.0) 0.5 (-.pi) 0. n in
+ let x7,y7 = line (0.,0.1) (0.4,0.1) (n/4) in
+ let x8,y8 = line (3.,0.1) (2.6,0.1) (n/4) in
+ let x = Array.concat [x1;x2;x3;x4;x5;x6;x7;x8] in
+ let y = Array.concat [y1;y2;y3;y4;y5;y6;y7;y8] in
+ let f a = Array.of_list (shuffle (shuffle (Array.to_list a))) in
+ { dx = f x; dy = f y }
+
(**** dump loading ****)
@@ -76,18 +100,39 @@ let load_file ?(min_dist=0.15) ?(max_dist=6.) f =
ts, { dx = Array.of_list x; dy = Array.of_list y }) l in
Array.of_list l'
+let load_raw_file f =
+ let l = load_file' f in
+ let l' =
+ List.map (fun (ts,v) ->
+ let x,y = List.split v in
+ ts, { dx = Array.of_list x; dy = Array.of_list y }) l in
+ Array.of_list l'
(**** filtering ****)
-let far_enougth_filter kd a min_dist data =
- let dist = distance_transform (fun i -> i) kd a data in
- let dist = Array.mapi (fun i d -> i,d) dist in
- let distl = Array.to_list dist in
- let min_dist2 = min_dist *. min_dist in
- let far_enougth (i,d) = d >= min_dist2 in
- let ai = Array.of_list (List.filter far_enougth distl) in
- { dx = Array.map (fun (i,_) -> data.dx.(i)) ai;
- dy = Array.map (fun (i,_) -> data.dy.(i)) ai }
+(* TODO: can be done more efficiently using Kd_tree.closer *)
+(* let far_enougth_filter kd a min_dist data = *)
+(* let dist = distance_transform (fun i -> i) kd a data in *)
+(* let dist = Array.mapi (fun i d -> i,d) dist in *)
+(* let distl = Array.to_list dist in *)
+(* let min_dist2 = min_dist *. min_dist in *)
+(* let far_enougth (i,d) = d >= min_dist2 in *)
+(* let ai = Array.of_list (List.filter far_enougth distl) in *)
+(* { dx = Array.map (fun (i,_) -> data.dx.(i)) ai; *)
+(* dy = Array.map (fun (i,_) -> data.dy.(i)) ai } *)
+
+let far_enougth_filter kd min_dist data =
+ let dx = data.dx in
+ let dy = data.dy in
+ let l = ref [] in
+ for i = 0 to Array.length dx - 1 do
+ if not (Kd_tree.closer min_dist { Kd_tree.x = dx.(i); y = dy.(i) } kd)
+ then l := i :: !l
+ done;
+ let l = Array.of_list !l in
+ let x = Array.map (fun i -> dx.(i)) l in
+ let y = Array.map (fun i -> dy.(i)) l in
+ { dx = x; dy = y }
let invert_transform a =
let co = cos (-. a.ath) in
diff --git a/info/control2011/src/icp/icp_utils.mli b/info/control2011/src/icp/icp_utils.mli
index 1e23bba..55c30e3 100644
--- a/info/control2011/src/icp/icp_utils.mli
+++ b/info/control2011/src/icp/icp_utils.mli
@@ -2,15 +2,19 @@ open Icp_minimisation
val table : width:float -> length:float -> int -> data
+val real_table : int -> data
+
val load_file : ?min_dist:float -> ?max_dist:float -> string ->
(float * data) array
(** load a dump file in the format dumpped by [krobot_urg -listen] *)
+val load_raw_file : string -> (float * data) array
+(** load a dump file in the format dumpped by [krobot_urg -listen] *)
(* filtering *)
-val far_enougth_filter : 'a Kd_tree.t -> a -> float -> data -> data
-(** [far_enougth_filter kd a min_dist data] filter out values of
- [data] that are closer than [min_dist] to a vertex of kd *)
+val far_enougth_filter : 'a Kd_tree.t -> float -> data -> data
+(** [far_enougth_filter kd min_dist data] filter out values of [data]
+ that are closer than [min_dist] to a vertex of kd *)
val invert_transform : a -> a
diff --git a/info/control2011/src/icp/kd_tree.ml b/info/control2011/src/icp/kd_tree.ml
index 45601d4..96631a7 100644
--- a/info/control2011/src/icp/kd_tree.ml
+++ b/info/control2011/src/icp/kd_tree.ml
@@ -98,6 +98,58 @@ let nearest_neighbor v = function
let curr = (med_val, median_elt, sq_dist median_elt v) in
nearest_neighbor' v curr t
+let rec closer' dist_sq v t = match t with
+ | Empty -> false
+ | Leaf (va,p) -> sq_dist v p <= dist_sq
+ | Split (dir, med_val, median_elt, left, right) ->
+ let split_distance = match dir with
+ | Hori -> sq (v.x -. median_elt.x)
+ | Vert -> sq (v.y -. median_elt.y) in
+ if split_distance <= dist_sq
+ then begin
+ if sq_dist v median_elt <= dist_sq
+ then true
+ else closer' dist_sq v left || closer' dist_sq v right
+ end
+ else
+ let cmp = match dir with
+ | Hori -> compare v.x median_elt.x
+ | Vert -> compare v.y median_elt.y in
+ if cmp > 0
+ then closer' dist_sq v right
+ else closer' dist_sq v left
+
+let closer dist v t = closer' (sq dist) v t
+
+let add_closer dist_sq v p va acc =
+ if sq_dist v p <= dist_sq
+ then va::acc
+ else acc
+
+let rec closer_points' dist_sq v t acc = match t with
+ | Empty -> acc
+ | Leaf (va,p) ->
+ add_closer dist_sq v p va acc
+ | Split (dir, med_val, median_elt, left, right) ->
+ let split_distance = match dir with
+ | Hori -> sq (v.x -. median_elt.x)
+ | Vert -> sq (v.y -. median_elt.y) in
+ if split_distance <= dist_sq
+ then begin
+ let acc = add_closer dist_sq v median_elt med_val acc in
+ let acc = closer_points' dist_sq v left acc in
+ closer_points' dist_sq v right acc
+ end
+ else
+ let cmp = match dir with
+ | Hori -> compare v.x median_elt.x
+ | Vert -> compare v.y median_elt.y in
+ if cmp > 0
+ then closer_points' dist_sq v right acc
+ else closer_points' dist_sq v left acc
+
+let closer_points dist v t = closer_points' (sq dist) v t []
+
let rec depth = function
| Split (_, _, _, t1,t2 ) ->
1 + max (depth t1) (depth t2)
diff --git a/info/control2011/src/icp/kd_tree.mli b/info/control2011/src/icp/kd_tree.mli
index 947b1ad..9fd0f7c 100644
--- a/info/control2011/src/icp/kd_tree.mli
+++ b/info/control2011/src/icp/kd_tree.mli
@@ -12,3 +12,11 @@ val nearest_neighbor : vertice -> 'a t -> 'a * vertice * float
[vert] *)
val depth : 'a t -> int
+
+val closer : float -> vertice -> 'a t -> bool
+(** [closer dist vert t] returns true if there is a point in [t]
+ closer than [dist] of [vert] *)
+
+val closer_points : float -> vertice -> 'a t -> 'a list
+(** [closer_points dist vert t] returns the lis tof all points in [t]
+ closer than [dist] of [vert] *)
diff --git a/info/control2011/src/lib/krobot_action.ml b/info/control2011/src/lib/krobot_action.ml
index 8b86239..290f661 100644
--- a/info/control2011/src/lib/krobot_action.ml
+++ b/info/control2011/src/lib/krobot_action.ml
@@ -10,15 +10,24 @@
open Printf
open Krobot_geom
-type t =
- | Node of t option * t list
+type node_kind =
+ | Simple
+ | Retry of int * t
+ | Loop of t
+ | Next
+
+and t =
+ | Node of node_kind * t list
| Stop
| Think
| Goto of vertice * vector option
+ | Simple_goto of vertice * vector option
+ | Random_move of ( vertice * vertice )
| 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
+ | Move_back of float
+ | Set_curve of (bool * Bezier.curve) option
| Wait_for_jack of bool
| Wait_for_bezier_moving of bool * float option
| Wait_for_motors_moving of bool * float option
@@ -27,18 +36,11 @@ type t =
| Try_something of vertice
| Fail
| Wait_for_odometry_reset of [ `Red | `Blue | `Auto ]
- | Load of [ `Front | `Back ]
- | Lift_down of [ `Front | `Back ]
- | Lift_up of [ `Front | `Back ]
- | Open_grip_low of [ `Front | `Back ]
- | Close_grip_low of [ `Front | `Back ]
- | Open_grip_high of [ `Front | `Back ]
- | Close_grip_high of [ `Front | `Back ]
| Wait_for of float
| Wait_until of float
- | Wait_for_grip_open_low of [ `Front | `Back ]
- | Wait_for_grip_close_low of [ `Front | `Back ]
| Start_timer of float * t list
+ | Stop_timer
+ | Start_match
| Can of Krobot_can.frame
| Set_led of [ `Red | `Yellow | `Green ] * bool
| Set_orientation of float
@@ -58,14 +60,27 @@ let string_of_face = function
| `Back -> "`Back"
let rec to_string = function
- | Node (t,l) ->
- sprintf "Node [%s, %s]" (string_of_option to_string t) (list_to_string l)
+ | Node (Simple,l) ->
+ sprintf "Node [%s]" (list_to_string l)
+ | Node (Next,l) ->
+ sprintf "Node [Next,%s]" (list_to_string l)
+ | Node (Retry(n,l'),l) ->
+ sprintf "Node [%i, %s, %s]"
+ n (to_string l') (list_to_string l)
+ | Node (Loop t,l) ->
+ sprintf "Node [loop, %s, %s]" (to_string t) (list_to_string l)
| Stop ->
"Stop"
| Think ->
"Think"
+ | Move_back f ->
+ sprintf "Move_back %f" f
+ | Random_move (v1,v2) ->
+ sprintf "Random move %s %s" (string_of_vertice v1) (string_of_vertice v2)
| Goto (v,vect) ->
sprintf "Goto %s %s" (string_of_vertice v) (string_of_option string_of_vector vect)
+ | Simple_goto (v,vect) ->
+ sprintf "Simple_goto %s %s" (string_of_vertice v) (string_of_option string_of_vector vect)
| 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"
@@ -83,8 +98,8 @@ let rec to_string = function
(string_of_vertice r)
(string_of_vertice s)
end_velocity
- | Set_curve(Some c) ->
- sprintf "Set_curve(Some %s)" (Bezier.string_of_curve c)
+ | Set_curve(Some (dir,c)) ->
+ sprintf "Set_curve(Some (%b, %s))" dir (Bezier.string_of_curve c)
| Set_curve None ->
"Set_curve None"
| Wait_for_jack st ->
@@ -115,30 +130,14 @@ let rec to_string = function
"Wait_for_odometry_reset `Blue"
| Wait_for_odometry_reset `Auto ->
"Wait_for_odometry_reset `Auto"
- | Load face ->
- sprintf "Load %s" (string_of_face face)
- | Lift_down face ->
- sprintf "Lift_down %s" (string_of_face face)
- | Lift_up face ->
- sprintf "Lift_up %s" (string_of_face face)
- | Open_grip_low face ->
- sprintf "Open_grip_low %s" (string_of_face face)
- | Close_grip_low face ->
- sprintf "Close_grip_low %s" (string_of_face face)
- | Open_grip_high face ->
- sprintf "Open_grip_high %s" (string_of_face face)
- | Close_grip_high face ->
- sprintf "Close_grip_high %s" (string_of_face face)
| Wait_for t ->
sprintf "Wait_for %f" t
| Wait_until t ->
sprintf "Wait_until %f" t
- | Wait_for_grip_open_low face ->
- sprintf "Wait_for_grip_open_low %S" (string_of_face face)
- | Wait_for_grip_close_low face ->
- sprintf "Wait_for_grip_close_low %S" (string_of_face face)
| Start_timer (delay,t) ->
sprintf "Start_timer(%f,%s)" delay (list_to_string t)
+ | Stop_timer ->
+ sprintf "Stop_timer"
| Can c -> "Can"
| Try_something v ->
sprintf "Try_something %s" (string_of_vertice v)
@@ -152,5 +151,6 @@ let rec to_string = function
(string_of_option string_of_float o)
| Calibrate (_,_,_,_,_,_) -> "Calibrate"
| End -> "End"
+ | Start_match -> "Start_match"
and list_to_string l = String.concat "; " (List.map to_string l)
diff --git a/info/control2011/src/lib/krobot_action.mli b/info/control2011/src/lib/krobot_action.mli
index a2ebb91..0ec9114 100644
--- a/info/control2011/src/lib/krobot_action.mli
+++ b/info/control2011/src/lib/krobot_action.mli
@@ -11,9 +11,16 @@
open Krobot_geom
+type node_kind =
+ | Simple
+ | Retry of int * t
+ | Loop of t
+ | Next
+
(** Type of actions. *)
-type t =
- | Node of t option * t list
+and t =
+ | Node of node_kind * t list
+
(** A sequence of action to execute in order. *)
| Stop
(** Stop all actions. *)
@@ -24,6 +31,10 @@ type t =
(** Go to the given point.
if the bool parameter is true, the path is inverted according
to the robot team *)
+ | Simple_goto of vertice * vector option
+ (** Same as Goto but can fail when obstacles block the trajectory *)
+
+ | Random_move of ( vertice * vertice )
| Set_limits of float * float * float * float
(** limit the speed *)
@@ -40,7 +51,8 @@ type t =
(** Follow the bezier curve determined by the given four
vertices. The first float is the sign, the last one is the
end velocity. *)
- | Set_curve of Bezier.curve option
+ | Move_back of float
+ | Set_curve of (bool * Bezier.curve) option
(** Set the curve currently being followed. *)
| Wait_for_jack of bool
(** Wait for the jack to be in the given state. *)
@@ -65,30 +77,14 @@ type t =
| Wait_for_odometry_reset of [ `Red | `Blue | `Auto ]
(** Wait for the odometry to say it is in red or blue initial position. *)
- | Load of [ `Front | `Back ]
- (** Load a pawn. *)
- | Lift_down of [ `Front | `Back ]
- (** Move the front or back lift down. *)
- | Lift_up of [ `Front | `Back ]
- (** Move the front or back lift up. *)
- | Open_grip_low of [ `Front | `Back ]
- (** Open the front or back low grip. *)
- | Close_grip_low of [ `Front | `Back ]
- (** Close the front or back low grip. *)
- | Open_grip_high of [ `Front | `Back ]
- (** Open the front or back low grip. *)
- | Close_grip_high of [ `Front | `Back ]
- (** Close the front or back low grip. *)
| Wait_for of float
(** Wait for the given number of seconds. *)
| Wait_until of float
(** Wait until the given date. *)
- | Wait_for_grip_open_low of [ `Front | `Back ]
- (** Wait for the given low grip to be opened. *)
- | Wait_for_grip_close_low of [ `Front | `Back ]
- (** Wait for the given low grip to be opened. *)
| Start_timer of float * t list
+ | Stop_timer
+ | Start_match
| Can of Krobot_can.frame
| Set_led of [ `Red | `Yellow | `Green ] * bool
@@ -110,3 +106,5 @@ type t =
val to_string : t -> string
(** [to_string action] returns the string representation of the
given string. *)
+
+val list_to_string : t list -> string
diff --git a/info/control2011/src/lib/krobot_bus.ml b/info/control2011/src/lib/krobot_bus.ml
index 387f972..25b85b7 100644
--- a/info/control2011/src/lib/krobot_bus.ml
+++ b/info/control2011/src/lib/krobot_bus.ml
@@ -31,7 +31,7 @@ type message =
| Trajectory_simplify of float
| Trajectory_go
| Trajectory_find_path
- | Objects of vertice list
+ | Objects of (vertice*float) list
| Sharps of float array
| Strategy_append of Krobot_action.t list
| Strategy_stop
@@ -45,6 +45,7 @@ type message =
| Urg_lines of (vertice*vertice) array
| Beacon_raw of (int * int * int * int * int * int
* int * int * int * int * int)
+ | Match_start
type t = {
oc : Lwt_io.output_channel;
@@ -60,6 +61,9 @@ open Printf
let string_of_vertice v =
sprintf "{ x = %f; y = %f }" v.x v.y
+let string_of_object (v,d) =
+ sprintf "{ x = %f; y = %f; d = %f }" v.x v.y d
+
let string_of_vector v =
sprintf "{ vx = %f; vy = %f }" v.vx v.vy
@@ -109,7 +113,7 @@ let string_of_message = function
| Objects objects ->
sprintf
"Objects [%s]"
- (String.concat "; " (List.map string_of_vertice objects))
+ (String.concat "; " (List.map string_of_object objects))
| Sharps a ->
sprintf
"Sharps [|%s|]"
@@ -156,6 +160,8 @@ let string_of_message = function
sprintf "Urg_lines (many_lines...)"
| Beacon_raw _ ->
sprintf "Raw beacon packet"
+ | Match_start ->
+ sprintf "Match start"
(* +-----------------------------------------------------------------+
| Sending/receiving messages |
diff --git a/info/control2011/src/lib/krobot_bus.mli b/info/control2011/src/lib/krobot_bus.mli
index b04aac3..28a6c1d 100644
--- a/info/control2011/src/lib/krobot_bus.mli
+++ b/info/control2011/src/lib/krobot_bus.mli
@@ -52,7 +52,7 @@ type message =
(** Objects *)
- | Objects of vertice list
+ | Objects of (vertice*float) list
(** The list of objects on the board. *)
(** Sharps *)
@@ -93,6 +93,8 @@ type message =
| Beacon_raw of (int * int * int * int * int * int
* int * int * int * int * int)
+ | Match_start
+
val string_of_message : message -> string
(** Returns a string representation of the given message. *)
diff --git a/info/control2011/src/lib/krobot_config.ml b/info/control2011/src/lib/krobot_config.ml
index e7975da..8eb06b0 100644
--- a/info/control2011/src/lib/krobot_config.ml
+++ b/info/control2011/src/lib/krobot_config.ml
@@ -11,7 +11,7 @@ let sqr x = x *. x
let world_height = 2.
let world_width = 3.
-let robot_length = 0.225
+let robot_length = 0.23
let robot_width = 0.30
let wheels_diameter = 0.098
let wheels_distance = 0.224
@@ -22,7 +22,7 @@ let robot_radius =
sqrt (l1 *. l1 +. l2 *. l2)
let rotary_beacon_index_pos = 0.
-let safety_margin = 0.01
+let safety_margin = 0.0
let beacon_radius = 0.2
@@ -33,14 +33,14 @@ open Krobot_geom
let pi = 4. *. atan 1.
let red_initial_position =
- { x = 0.10;
- y = world_height -. 0.25; },
- 0.
+ { x = world_width -. wheels_position -. 0.001;
+ y = (0.10 +. robot_width/.2.); },
+ pi
let blue_initial_position =
- { x = world_width -. 0.10;
- y = world_height -. 0.25; },
- pi
+ { x = wheels_position +. 0.001;
+ y = (0.10 +. robot_width/.2.); },
+ 0.
let symetrical p =
@@ -65,42 +65,33 @@ let line_obs p1 p2 =
let left_obstacles =
[
- (* the trees *)
- (*{ pos =
- { x = 0.64 +. 0.477;
- y = 1. };
- size = sqrt (2. *. 0.125 *. 0.125) +. 0.12; };*)
-
- (* entry position *)
- (*{ pos =
- { x = 0.;
- y = 1.5 };
- size = 0.02; };
- { pos =
- { x = 0.1;
- y = 1.5 };
- size = 0.02; };
- { pos =
- { x = 0.2;
- y = 1.5 };
- size = 0.02; };
- { pos =
- { x = 0.3;
- y = 1.5 };
- size = 0.02; };
- { pos =
- { x = 0.4;
- y = 1.5 };
- size = 0.02; };*)
(* coins at the bottom *)
-(*
- { pos =
- { x = 1.5;
- y = 0.3; };
- size = 0.15 };
-*)
-
+ (* we cheat a bit on the size to avoid problems... *)
+ (* { pos = *)
+ (* { x = 0.0; *)
+ (* y = 0.0; }; *)
+ (* size = 0.05 }; *)
+ (* { pos = *)
+ (* { x = 0.1; *)
+ (* y = 0.0; }; *)
+ (* size = 0.05 }; *)
+ (* { pos = *)
+ (* { x = 0.2; *)
+ (* y = 0.0; }; *)
+ (* size = 0.05 }; *)
+ (* { pos = *)
+ (* { x = 0.3; *)
+ (* y = 0.0; }; *)
+ (* size = 0.05 }; *)
+ (* { pos = *)
+ (* { x = 0.4; *)
+ (* y = 0.0; }; *)
+ (* size = 0.05 }; *)
+ (* { pos = *)
+ (* { x = 0.35; *)
+ (* y = 0.05; }; *)
+ (* size = 0.05 }; *)
(*] @ (line_obs { x = 0.325; y = 0. } { x = 0.325 +. (0.075 /. 2.); y = 0.75 } )*)
]
@@ -113,6 +104,11 @@ let fixed_obstacles =
size = 0.5; };
] @ (List.map symetrical left_obstacles) @ left_obstacles
+let test_obstacles =
+ [ { pos =
+ { x = 1.5;
+ y = 1.0 };
+ size = 0.1; }; ]
let initial_coins = []
(*
@@ -123,7 +119,7 @@ let initial_coins = []
(* 2.55, 0.3; *) ]
*)
-let urg_position = { x = 0.135; y = 0. }
+let urg_position = { x = 0.095; y = 0. }
let urg_angles =
[|-2.356194490192344836998472601408; -2.350058567040802071090865865699;
diff --git a/info/control2011/src/lib/krobot_config.mli b/info/control2011/src/lib/krobot_config.mli
index 1f88fa2..313c3b1 100644
--- a/info/control2011/src/lib/krobot_config.mli
+++ b/info/control2011/src/lib/krobot_config.mli
@@ -54,6 +54,8 @@ val blue_initial_position : Krobot_geom.vertice * float
val fixed_obstacles : Krobot_geom.obj list
+val test_obstacles : Krobot_geom.obj list
+
val initial_coins : Krobot_geom.vertice list
val urg_position : Krobot_geom.vertice
diff --git a/info/control2011/src/lib/krobot_geom.ml b/info/control2011/src/lib/krobot_geom.ml
index ddcd72e..8169410 100644
--- a/info/control2011/src/lib/krobot_geom.ml
+++ b/info/control2011/src/lib/krobot_geom.ml
@@ -236,7 +236,6 @@ module Bezier = struct
exception Exit_for_vel
let velocity_profile b v_max omega_max at_max ar_max v_ini v_end du =
- Printf.printf "inside velocity_profile\n%!";
let n_pts = int_of_float ( 1. /. du +. 1.) in
let us = Array.init n_pts (fun i -> (float_of_int i) *. du) in
let v_tab = Array.map (fun _ -> v_max) us in
diff --git a/info/control2011/src/lib/krobot_path.ml b/info/control2011/src/lib/krobot_path.ml
index 49b2311..84818cf 100644
--- a/info/control2011/src/lib/krobot_path.ml
+++ b/info/control2011/src/lib/krobot_path.ml
@@ -18,18 +18,22 @@ let rec prev_last = function
| _ :: l ->
prev_last l
-let find ~src ~dst ~beacon =
+let find ?src_orient ?dst_orient ~src ~dst ~beacon objects =
let fixed_objects = List.map (fun { pos; size } -> pos,
size +. Krobot_config.robot_radius +. 0.01)
Krobot_config.fixed_obstacles in
+ let objects = List.map (fun { pos; size } -> pos,
+ size +. Krobot_config.robot_radius +. 0.01)
+ objects in
+
(* do that in a better way when we have time... *)
let init_coins = List.map (fun pos -> pos,
Krobot_config.coin_radius +. Krobot_config.robot_radius +. 0.01)
Krobot_config.initial_coins in
- let l = fixed_objects @ init_coins in
+ let l = objects @ init_coins @ fixed_objects in
let l =
match beacon with
| (Some v, None)
@@ -47,7 +51,7 @@ let find ~src ~dst ~beacon =
in
let l = List.map (fun (v,s) -> (v, min s (distance v src -. 0.1))) l in
let min_distance = Krobot_config.robot_radius +. safety_margin in
- Krobot_pathfinding.find_path ~src ~dst
+ Krobot_pathfinding.find_path ?src_orient ?dst_orient ~src ~dst
({ x = min_distance;
y = min_distance},
{ x = world_width -. min_distance;
diff --git a/info/control2011/src/lib/krobot_path.mli b/info/control2011/src/lib/krobot_path.mli
index b7d3637..f3294c7 100644
--- a/info/control2011/src/lib/krobot_path.mli
+++ b/info/control2011/src/lib/krobot_path.mli
@@ -11,7 +11,13 @@
open Krobot_geom
-val find : src : vertice -> dst : vertice -> beacon : vertice option * vertice option -> vertice list option
+val find :
+ ?src_orient:float * vector ->
+ ?dst_orient:float * vector ->
+ src : vertice -> dst : vertice ->
+ beacon : vertice option * vertice option ->
+ obj list ->
+ vertice list option
(** [goto ~src ~dst ~beacon] find a path from [src] to [dst]. *)
(*
val goto_object : src : vertice -> dst : vertice -> beacon : vertice option * vertice option -> vertice option
diff --git a/info/control2011/src/lib/krobot_pathfinding.ml b/info/control2011/src/lib/krobot_pathfinding.ml
index ebfbd64..9710d68 100644
--- a/info/control2011/src/lib/krobot_pathfinding.ml
+++ b/info/control2011/src/lib/krobot_pathfinding.ml
@@ -358,11 +358,38 @@ let v_of_vertice { Krobot_geom.x; y } =
let vertice_of_seg { p1 = {px;py} } =
{ Krobot_geom.x = px; y = py }
-let find_path ~src ~dst (box_min,box_max) objects =
+let orientation_circles radius vect p =
+ let unit = unitaire vect in
+ let back_left = unitaire ((turn_trigo unit) -| (0.5 *@ unit)) in
+ let back_right = unitaire ((turn_antitrigo unit) -| (0.5 *@ unit)) in
+ let c1 = p +! (radius +. epsilon_float) *@ back_left in
+ let c2 = p +! (radius +. epsilon_float) *@ back_right in
+ [{ c = c1; r = radius };
+ { c = c2; r = radius }]
+
+let v_of_geom_v { Krobot_geom.vx; Krobot_geom.vy} = { vx; vy }
+
+let add_start_orientation b radius vect =
+ let crcls = orientation_circles radius vect b.src in
+ { b with obstacles = crcls @ b.obstacles }
+
+let add_end_orientation b radius vect =
+ let crcls = orientation_circles radius (-. 1. *@ vect) b.dst in
+ { b with obstacles = crcls @ b.obstacles }
+
+let find_path ?src_orient ?dst_orient ~src ~dst (box_min,box_max) objects =
let b = { src = v_of_vertice src;
dst = v_of_vertice dst;
obstacles = List.map (fun (v,r) -> { c = v_of_vertice v; r }) objects;
box = (v_of_vertice box_min, v_of_vertice box_max)} in
+
+ let b = match src_orient with
+ | None -> b
+ | Some (radius,vect) -> add_start_orientation b radius (v_of_geom_v vect) in
+ let b = match dst_orient with
+ | None -> b
+ | Some (radius,vect) -> add_end_orientation b radius (v_of_geom_v vect) in
+
(* let b = filter_far_objects b b.src in *)
if not ( (in_box b.box b.dst) && (check_point b b.dst) && (check_point b b.src) )
then
diff --git a/info/control2011/src/lib/krobot_pathfinding.mli b/info/control2011/src/lib/krobot_pathfinding.mli
index 1b6644d..dcef822 100644
--- a/info/control2011/src/lib/krobot_pathfinding.mli
+++ b/info/control2011/src/lib/krobot_pathfinding.mli
@@ -2,6 +2,8 @@
open Krobot_geom
val find_path :
+ ?src_orient:float * vector ->
+ ?dst_orient:float * vector ->
src:vertice ->
dst:vertice ->
vertice * vertice ->
diff --git a/info/control2011/src/tools/krobot_homologation.ml b/info/control2011/src/tools/krobot_homologation.ml
index 7823746..52051b0 100644
--- a/info/control2011/src/tools/krobot_homologation.ml
+++ b/info/control2011/src/tools/krobot_homologation.ml
@@ -16,65 +16,160 @@ open Krobot_bus
open Krobot_action
open Krobot_geom
-let init_pos, init_angle = Krobot_config.red_initial_position
-
-let path =
- [
- { x = 0.7; y = init_pos.y -. 0.1};
- { x = 0.85; y = 1.5 };
- { x = 0.75; y = 1.20 };
- { x = 0.55; y = 1.15 };
- { x = 0.4; y = 1.15 };
- ]
-
-let strat_loop =
- [
- Calibrate ( { x = 0.64 +. 0.477; y = 1. +. 0.125 +. 0.1; },
- pi /. 2.,
- 0.20,
- None,
- Some (1. +. 0.125),
- Some (pi /. 2.) );
-
- Calibrate ( { x = 0.9; y = 1.; },
- pi,
- 0.20,
- Some (0.64 +. 0.477 -. 0.125),
- None,
- Some (pi) );
-
- Goto ({ x = 1.5; y = 0.5 }, None);
- Goto ({ x = 1.5; y = 1.5 }, None);
- Goto ({ x = 1.5; y = 0.5 }, None);
- Goto ({ x = 1.5; y = 1.5 }, None);
- Wait_for 2.;
- End;
- ]
-
-let strat_base =
- [
- Can (Krobot_message.encode (Drive_activation true));
- Wait_for_jack true;
- Wait_for 1.;
- Wait_for_jack false;
- Start_timer (10.,[End]);
- Set_led(`Red,false);
- Set_led(`Green,false);
- Reset_odometry `Auto;
- Wait_for_odometry_reset `Auto;
- Set_limits (0.3,3.14,1.0,1.0);
- End;
- ]
-
-let launch bus =
- Krobot_bus.send bus
- (Unix.gettimeofday (),
- Strategy_set strat_base)
-
-let loop bus =
- Krobot_bus.send bus
- (Unix.gettimeofday (),
- Strategy_set strat_loop)
+type reset = [ `Auto | `Blue | `Red ]
+
+let gift_width = 0.15
+
+let init_y = 0.244
+
+let ax12_2_base_position = 519
+let ax12_2_high_position = 210
+
+let ax12_2_mid_position = 400
+let ax12_1_mid_position = 600
+
+let ax12_1_base_position = 518
+let ax12_1_high_position = 823
+
+let init_blue_pos =
+ Set_odometry (
+ Some (Krobot_config.wheels_position +. 0.001),
+ Some 24.4,
+ Some 0.)
+
+let gifts_positions =
+ [ { x = 0.60 ; y = 0. };
+ { x = 1.2 ; y = 0. };
+ { x = 1.8 ; y = 0. };
+ { x = 2.4 ; y = 0. }; ]
+
+let gift_shift = -. (gift_width /. 2.) -. 0.09
+let gift_position1 = { x = 0.60 ; y = 0. }
+let gift_position2 = { x = 1.2 ; y = 0. }
+let gift_position3 = { x = 1.8 ; y = 0. }
+let gift_position4 = { x = 2.4 ; y = 0. }
+
+let gift_go pos = { x = pos.x +. gift_shift ; y = init_y }
+let gift_go_red pos = { x = pos.x -. gift_shift ; y = init_y }
+
+let gitf_go1 = { x = gift_position1.x +. gift_shift ; y = init_y }
+let gift_dir1_blue = { vx = -.1.; vy = 0. }
+let gift_dir1_red = { vx = 1.; vy = 0. }
+
+let retry_n n t = Node(Retry(n,Node(Simple,[Stop;Wait_for 0.2;t])),[t])
+
+let node t = Node(Simple,t)
+
+let back_and_retry t =
+ Node(Retry(3,
+ node ((Node(Next,[Stop;Move_back 0.1])) :: t)),
+ t)
+
+let retry_follow n dst dir =
+ let follow =
+ Follow_path ([dst], Some dir, false) in
+ let tmp =
+ Node(Retry(n,node [Stop;Wait_for 0.2;follow]),[follow]) in
+ Node(Next,[back_and_retry [tmp]])
+
+let act1_blue = retry_follow 15 (gift_go gift_position1) gift_dir1_blue
+let act2_blue = retry_follow 15 (gift_go gift_position2) gift_dir1_blue
+let act3_blue = retry_follow 15 (gift_go gift_position3) gift_dir1_blue
+let act4_blue = retry_follow 15 (gift_go gift_position4) gift_dir1_blue
+
+let act1_red = retry_follow 15 (gift_go_red gift_position4) gift_dir1_red
+let act2_red = retry_follow 15 (gift_go_red gift_position3) gift_dir1_red
+let act3_red = retry_follow 15 (gift_go_red gift_position2) gift_dir1_red
+let act4_red = retry_follow 15 (gift_go_red gift_position1) gift_dir1_red
+
+
+let hit_ax12_id id =
+ let base, high =
+ if id = 1
+ then ax12_1_base_position, ax12_1_high_position
+ else ax12_2_base_position, ax12_2_high_position
+ in
+ Node (Simple,
+ [Can (Krobot_message.encode (Ax12_Goto (id, high, 0)));
+ Wait_for 0.3;
+ Can (Krobot_message.encode (Ax12_Goto (id, base, 0)));
+ Wait_for 0.3])
+
+let ramene p1 p2 dir =
+ [ Goto (p1, None);
+ Move_back 0.4;
+ Goto (p2, Some dir); ]
+
+let pos1 = { x = 0.4 ; y = 0.7 }
+let pos2 = { x = 2.6 ; y = 0.7 }
+let pos3 = { x = 2.6 ; y = 1.5 }
+let pos4 = { x = 2.6 ; y = 1.0 }
+
+let haut = { vx = 0. ; vy = 1. }
+let bas = { vx = 0. ; vy = -.1. }
+
+let inner_loop_blue =
+ let l =
+ ramene pos1 pos2 haut
+ @ ramene pos1 pos3 bas
+ @ ramene pos1 pos4 haut in
+ Node(Loop(node l),l)
+
+let sym_pos p = { p with x = 3. -. p.x }
+
+let pos1_red = sym_pos { x = 0.4 ; y = 0.7 }
+let pos2_red = sym_pos { x = 2.6 ; y = 0.7 }
+let pos3_red = sym_pos { x = 2.6 ; y = 1.5 }
+let pos4_red = sym_pos { x = 2.6 ; y = 1.0 }
+
+let inner_loop_red =
+ let l =
+ ramene pos1_red pos2_red haut
+ @ ramene pos1_red pos3_red bas
+ @ ramene pos1_red pos4_red haut in
+ Node(Loop(node l),l)
+
+
+let run_strategy_blue =
+ [ act1_blue;
+ Stop;
+ hit_ax12_id 2;
+ act2_blue;
+ Stop;
+ hit_ax12_id 2;
+ act3_blue;
+ Stop;
+ hit_ax12_id 2;
+ act4_blue;
+ Stop;
+ hit_ax12_id 2;
+ inner_loop_blue;
+ ]
+
+let run_strategy_red =
+ [ act1_red;
+ Stop;
+ hit_ax12_id 1;
+ act2_red;
+ Stop;
+ hit_ax12_id 1;
+ act3_red;
+ Stop;
+ hit_ax12_id 1;
+ act4_red;
+ Stop;
+ hit_ax12_id 1;
+ inner_loop_red;
+ ]
+
+let team_gift_position team p =
+ match team with
+ | `Red -> { x = p.x +. (gift_width /. 2. +. 0.09); y = p.y }
+ | `Blue -> { x= p.x -. (gift_width /. 2. +. 0.09); y = p.y }
+
+(* let gift_destination team p = *)
+(* let p = team_gift_position team p in *)
+(* { x = p.x; y = p.y +. gift_distance } *)
type status = {
bus : Krobot_bus.t;
@@ -83,12 +178,213 @@ type status = {
(* The state of the team selector. *)
}
+let vmax = 0.5
+let omega_max = 3.14 /. 2.
+let accel_tan_max = 1.0
+let accel_rad_max = 1.0
+
+let gonfle_baloon =
+ [Can (Krobot_message.encode (Motor_command (2,3600)));
+ Wait_for 10.;
+ Can (Krobot_message.encode (Motor_command (2,0)));
+ Wait_for 0.1]
+
+let start team =
+ (* assert(team = `Blue); *)
+ [ Stop_timer;
+ Wait_for 0.1;
+ (* init_blue_pos; *)
+ Reset_odometry (team:>reset);
+ Can (Krobot_message.encode (Drive_activation true));
+ Can (Krobot_message.encode (Ax12_Set_Torque_Enable (2,true)));
+ Wait_for 0.1;
+ Can (Krobot_message.encode (Ax12_Set_Torque_Enable (1,true)));
+ Wait_for 0.1;
+ Can (Krobot_message.encode (Ax12_Goto (2, ax12_2_mid_position, 0)));
+ Wait_for 0.1;
+ Can (Krobot_message.encode (Ax12_Goto (1, ax12_1_mid_position, 0)));
+ Wait_for 0.1;
+ Can (Krobot_message.encode (Ax12_Goto (2, ax12_2_base_position, 0)));
+ Wait_for 0.1;
+ Can (Krobot_message.encode (Ax12_Goto (1, ax12_1_base_position, 0)));
+ Wait_for_jack true;
+ Wait_for 0.4;
+ Wait_for_jack false;
+ Start_timer (90.,[Stop] @ gonfle_baloon @ [End]);
+ Start_match;
+ Set_led(`Red,false);
+ Set_led(`Green,false);
+ Reset_odometry (team:>reset);
+ Set_led(`Red,true);
+ (* init_blue_pos; *)
+ (* Wait_for 0.05; *)
+ Wait_for_odometry_reset (team:>reset);
+ Set_led(`Red,true);
+ Set_limits (vmax,omega_max,accel_tan_max,accel_rad_max) ]
+
+let end_ =
+ [ End; ]
+
+let hit_ax12_id id =
+ let base, high =
+ if id = 1
+ then ax12_1_base_position, ax12_1_high_position
+ else ax12_2_base_position, ax12_2_high_position
+ in
+ [Can (Krobot_message.encode (Ax12_Set_Torque_Enable (2,true)));
+ Wait_for 0.1;
+ Can (Krobot_message.encode (Ax12_Goto (id, high, 100)));
+ Wait_for 1.;
+ Can (Krobot_message.encode (Ax12_Goto (id, base, 100)));
+ Wait_for 1.]
+
+let hit_ax12_dir dir =
+ (* TODO: test ! *)
+ let ax12id = if dir.vx < 0. then 1 else 2 in
+ hit_ax12_id ax12id
+
+let retry_follow_node dest dir =
+ let rec follow_node =
+ Node(Retry(1, Node(Simple,[Wait_for 0.2; follow_node])),
+ [Follow_path ([dest], Some dir, false)]) in
+ follow_node
+
+let direction pos = function
+ | `Blue ->
+ (* if pos.x >= 2.2 *)
+ (* then { vx = -.1.; vy = 0. } *)
+ (* else *)
+ { vx = 1.; vy = 0. }
+ | `Red ->
+ (* if pos.x <= 0.8 *)
+ (* then { vx = 1.; vy = 0. } *)
+ (* else *)
+ { vx = -.1.; vy = 0. }
+
+let retry_n n t = Node(Retry(n,Node(Simple,[Stop;Wait_for 0.05;t])),[t])
+
+let go_first_gift pos dir =
+ let shift_len = 0.2 in
+ let shift =
+ if dir.vx >= 0.
+ then shift_len
+ else -. shift_len in
+ let back_shifted_position =
+ { x = pos.x +. shift; y = Krobot_config.robot_radius +. 0.01 } in
+ Node(Simple,
+ [ retry_n 2 (Simple_goto (back_shifted_position, Some (minus dir)));
+ retry_n 2 (Follow_path ([pos], Some dir, false)); ])
+
+let approach_lower_border retries pos dir =
+ let shift_len = 0.2 in
+ let shift =
+ if dir.vx >= 0.
+ then shift_len
+ else -. shift_len in
+ let shifted_position =
+ { x = pos.x -. shift; y = Krobot_config.robot_radius +. 0.01 } in
+ let back_shifted_position =
+ { x = pos.x +. shift; y = Krobot_config.robot_radius +. 0.01 } in
+ let goto_normal =
+ Node(Simple,
+ [ retry_n retries (Simple_goto (shifted_position, Some (minus dir)));
+ retry_n retries (Follow_path ([pos], Some (minus dir), false)); ]) in
+ let goto_back =
+ Node(Simple,
+ [ retry_n retries (Simple_goto (back_shifted_position, Some (minus dir)));
+ retry_n retries (Follow_path ([pos], Some dir, false)); ]) in
+ [Node(Retry(1,goto_back),[goto_normal])]
+
+ (* let goto_path = *)
+ (* Node(Simple, *)
+ (* [ retry_n retries (Simple_goto (shifted_position, Some (minus dir))); *)
+ (* retry_n retries (Follow_path ([pos], Some dir, false)); ]) in *)
+ (* let simple_path = *)
+ (* retry_n 2 (Follow_path ([pos], Some (minus dir), false)) in *)
+ (* [Node (Retry(1,goto_path), [simple_path])] *)
+
+(* let approach_lower_border pos dir = *)
+(* let shift_len = 0.1 in *)
+(* let shift = *)
+(* if dir.vx >= 0. *)
+(* then shift_len *)
+(* else -. shift_len in *)
+(* let shifted_position = *)
+(* { x = pos.x +. shift; y = Krobot_config.robot_radius +. 0.01 } in *)
+(* [ Goto (shifted_position, Some (minus dir)); *)
+(* (\* Follow_path ([pos], Some dir, false); *\) *)
+(* retry_follow_node pos dir; ] *)
+
+(* let goto_gift retries team gift = *)
+(* let destination = gift_destination team gift in *)
+(* approach_lower_border retries destination (direction destination team) *)
+
+(* let do_gift retries team gift = *)
+(* let destination = gift_destination team gift in *)
+(* let dir = direction destination team in *)
+(* let goto = approach_lower_border retries destination dir in *)
+(* let hit = hit_ax12_dir dir in *)
+(* Node(Next,goto @ hit) *)
+
+(* let do_first_gift team gift = *)
+(* let destination = gift_destination team gift in *)
+(* let dir = direction destination team in *)
+(* let goto = go_first_gift destination dir in *)
+(* let hit = hit_ax12_dir dir in *)
+(* Node(Next,goto :: hit) *)
+
+(* let gifts_actions retries team = *)
+(* let gifts = match team with *)
+(* | `Red -> List.rev gifts_positions *)
+(* | `Blue -> gifts_positions *)
+(* in *)
+(* let first = List.hd gifts in *)
+(* let rest = List.tl gifts in *)
+(* do_first_gift team first *)
+(* ,(List.map (do_gift retries team) rest) *)
+
+(* let random team = *)
+(* let zone = match team with *)
+(* | `Red -> ({x = 0.3; y = 0.4},{x = 0.6; y = 1.5}) *)
+(* | `Blue -> ({x = 2.2; y = 0.4},{x = 2.5; y = 1.5}) in *)
+(* let home = match team with *)
+(* | `Red -> ({x = 2.5; y = 0.4},{x = 2.4; y = 1.}) *)
+(* | `Blue -> ({x = 0.4; y = 0.4},{x = 0.5; y = 1.}) in *)
+(* let go = Random_move zone in *)
+(* let back' = Random_move home in *)
+(* let back = Node(Retry(3,Node(Simple,[Stop;back'])),[back']) in *)
+(* let r = Node(Simple,[Stop;go;back;Move_back 0.1]) in *)
+(* [Node(Loop(Node(Simple,[Stop;Wait_for 0.1;r])),[r])] *)
+
+(* let loop l = Node(Loop(Node(Simple,l)),l) *)
+
+(* let strategy gift_retries team = *)
+(* let first_gift, rest_gift = gifts_actions gift_retries team in *)
+(* start team @ [first_gift] @ rest_gift @ random team @ end_ *)
+
+
+(* let launch bus team = *)
+(* Krobot_bus.send bus *)
+(* (Unix.gettimeofday (), *)
+(* Strategy_set (strategy 20 team)) *)
+
+
+let launch bus team =
+ let s = match team with
+ | `Red -> run_strategy_red
+ | `Blue -> run_strategy_blue
+ in
+ let strat = start team @ s in
+ Krobot_bus.send bus (Unix.gettimeofday (),
+ Strategy_set strat)
+
+
let update_team_led status =
let m1,m2 =
if status.team = `Red then
- Switch_request(7,true), Switch_request(6,false)
- else
Switch_request(7,false), Switch_request(6,true)
+ else
+ Switch_request(7,true), Switch_request(6,false)
in
lwt () = Krobot_message.send status.bus (Unix.gettimeofday (),m1) in
Krobot_message.send status.bus (Unix.gettimeofday (), m2)
@@ -106,14 +402,14 @@ let handle_message status (timestamp, message) =
begin
status.team <- team;
ignore (update_team_led status);
- ignore (launch bus)
+ ignore (launch bus status.team)
end
| _ ->
()
end
- | Strategy_finished ->
- ignore (loop bus)
+ (* | Strategy_finished -> *)
+ (* ignore (loop bus) *)
| Kill "homologation" ->
exit 0
diff --git a/info/control2011/src/tools/krobot_objects.ml b/info/control2011/src/tools/krobot_objects.ml
index 23a855a..460a925 100644
--- a/info/control2011/src/tools/krobot_objects.ml
+++ b/info/control2011/src/tools/krobot_objects.ml
@@ -32,6 +32,8 @@ type objects = {
| Message handling |
+-----------------------------------------------------------------+ *)
+let add_diameter = List.map (fun v -> v,0.1)
+
let handle_message objects (timestamp, message) =
match message with
| Kill "objects" ->
@@ -40,7 +42,7 @@ let handle_message objects (timestamp, message) =
| Send ->
ignore (
let ts = Unix.gettimeofday () in
- Krobot_bus.send objects.bus (ts, Objects objects.objects)
+ Krobot_bus.send objects.bus (ts, Objects (add_diameter objects.objects))
)
| _ ->
@@ -109,7 +111,7 @@ lwt () =
E.keep (E.map (handle_message objects) (Krobot_bus.recv bus));
(* Sends initial objects. *)
- lwt () = Krobot_bus.send objects.bus (Unix.gettimeofday (), Objects objects.objects) in
+ lwt () = Krobot_bus.send objects.bus (Unix.gettimeofday (), Objects (add_diameter objects.objects)) in
(* Wait forever. *)
fst (wait ())
diff --git a/info/control2011/src/tools/krobot_planner.ml b/info/control2011/src/tools/krobot_planner.ml
index 6b8e8d5..209a9c6 100644
--- a/info/control2011/src/tools/krobot_planner.ml
+++ b/info/control2011/src/tools/krobot_planner.ml
@@ -50,7 +50,7 @@ type planner = {
+-----------------------------------------------------------------+ *)
let find_path planner src dst =
- Krobot_path.find ~src ~dst ~beacon:planner.beacon
+ Krobot_path.find ~src ~dst ~beacon:planner.beacon []
(* +-----------------------------------------------------------------+
| Primitives |
diff --git a/info/control2011/src/tools/krobot_simulator.ml b/info/control2011/src/tools/krobot_simulator.ml
index 7d5df77..d27c3ff 100644
--- a/info/control2011/src/tools/krobot_simulator.ml
+++ b/info/control2011/src/tools/krobot_simulator.ml
@@ -148,7 +148,8 @@ let velocities sim dt =
in
match sim.bezier_next with
| None ->
- sim.command <- Idle
+ sim.command <- Idle;
+ sim.bezier_curve <- None
| Some (curve,v_end,dir) ->
let v_max, omega_max, at_max, ar_max = sim.bezier_limits in
sim.command <-
@@ -417,10 +418,10 @@ let handle_message bus (timestamp, message) =
else begin
match decode frame with
| Motor_move(dist, speed, acc) ->
- Lwt_unix.run (Lwt_log.info_f "received: move(%f, %f, %f)" dist speed acc);
+ ignore (Lwt_log.info_f "received: move(%f, %f, %f)" dist speed acc);
move sim dist speed acc
| Motor_turn(angle, speed, acc) ->
- Lwt_unix.run (Lwt_log.info_f "received: turn(%f, %f, %f)" angle speed acc);
+ ignore (Lwt_log.info_f "received: turn(%f, %f, %f)" angle speed acc);
turn sim angle speed acc
| Motor_stop(lin_acc, rot_acc) ->
sim.command <- Idle;
@@ -433,7 +434,7 @@ let handle_message bus (timestamp, message) =
| 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);
+ ignore (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)
| _ ->
() end);
@@ -539,12 +540,22 @@ let closest_obstacle x y theta objs =
done;
Array.of_list !l*)
+let circle_obstacle r { pos = { Krobot_geom.x; y }; size } =
+ let t = Unix.gettimeofday () in
+ let t = 0.3 *. t in
+ let x' = x +. r *. cos t in
+ let y' = y +. r *. sin t in
+ { pos = { Krobot_geom.x = x'; y = y' }; size }
+
let gen_data robot =
let dim = Array.length Krobot_config.urg_angles in
let l = ref [] in
for i = 0 to dim - 1 do
let angle = Krobot_config.urg_angles.(i) in
- match closest_obstacle robot.x robot.y (robot.theta +. angle) Krobot_config.fixed_obstacles with
+ let test_obstacles = List.map (circle_obstacle 0.2)
+ Krobot_config.test_obstacles in
+ let obstacles = test_obstacles @ Krobot_config.fixed_obstacles in
+ match closest_obstacle robot.x robot.y (robot.theta +. angle) obstacles with
| Some dist ->
let x = dist *. cos angle in
let y = dist *. sin angle in
diff --git a/info/control2011/src/tools/krobot_urg.ml b/info/control2011/src/tools/krobot_urg.ml
index aecb465..f01903a 100644
--- a/info/control2011/src/tools/krobot_urg.ml
+++ b/info/control2011/src/tools/krobot_urg.ml
@@ -22,6 +22,11 @@ let section = Lwt_log.Section.make "krobot(urg)"
| read/send loop |
+-----------------------------------------------------------------+ *)
+let min_index = 90
+let max_index = 690
+
+let min_distance = 60
+
let scale = 0.001
let convert_pos dist angle =
@@ -32,9 +37,9 @@ let convert_pos dist angle =
let convert (b:Urg.point_data) =
let dim = Bigarray.Array1.dim b in
let l = ref [] in
- for i = 0 to dim - 1 do
+ for i = max 0 min_index to min (dim - 1) max_index do
let data = Nativeint.to_int b.{i} in
- if data > 0
+ if data > min_distance
then
let angle = Krobot_config.urg_angles.(i) in
let (x,y) = convert_pos data angle in
diff --git a/info/control2011/src/tools/krobot_urg_extract.ml b/info/control2011/src/tools/krobot_urg_extract.ml
new file mode 100644
index 0000000..75a88dc
--- /dev/null
+++ b/info/control2011/src/tools/krobot_urg_extract.ml
@@ -0,0 +1,182 @@
+open Lwt
+open Lwt_react
+open Lwt_preemptive
+open Krobot_bus
+open Krobot_message
+open Krobot_geom
+open Icp_minimisation
+
+let section = Lwt_log.Section.make "krobot(urg_extract)"
+
+type info = {
+ bus : Krobot_bus.t;
+ (* The message bus used to communicate with the robot. *)
+
+ mutable position : vertice;
+ (* The position of the robot on the table. *)
+
+ mutable orientation : float;
+ (* The orientation of the robot. *)
+
+ mutable urg : Krobot_geom.vertice array;
+}
+
+
+(**********************)
+
+let default_obstacle_diameter = 0.04
+let keep_above_dist = 0.1
+
+(* let table = Icp_utils.table 2. 3. 200 *)
+let table = Icp_utils.real_table 100
+let table_kd = make_kd_tree table
+
+let a0 = { ath = 0.; ax = 0.; ay = 0. }
+
+let filter_data keep_above_dist robot_transform data =
+ let back_transform = Icp_utils.invert_transform robot_transform in
+ let data = transform back_transform data in
+ Icp_utils.far_enougth_filter table_kd keep_above_dist data
+
+let mark_close kd marking dist v marking_val =
+ let close_points = Kd_tree.closer_points dist v kd in
+ List.iter (fun i -> marking.(i) <- Some marking_val) close_points
+
+let mark_circles diameter data =
+ let kd = make_kd_tree data in
+ let marking = Array.map (fun _ -> None) data.dx in
+ let count = ref 0 in
+ let a = Array.mapi (fun i -> function
+ | Some c -> c
+ | None ->
+ let v = { Kd_tree.x = data.dx.(i); y = data.dy.(i) } in
+ mark_close kd marking diameter v !count;
+ let c = !count in
+ incr count;
+ c) marking in
+ !count, a
+
+let baricenter { dx; dy } l =
+ let len = List.length l in
+ let accx = List.fold_left (fun acc i -> acc +. dx.(i)) 0. l in
+ let accy = List.fold_left (fun acc i -> acc +. dy.(i)) 0. l in
+ { Krobot_geom.x = accx /. (float len)...
[truncated message content] |
|
From: Pierre C. <Ba...@us...> - 2013-05-07 23:14:10
|
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 bc96bdd7d9d8342fe018fe76ff3bf9b5a6fdc3c7 (commit)
via e031a062d10774228d8fa5d70ac5ea54070646b3 (commit)
via ca86b4563865e89edcc47c6fc295a0ab016c60be (commit)
via 47300d0fa1bc6b24cbd059b39e099913f29531ba (commit)
from 8987afa29cc0e2829cdc4618b9e59386e0eb48fb (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 bc96bdd7d9d8342fe018fe76ff3bf9b5a6fdc3c7
Author: Pierre Chambart <pie...@oc...>
Date: Wed May 8 01:13:00 2013 +0200
invert transform
commit e031a062d10774228d8fa5d70ac5ea54070646b3
Author: Pierre Chambart <pie...@oc...>
Date: Wed May 8 00:16:50 2013 +0200
Function to filter out borders from urg data
commit ca86b4563865e89edcc47c6fc295a0ab016c60be
Author: Pierre Chambart <pie...@oc...>
Date: Tue May 7 23:54:40 2013 +0200
icp registration: new dependency lacaml
commit 47300d0fa1bc6b24cbd059b39e099913f29531ba
Author: Pierre Chambart <pie...@oc...>
Date: Mon May 6 18:09:45 2013 +0200
Urg message update display.
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/_oasis b/info/control2011/_oasis
index 94c60bf..85d5e9e 100644
--- a/info/control2011/_oasis
+++ b/info/control2011/_oasis
@@ -74,6 +74,18 @@ Library "krobot-can"
Krobot_can_desc_parser
CSources: can_stubs.c
+Library "krobot-icp"
+ FindlibName: icp
+ FindlibParent: krobot
+ BuildDepends: lacaml
+ XMETADescription: 2d Point cloud registration
+ XMETARequires: lacaml
+ Path: src/icp
+ Install: true
+ Modules:
+ Kd_tree,
+ Icp_minimisation,
+ Icp_utils
# +-------------------------------------------------------------------+
# | The driver |
# +-------------------------------------------------------------------+
diff --git a/info/control2011/_tags b/info/control2011/_tags
index 8609920..4b773d9 100644
--- a/info/control2011/_tags
+++ b/info/control2011/_tags
@@ -1,5 +1,6 @@
<**/*.ml{,i}>: syntax_camlp4o
<src/interfaces/*.ml>: -syntax_camlp4o
+<src/icp/*.ml>: -syntax_camlp4o
# OASIS_START
# DO NOT EDIT (digest: 9a283fb86531fb8de178da3ad5f872b1)
diff --git a/info/control2011/src/icp/icp_minimisation.ml b/info/control2011/src/icp/icp_minimisation.ml
new file mode 100644
index 0000000..fa545ac
--- /dev/null
+++ b/info/control2011/src/icp/icp_minimisation.ml
@@ -0,0 +1,196 @@
+
+(************************************************
+ 2d Point cloud registration: based on
+ 'Robust Registration of 2D and 3D Point Sets'
+ by Andrew W. Fitzgibbon
+ ************************************************)
+
+open Kd_tree
+open Lacaml.D
+
+type a = { ath : float; ax : float; ay : float }
+type data = { dx : float array; dy : float array }
+
+let transform' dth dx dy ax ay =
+ let co = cos dth in
+ let si = sin dth in
+ let len = Array.length ax in
+ let x' = Array.init len (fun i -> ax.(i) *. co -. ay.(i) *. si +. dx) in
+ let y' = Array.init len (fun i -> ax.(i) *. si +. ay.(i) *. co +. dy) in
+ x', y'
+
+let transform { ath; ax; ay } { dx; dy } =
+ let dx, dy = transform' ath ax ay dx dy in
+ { dx; dy }
+
+let make_kd_tree { dx; dy } =
+ let a = Array.init (Array.length dx) (fun i -> i, { x = dx.(i); y = dy.(i) }) in
+ Kd_tree.make a
+
+type kernel = float -> float
+
+let closest_points kd_tree { dx; dy } =
+ Array.init (Array.length dx)
+ (fun i ->
+ let pos, _, sq_dist = Kd_tree.nearest_neighbor
+ { Kd_tree.x = dx.(i); Kd_tree.y = dy.(i) } kd_tree in
+ pos, sq_dist)
+
+let distance_transform (kernel:float -> float) kd_tree a data =
+ let { dx = datax'; dy = datay' } = transform a data in
+ Array.init (Array.length datax')
+ (fun i ->
+ let _, _, sq_dist = nearest_neighbor { x = datax'.(i); y = datay'.(i) } kd_tree in
+ kernel sq_dist)
+
+let epsilon = 1e-15
+
+let jacobian ?(kernel=fun i -> i) kd_tree a data =
+ let v_base = distance_transform kernel kd_tree a data in
+ let a_dth = { a with ath = a.ath +. epsilon } in
+ let a_dx = { a with ax = a.ax +. epsilon } in
+ let a_dy = { a with ay = a.ay +. epsilon } in
+ let diff a =
+ let v_diff = distance_transform kernel kd_tree a data in
+ Array.mapi (fun i v -> (v -. v_base.(i)) /. epsilon) v_diff
+ in
+ let v_dth = diff a_dth in
+ let v_dx = diff a_dx in
+ let v_dy = diff a_dy in
+ v_base, [|v_dth; v_dx; v_dy|]
+
+
+open Lacaml.D
+
+let mat_sum m1 m2 =
+ assert(Mat.dim1 m1 = Mat.dim1 m2);
+ assert(Mat.dim2 m1 = Mat.dim2 m2);
+ let v1 = Mat.to_col_vecs m1 in
+ let v2 = Mat.to_col_vecs m2 in
+ Mat.of_col_vecs
+ (Array.init (Array.length v1) (fun i -> Vec.add v1.(i) v2.(i)))
+
+let mat_sub m1 m2 =
+ assert(Mat.dim1 m1 = Mat.dim1 m2);
+ assert(Mat.dim2 m1 = Mat.dim2 m2);
+ let v1 = Mat.to_col_vecs m1 in
+ let v2 = Mat.to_col_vecs m2 in
+ Mat.of_col_vecs
+ (Array.init (Array.length v1) (fun i -> Vec.add v1.(i) v2.(i)))
+
+let lid n l =
+ let m = Mat.identity n in
+ Mat.scal l m;
+ m
+
+(* gradiant descent step *)
+
+let step ?kernel ?(lambda=1.) kd data a =
+ let e',j' = jacobian ?kernel kd a data in
+ let e = Vec.of_array e' in
+ let jt = Mat.of_array j' in
+ let j = Mat.transpose jt in
+ let size = Lacaml.D.Mat.dim2 j in
+
+ let t1' = gemm jt j in
+ let t1'' = lid size lambda in
+ let t1 = mat_sum t1' t1'' in
+ getri t1;
+ let t2 = gemm t1 jt in
+ let update = gemv t2 e in
+ let a = Vec.of_array [|a.ath; a.ax; a.ay|] in
+ let a = Vec.sub a update in
+ let a = Vec.to_array a in
+ { ath = a.(0); ax = a.(1); ay = a.(2) }
+
+(* simpler evolution step, equivalent to [step] with [lambda = 0.] *)
+
+let simple_step ?kernel kd data a =
+ let e',j' = jacobian ?kernel kd a data in
+ let e = Vec.of_array e' in
+ let jt = Mat.of_array j' in
+ let j = Mat.transpose jt in
+
+ let t1 = gemm jt j in
+ getri t1;
+ let t2 = gemm t1 jt in
+ let update = gemv t2 e in
+ let a = Vec.of_array [|a.ath; a.ax; a.ay|] in
+ let a = Vec.sub a update in
+ let a = Vec.to_array a in
+ { ath = a.(0); ax = a.(1); ay = a.(2) }
+
+(***** default kernel ****)
+
+let sigma = 0.05
+let sigma2 = sigma*.sigma
+let default_kernel x =
+ if x < sigma2
+ then x
+ else 2. *. sigma *. (sqrt x) -. sigma2
+
+
+
+(***** data filtering *****)
+
+(* filtering multiple point associated to the same value: if more than
+ one point closest to a point [p], keep only the closest one *)
+
+let closest_match ai =
+ let n = Array.fold_left (fun m (n,_) -> max m n) (-1) ai in
+ let matcher = Array.init (n+1) (fun _ -> None) in
+ Array.iteri (fun index (matched,(d:float)) ->
+ match matcher.(matched) with
+ | None -> matcher.(matched) <- Some (index,d)
+ | Some (_,d1) ->
+ if d < d1 then matcher.(matched) <- Some (index,d)) ai;
+ matcher
+
+let get_closest matcher ((ax',ay'):float array * float array) =
+ let r = ref [] in
+ Array.iter (function
+ | None -> ()
+ | Some (i,_) ->
+ r := (ax'.(i), ay'.(i)) :: !r) matcher;
+ let x,y = List.split !r in
+ Array.of_list x, Array.of_list y
+
+let closest_filter kd a d2 =
+ let d' = transform a d2 in
+ let cp = closest_points kd d' in
+ let cm = closest_match cp in
+ let dx, dy = get_closest cm (d2.dx,d2.dy) in
+ { dx; dy }
+
+(* filter point farther than 3 time the median distance *)
+
+let far_filter kd a d2 =
+ let d' = transform a d2 in
+ let cp = closest_points kd d' in
+ let cp = Array.mapi (fun i (_,d) -> i,d) cp in
+ Array.sort (fun (_,d1) (_,d2) -> compare d1 d2) cp;
+ let (_,median) = cp.(Array.length cp/2) in
+ let cl = Array.to_list cp in
+ let too_far (i,d) = d <= median *. 9. in
+ let ai = Array.of_list (List.filter too_far cl) in
+ { dx = Array.map (fun (i,_) -> d2.dx.(i)) ai;
+ dy = Array.map (fun (i,_) -> d2.dy.(i)) ai }
+
+
+(*************************)
+
+let rec register_simple ?kernel kd data a n =
+ if n = 0 then a
+ else
+ let data' = closest_filter kd a data in
+ let data' = far_filter kd a data' in
+ let a = simple_step ?kernel kd data' a in
+ register_simple ?kernel kd data a (n-1)
+
+let rec register ?kernel ?lambda kd data a n =
+ if n = 0 then a
+ else
+ let data' = closest_filter kd a data in
+ let data' = far_filter kd a data' in
+ let a = step ?kernel ?lambda kd data' a in
+ register ?kernel ?lambda kd data a (n-1)
diff --git a/info/control2011/src/icp/icp_minimisation.mli b/info/control2011/src/icp/icp_minimisation.mli
new file mode 100644
index 0000000..88aa12e
--- /dev/null
+++ b/info/control2011/src/icp/icp_minimisation.mli
@@ -0,0 +1,32 @@
+
+type a = { ath : float; ax : float; ay : float; }
+(** representing rigid transformation: rotation by [ath] then
+ translation by [ax,ay] *)
+
+type data = { dx : float array; dy : float array; }
+
+type kernel = float -> float
+
+val transform : a -> data -> data
+(** apply transformation to data *)
+
+val make_kd_tree : data -> int Kd_tree.t
+(** create a kd tree containting index in the data *)
+
+val distance_transform : kernel -> 'a Kd_tree.t -> a -> data -> float array
+(** [distance_transform kernel kd a data] Calculates the square of the
+ distance for each point of [data] after transformation to
+ the closest point in the kd tree. the kernel is applied to the
+ result after *)
+
+val default_kernel : kernel
+(** Huber kernel *)
+
+(**********)
+
+val register_simple : ?kernel:kernel -> int Kd_tree.t -> data -> a -> int -> a
+(** Gauss-Newton gradient descent *)
+
+val register : ?kernel:kernel -> ?lambda:Lacaml.D.num_type ->
+ int Kd_tree.t -> data -> a -> int -> a
+(** Levenberg-Marquardt gradient descent *)
diff --git a/info/control2011/src/icp/icp_utils.ml b/info/control2011/src/icp/icp_utils.ml
new file mode 100644
index 0000000..a961e5e
--- /dev/null
+++ b/info/control2011/src/icp/icp_utils.ml
@@ -0,0 +1,98 @@
+open Icp_minimisation
+
+(**** table ****)
+
+let linear a0 an n =
+ let dx = (an -. a0) /. (float (n-1)) in
+ let p n = a0 +. (float n) *. dx in
+ Array.init n p
+
+let line (x0,y0) (xn,yn) n =
+ linear x0 xn n, linear y0 yn n
+
+let shuffle l =
+ let rec split (ac1,ac2) = function
+ | [] -> ac1,ac2
+ | [t] -> t::ac1,ac2
+ | t1::t2::q -> split (t1::ac1,t2::ac2) q in
+ let rec merge acc l1 l2 = match l1,l2 with
+ | [],l | l, [] -> l@acc
+ | t1::q1, t2::q2 -> merge (t1::t2::acc) q1 q2 in
+ let l1,l2 = split ([],[]) l in
+ merge [] l1 (List.rev l2)
+
+let table ~width ~length n =
+ let x1,y1 = line (0.,0.) (length,0.) n in
+ let x2,y2 = line (length,0.) (length,width) n in
+ let x3,y3 = line (length,width) (0.,width) n in
+ let x4,y4 = line (0.,width) (0.,0.) n in
+ let x = Array.concat [x1;x2;x3;x4] in
+ let y = Array.concat [y1;y2;y3;y4] in
+ let f a = Array.of_list (shuffle (shuffle (Array.to_list a))) in
+ { dx = f x; dy = f y }
+
+
+
+(**** dump loading ****)
+
+let load_float ic =
+ Scanf.bscanf ic " %f " (fun f -> f)
+
+let load_float_cpl ic =
+ Scanf.bscanf ic " %f %f " (fun f1 f2 -> f1, f2)
+
+let load_line ic =
+ let line = Pervasives.input_line ic in
+ let ic = Scanf.Scanning.from_string line in
+ let ts = load_float ic in
+ let rec aux () =
+ let c = try Some (load_float_cpl ic) with _ -> None in
+ match c with
+ | Some c -> c :: aux ()
+ | None -> []
+ in
+ ts, aux ()
+
+let load_file' f =
+ let ic = open_in f in
+ let rec aux () =
+ let l = try Some (load_line ic) with _ -> None in
+ match l with
+ | Some l -> l::aux ()
+ | None -> []
+ in
+ aux ()
+
+let filter_dist min max (x,y) =
+ let d = sqrt (x*.x +. y*.y) in
+ d >= min && d <= max
+
+let load_file ?(min_dist=0.15) ?(max_dist=6.) f =
+ let l = load_file' f in
+ let l' =
+ List.map (fun (ts,v) ->
+ let v = List.filter (filter_dist min_dist max_dist) v in
+ let x,y = List.split v in
+ ts, { dx = Array.of_list x; dy = Array.of_list y }) l in
+ Array.of_list l'
+
+
+(**** filtering ****)
+
+let far_enougth_filter kd a min_dist data =
+ let dist = distance_transform (fun i -> i) kd a data in
+ let dist = Array.mapi (fun i d -> i,d) dist in
+ let distl = Array.to_list dist in
+ let min_dist2 = min_dist *. min_dist in
+ let far_enougth (i,d) = d >= min_dist2 in
+ let ai = Array.of_list (List.filter far_enougth distl) in
+ { dx = Array.map (fun (i,_) -> data.dx.(i)) ai;
+ dy = Array.map (fun (i,_) -> data.dy.(i)) ai }
+
+let invert_transform a =
+ let co = cos (-. a.ath) in
+ let si = sin (-. a.ath) in
+ let x' = a.ax *. co -. a.ay *. si in
+ let y' = a.ax *. si +. a.ay *. co in
+ { ath = -. a.ath; ax = -. x'; ay = -. y'}
+
diff --git a/info/control2011/src/icp/icp_utils.mli b/info/control2011/src/icp/icp_utils.mli
new file mode 100644
index 0000000..1e23bba
--- /dev/null
+++ b/info/control2011/src/icp/icp_utils.mli
@@ -0,0 +1,16 @@
+open Icp_minimisation
+
+val table : width:float -> length:float -> int -> data
+
+val load_file : ?min_dist:float -> ?max_dist:float -> string ->
+ (float * data) array
+(** load a dump file in the format dumpped by [krobot_urg -listen] *)
+
+
+(* filtering *)
+
+val far_enougth_filter : 'a Kd_tree.t -> a -> float -> data -> data
+(** [far_enougth_filter kd a min_dist data] filter out values of
+ [data] that are closer than [min_dist] to a vertex of kd *)
+
+val invert_transform : a -> a
diff --git a/info/control2011/src/icp/kd_tree.ml b/info/control2011/src/icp/kd_tree.ml
new file mode 100644
index 0000000..45601d4
--- /dev/null
+++ b/info/control2011/src/icp/kd_tree.ml
@@ -0,0 +1,110 @@
+
+type direction =
+ | Vert
+ | Hori
+
+type vertice = { x : float; y : float }
+
+type 'a t =
+ | Split of direction * 'a * vertice * 'a t * 'a t
+ | Leaf of 'a * vertice
+ | Empty
+
+let splitn n l =
+ let rec aux n l acc =
+ if n = 0
+ then List.rev acc, l
+ else match l with
+ | [] -> assert false
+ | t::q -> aux (n-1) q (t::acc) in
+ aux n l []
+
+let rec make_horiz a =
+ match Array.length a with
+ | 0 -> Empty
+ | 1 ->
+ let va, vert = a.(0) in
+ Leaf (va, vert)
+ | len ->
+ Array.sort (fun (_,{ x = x1 }) (_,{ x = x2 }) -> compare x1 x2 ) a;
+ let median = (len / 2) in
+ let med_val, median_elt = a.(median) in
+ let left = Array.sub a 0 median in
+ let right = Array.sub a (median + 1) (len - median - 1) in
+ let l1 = make_vert left in
+ let l2 = make_vert right in
+ Split (Hori, med_val, median_elt, l1, l2)
+
+and make_vert a =
+ match Array.length a with
+ | 0 -> Empty
+ | 1 ->
+ let va, vert = a.(0) in
+ Leaf (va, vert)
+ | len ->
+ Array.sort (fun (_,{ y = y1 }) (_,{ y = y2 }) -> compare y1 y2 ) a;
+ let median = (len / 2) in
+ let med_val, median_elt = a.(median) in
+ let left = Array.sub a 0 median in
+ let right = Array.sub a (median + 1) (len - median - 1) in
+ let l1 = make_vert left in
+ let l2 = make_vert right in
+ Split (Vert, med_val, median_elt, l1, l2)
+
+let make a = make_horiz a
+
+let sq_dist v1 v2 =
+ let dx = v1.x -. v2.x in
+ let dy = v1.y -. v2.y in
+ dx *. dx +. dy *. dy
+
+let sq x = x *. x
+
+let rec nearest_neighbor' v ((cur_val,cur,cur_dist) as curr) t = match t with
+ | Empty -> curr
+ | Leaf (va,p) ->
+ let dist = sq_dist v p in
+ if dist < cur_dist
+ then va, p, dist
+ else curr
+ | Split (dir, med_val, median_elt, left, right) ->
+ let split_distance = match dir with
+ | Hori -> sq (v.x -. median_elt.x)
+ | Vert -> sq (v.y -. median_elt.y) in
+ let (_, _, cur_dist) as curr =
+ let dist = sq_dist v median_elt in
+ if dist < cur_dist
+ then med_val, median_elt, dist
+ else curr in
+ let cmp = match dir with
+ | Hori -> compare v.x median_elt.x
+ | Vert -> compare v.y median_elt.y in
+ if cmp < 0
+ then
+ let (_, _, cur_dist) as curr = nearest_neighbor' v curr left in
+ if split_distance >= cur_dist
+ then curr
+ else nearest_neighbor' v curr right
+ else
+ let (_, _, cur_dist) as curr = nearest_neighbor' v curr right in
+ if split_distance >= cur_dist
+ then curr
+ else nearest_neighbor' v curr left
+
+let nearest_neighbor v = function
+ | Empty -> failwith "nearest_neighbor: empty"
+ | Leaf (va, p) -> va, p, sq_dist p v
+ | Split (_, med_val, median_elt, _, _) as t ->
+ let curr = (med_val, median_elt, sq_dist median_elt v) in
+ nearest_neighbor' v curr t
+
+let rec depth = function
+ | Split (_, _, _, t1,t2 ) ->
+ 1 + max (depth t1) (depth t2)
+ | Leaf _ -> 1
+ | Empty -> 0
+
+(* let a = [|{x = 1.; y = 2.}; {x = 3.; y = 4.}; {x = 2.; y = 2.}; {x = 3.; y = 1.}; {x = 4.; y = 2.}|] *)
+(* let a' = Array.mapi (fun i v -> i,v) a *)
+(* let t = make a' *)
+(* let (i,r,d) = nearest_neighbor { x = 3.; y = 2.5 } t *)
diff --git a/info/control2011/src/icp/kd_tree.mli b/info/control2011/src/icp/kd_tree.mli
new file mode 100644
index 0000000..947b1ad
--- /dev/null
+++ b/info/control2011/src/icp/kd_tree.mli
@@ -0,0 +1,14 @@
+
+type 'a t
+
+type vertice = { x : float; y : float; }
+
+val make : ('a * vertice) array -> 'a t
+
+val nearest_neighbor : vertice -> 'a t -> 'a * vertice * float
+(** [nearest_neighbor v kd] finds the closest vertice [val] of
+ [v]. returns [val,vert,dist_sqr], with [val] the value associated
+ to [vert] and [dist_sqr] the square of the distance between [v] and
+ [vert] *)
+
+val depth : 'a t -> int
diff --git a/info/control2011/src/tools/krobot_viewer.ml b/info/control2011/src/tools/krobot_viewer.ml
index 4492bab..6e01962 100644
--- a/info/control2011/src/tools/krobot_viewer.ml
+++ b/info/control2011/src/tools/krobot_viewer.ml
@@ -665,7 +665,8 @@ let handle_message viewer (timestamp, message) =
queue_draw viewer
| Urg dist ->
- viewer.urg <- project_urg viewer dist
+ viewer.urg <- project_urg viewer dist;
+ queue_draw viewer
| Urg_lines lines ->
viewer.urg_lines <- project_urg_lines viewer lines
hooks/post-receive
--
krobot
|
|
From: Xavier L. <Ba...@us...> - 2013-05-06 21:48:48
|
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 8987afa29cc0e2829cdc4618b9e59386e0eb48fb (commit)
from ce6642eab29a499e22861634dea8201d3580c2e3 (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 8987afa29cc0e2829cdc4618b9e59386e0eb48fb
Author: Xavier Lagorce <Xav...@cr...>
Date: Mon May 6 23:48:57 2013 +0200
[Controller_Motor_STM32] Added initialization of omega_max (to prevent initial zero value)
and reactivate motor output for pump.
-----------------------------------------------------------------------
Changes:
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 ce2ad8c..8955dc1 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
@@ -59,6 +59,7 @@ static void init(void)
PROP_WHEEL_RADIUS_LEFT, PROP_WHEEL_RADIUS_RIGHT, PROP_SHAFT_WIDTH, // Structural parameters
8*2*M_PI, // Absolute wheel speed limitation
0.5, // Linear velocity limitation
+ 3.14,// Rotational speed limitation
1.0, // Linear acceleration limitation
1.0, // Radial acceleration limitation
0.4, 0.7, 1.0, // Controller gains
@@ -109,6 +110,8 @@ static void init(void)
timer_delay(100);
}
+ // Enable motor pump
+ enableMotor(MOTOR2);
}
static void NORETURN ind_process(void)
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 a9fe843..82a527e 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/lib/differential_drive.c
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/lib/differential_drive.c
@@ -167,7 +167,7 @@ static void NORETURN traj_following_process(void) {
void dd_start(uint8_t odometry_process,
float left_wheel_radius, float right_wheel_radius,
float shaft_width, float max_wheel_speed,
- float v_max, float at_max, float ar_max,
+ float v_max, float omega_max, float at_max, float ar_max,
float k1, float k2, float k3, float Ts) {
params.odometry_process = odometry_process;
params.left_wheel_radius = left_wheel_radius;
@@ -188,6 +188,7 @@ void dd_start(uint8_t odometry_process,
params.trajs[0].enabled = 0;
params.trajs[1].enabled = 0;
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 8be953d..5e3dddf 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/lib/differential_drive.h
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/lib/differential_drive.h
@@ -49,7 +49,7 @@
void dd_start(uint8_t odometry_process,
float left_wheel_radius, float right_wheel_radius,
float shaft_width, float max_wheel_speed,
- float v_max, float at_max, float ar_max,
+ float v_max, float omega_max, float at_max, float ar_max,
float k1, float k2, float k3, float Ts);
/* Pauses or Resumes the differential drive system.
hooks/post-receive
--
krobot
|
|
From: Xavier L. <Ba...@us...> - 2013-05-06 20:38:35
|
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 ce6642eab29a499e22861634dea8201d3580c2e3 (commit)
from 9e9d1491f3d038ec47349e046c9234fedde6ac64 (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 ce6642eab29a499e22861634dea8201d3580c2e3
Author: Xavier Lagorce <Xav...@cr...>
Date: Mon May 6 22:39:25 2013 +0200
[Krobot_config] Better parameters for perimeter and Urg positionning
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/src/lib/krobot_config.ml b/info/control2011/src/lib/krobot_config.ml
index be24feb..e7975da 100644
--- a/info/control2011/src/lib/krobot_config.ml
+++ b/info/control2011/src/lib/krobot_config.ml
@@ -11,11 +11,11 @@ let sqr x = x *. x
let world_height = 2.
let world_width = 3.
-let robot_length = 0.22
-let robot_width = 0.33
+let robot_length = 0.225
+let robot_width = 0.30
let wheels_diameter = 0.098
let wheels_distance = 0.224
-let wheels_position = 0.04
+let wheels_position = 0.055
let robot_radius =
let l1 = robot_length -. wheels_position in
let l2 = robot_width /. 2. in
@@ -123,7 +123,7 @@ let initial_coins = []
(* 2.55, 0.3; *) ]
*)
-let urg_position = { x = 0.19; y = 0. }
+let urg_position = { x = 0.135; y = 0. }
let urg_angles =
[|-2.356194490192344836998472601408; -2.350058567040802071090865865699;
hooks/post-receive
--
krobot
|
|
From: Xavier L. <Ba...@us...> - 2013-05-05 21:24:01
|
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 9e9d1491f3d038ec47349e046c9234fedde6ac64 (commit)
via 405ee0953efe7043668a7173e869d6f71963e12d (commit)
from 1380caeb212200f4423cd6ce13dbfa7ed9b6c8fa (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 9e9d1491f3d038ec47349e046c9234fedde6ac64
Author: Xavier Lagorce <Xav...@cr...>
Date: Sun May 5 23:12:47 2013 +0200
[krobot-simulator] Added velocity profiles along Bezier's curves
commit 405ee0953efe7043668a7173e869d6f71963e12d
Author: Xavier Lagorce <Xav...@cr...>
Date: Sun May 5 23:12:11 2013 +0200
[Krobot_geom] Added computation of velocity profile along Bezier curves
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/src/lib/krobot_geom.ml b/info/control2011/src/lib/krobot_geom.ml
index cb4f683..ddcd72e 100644
--- a/info/control2011/src/lib/krobot_geom.ml
+++ b/info/control2011/src/lib/krobot_geom.ml
@@ -223,11 +223,67 @@ module Bezier = struct
let ddt b t =
(b.a *| (6. *. t)) +| (b.b *| 2.)
+ let cr b t =
+ let db = dt b t in
+ let ddb = ddt b t in
+ ((db.vx*.db.vx +. db.vy*.db.vy) ** 1.5) /. (db.vx*.ddb.vy -. db.vy*.ddb.vx)
+
type robot_info =
{ r_width : float; (* distance between wheels: m *)
r_max_wheel_speed : float; (* m / s *)
r_max_a : float; } (* m / s^2 *)
+ exception Exit_for_vel
+
+ let velocity_profile b v_max omega_max at_max ar_max v_ini v_end du =
+ Printf.printf "inside velocity_profile\n%!";
+ let n_pts = int_of_float ( 1. /. du +. 1.) in
+ let us = Array.init n_pts (fun i -> (float_of_int i) *. du) in
+ let v_tab = Array.map (fun _ -> v_max) us in
+ let mins = ref [(0,v_ini); ((n_pts-1),v_end)] in
+ let cr_pp = ref (abs_float (cr b 0.)) in
+ let cr_p = ref (abs_float (cr b du)) in
+ Array.iteri (fun i u -> if (i > 1) then begin
+ let cr = abs_float (cr b u) in
+ if (cr >= !cr_p && !cr_p < !cr_pp) then begin
+ mins := (i, sqrt (ar_max*.cr)) :: !mins;
+ end;
+ cr_pp := !cr_p;
+ cr_p := cr;
+ end
+ ) us;
+ mins := List.rev !mins;
+ List.iter (fun (m, vm) ->
+ if (vm < v_tab.(m)) then begin
+ v_tab.(m) <- vm;
+ (try
+ for i = m-1 downto 0 do
+ let db = dt b (us.(i+1)) in
+ let dsu = sqrt (db.vx*.db.vx +. db.vy*.db.vy) in
+ let dt = (-.v_tab.(i+1)+.sqrt(v_tab.(i+1)*.v_tab.(i+1)+.2.*.at_max*.du*.dsu))/.at_max in
+ let nv = v_tab.(i+1)+.at_max*.dt in
+ if (nv < v_tab.(i)) then
+ v_tab.(i) <- nv
+ else
+ raise Exit_for_vel
+ done
+ with Exit_for_vel -> ());
+ (try
+ for i = m+1 to (n_pts-1) do
+ let db = dt b (us.(i-1)) in
+ let dsu = sqrt (db.vx*.db.vx +. db.vy*.db.vy) in
+ let dt = (-.v_tab.(i-1)+.sqrt(v_tab.(i-1)*.v_tab.(i-1)+.2.*.at_max*.du*.dsu))/.at_max in
+ let nv = v_tab.(i-1)+.at_max*.dt in
+ if (nv < v_tab.(i)) then
+ v_tab.(i) <- nv
+ else
+ raise Exit_for_vel
+ done
+ with Exit_for_vel -> ());
+ end
+ ) !mins;
+ v_tab
+
let wheel_speed_rapport r b t =
let s' = norm (dt b t) in
let { vx = x'; vy = y' } = dt b t in
diff --git a/info/control2011/src/lib/krobot_geom.mli b/info/control2011/src/lib/krobot_geom.mli
index 7e2355d..6a58739 100644
--- a/info/control2011/src/lib/krobot_geom.mli
+++ b/info/control2011/src/lib/krobot_geom.mli
@@ -113,15 +113,20 @@ module Bezier : sig
val mul_d2 : curve -> float -> curve
val dt : curve -> float -> vector
- (** [dt curve t] is the value of the dérivée de curve en t *)
+ (** [dt curve t] is the value of the derivative of curve in t *)
val ddt : curve -> float -> vector
- (** [ddt curve t] is the value of the dérivée seconde de curve en t *)
+ (** [ddt curve t] is the value of the second derivative of curve in t *)
+ val cr : curve -> float -> float
+ (** [cr curve t] is the value of the curvature radius of curve in t *)
type robot_info =
{ r_width : float; (* distance between wheels: m *)
r_max_wheel_speed : float; (* m / s *)
r_max_a : float; } (* m / s^2 *)
+ val velocity_profile : curve -> float -> float -> float -> float -> float -> float -> float -> float array
+ (** [velocity_profile curve v_max omega_max at_max ar_max v_ini v_end du] *)
+
val wheel_speed_rapport : robot_info -> curve -> float -> float
(** [wheel_speed_rapport width curve t] is the rapport between the
speed of both wheels at the point t of the curve if the 2 wheels
diff --git a/info/control2011/src/tools/krobot_simulator.ml b/info/control2011/src/tools/krobot_simulator.ml
index 2857a59..7d5df77 100644
--- a/info/control2011/src/tools/krobot_simulator.ml
+++ b/info/control2011/src/tools/krobot_simulator.ml
@@ -71,7 +71,7 @@ type command =
(* [Turn(t_acc, velocity)] *)
| Move of float * float
(* [Move(t_acc, velocity)] *)
- | Bezier_cmd of (float * float * float * float) * bool
+ | Bezier_cmd of (float array) * (float * float * float * float) * bool
(** [Motor_bezier_limits(v_max, omega_max, a_tan_max, a_rad_max)] *)
(* Type of simulators. *)
@@ -90,9 +90,9 @@ type simulator = {
(* The state of the ghost. *)
mutable bezier_u : float;
(* position on the Bezier's curve*)
- mutable bezier_curve : Bezier.curve option;
+ mutable bezier_curve : (Bezier.curve * float) option;
(* Bezier's curve currently being followed if existing *)
- mutable bezier_next : (Bezier.curve * bool) option;
+ mutable bezier_next : (Bezier.curve * float * bool) option;
(* Next Bezier's curve to follow *)
mutable time : float;
(* The current time. *)
@@ -140,14 +140,23 @@ velocities:
let velocities sim dt =
(* Put the robot into idle if the last command is terminated. *)
(match sim.command with
- | Bezier_cmd (_,cur_dir) ->
+ | Bezier_cmd (_,_,cur_dir) ->
if sim.bezier_u >= 1. then begin
+ let v_ini = match sim.bezier_curve with
+ | Some (curve, v_end) -> v_end
+ | None -> assert false
+ in
match sim.bezier_next with
| None ->
sim.command <- Idle
- | Some (curve,dir) ->
- sim.command <- Bezier_cmd (sim.bezier_limits, cur_dir);
- sim.bezier_curve <- Some curve;
+ | Some (curve,v_end,dir) ->
+ let v_max, omega_max, at_max, ar_max = sim.bezier_limits in
+ sim.command <-
+ Bezier_cmd (Bezier.velocity_profile
+ curve v_max omega_max at_max ar_max v_ini v_end 0.01,
+ sim.bezier_limits,
+ cur_dir);
+ sim.bezier_curve <- Some (curve, v_end);
sim.bezier_next <- None;
sim.bezier_u <- 0.
end
@@ -171,13 +180,22 @@ let velocities sim dt =
(vel, 0.)
else
(vel *. (sim.command_end -. sim.time) /. t_acc, 0.)
- | Bezier_cmd (limits, dir) ->
+ | Bezier_cmd (v_tab, limits, dir) ->
(match sim.bezier_curve with
| None ->
sim.command <- Idle;
(0., 0.)
- | Some curve ->
+ | Some (curve,_) ->
let (v_max,omega_max,_,a_r_max) = limits in
+ let ui = int_of_float (sim.bezier_u *. 100.) in
+ let v_max =
+ if ui >= (Array.length v_tab)-1 then
+ v_tab.((Array.length v_tab)-1)
+ else if ui < 0 then
+ v_tab.(0)
+ else
+ v_tab.(ui) +. (v_tab.(ui+1)-.v_tab.(ui)) *. (sim.bezier_u -. 0.01*.(float_of_int ui))/.0.01;
+ 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
@@ -204,7 +222,7 @@ let bezier sim (x_end, y_end, d1, d2, theta_end, v_end) =
| None ->
{Krobot_geom.x = sim.state.x; Krobot_geom.y = sim.state.y},
sim.state.theta
- | Some curve ->
+ | Some (curve,_) ->
let _,_,r,s = Bezier.pqrs curve in
s,
(angle (vector r s))
@@ -213,13 +231,19 @@ let bezier sim (x_end, y_end, d1, d2, theta_end, v_end) =
let q = translate p (vector_of_polar d1 theta_start) in
let r = translate s (vector_of_polar (-.d2) theta_end) in
let dir = d1 >= 0. in
+ let curve = Bezier.of_vertices p q r s in
match sim.bezier_curve with
| None ->
- sim.command <- Bezier_cmd (sim.bezier_limits,dir);
+ let v_max, omega_max, at_max, ar_max = sim.bezier_limits in
+ sim.command <-
+ Bezier_cmd (Bezier.velocity_profile
+ curve v_max omega_max at_max ar_max 0.01 v_end 0.01,
+ sim.bezier_limits,
+ dir);
sim.bezier_u <- 0.;
- sim.bezier_curve <- Some (Bezier.of_vertices p q r s);
+ sim.bezier_curve <- Some (curve, v_end);
| Some _ ->
- sim.bezier_next <- Some ((Bezier.of_vertices p q r s),dir)
+ sim.bezier_next <- Some (curve,v_end,dir)
let move sim distance velocity acceleration =
if distance <> 0. && velocity > 0. && acceleration > 0. then begin
hooks/post-receive
--
krobot
|
|
From: Pierre C. <Ba...@us...> - 2013-05-05 15:27:50
|
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 1380caeb212200f4423cd6ce13dbfa7ed9b6c8fa (commit)
from 8795043d51140b4e632d493acef16078577dca49 (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 1380caeb212200f4423cd6ce13dbfa7ed9b6c8fa
Author: Pierre Chambart <pie...@oc...>
Date: Sun May 5 17:27:12 2013 +0200
Allow setting a timout strategy: for match end
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/src/lib/krobot_action.ml b/info/control2011/src/lib/krobot_action.ml
index 58bbfd6..8b86239 100644
--- a/info/control2011/src/lib/krobot_action.ml
+++ b/info/control2011/src/lib/krobot_action.ml
@@ -38,7 +38,7 @@ type t =
| Wait_until of float
| Wait_for_grip_open_low of [ `Front | `Back ]
| Wait_for_grip_close_low of [ `Front | `Back ]
- | Start_timer
+ | Start_timer of float * t list
| Can of Krobot_can.frame
| Set_led of [ `Red | `Yellow | `Green ] * bool
| Set_orientation of float
@@ -59,7 +59,7 @@ let string_of_face = function
let rec to_string = function
| Node (t,l) ->
- sprintf "Node [%s, %s]" (string_of_option to_string t) (String.concat "; " (List.map to_string l))
+ sprintf "Node [%s, %s]" (string_of_option to_string t) (list_to_string l)
| Stop ->
"Stop"
| Think ->
@@ -137,7 +137,8 @@ let rec to_string = function
sprintf "Wait_for_grip_open_low %S" (string_of_face face)
| Wait_for_grip_close_low face ->
sprintf "Wait_for_grip_close_low %S" (string_of_face face)
- | Start_timer -> "Start_timer"
+ | Start_timer (delay,t) ->
+ sprintf "Start_timer(%f,%s)" delay (list_to_string t)
| Can c -> "Can"
| Try_something v ->
sprintf "Try_something %s" (string_of_vertice v)
@@ -151,3 +152,5 @@ let rec to_string = function
(string_of_option string_of_float o)
| Calibrate (_,_,_,_,_,_) -> "Calibrate"
| End -> "End"
+
+and list_to_string l = String.concat "; " (List.map to_string l)
diff --git a/info/control2011/src/lib/krobot_action.mli b/info/control2011/src/lib/krobot_action.mli
index 3722d9f..a2ebb91 100644
--- a/info/control2011/src/lib/krobot_action.mli
+++ b/info/control2011/src/lib/krobot_action.mli
@@ -88,7 +88,8 @@ type t =
| Wait_for_grip_close_low of [ `Front | `Back ]
(** Wait for the given low grip to be opened. *)
- | Start_timer
+ | Start_timer of float * t list
+
| Can of Krobot_can.frame
| Set_led of [ `Red | `Yellow | `Green ] * bool
| Set_orientation of float
diff --git a/info/control2011/src/tools/krobot_homologation.ml b/info/control2011/src/tools/krobot_homologation.ml
index 2f5c4e7..7823746 100644
--- a/info/control2011/src/tools/krobot_homologation.ml
+++ b/info/control2011/src/tools/krobot_homologation.ml
@@ -57,7 +57,7 @@ let strat_base =
Wait_for_jack true;
Wait_for 1.;
Wait_for_jack false;
- Start_timer;
+ Start_timer (10.,[End]);
Set_led(`Red,false);
Set_led(`Green,false);
Reset_odometry `Auto;
diff --git a/info/control2011/src/tools/krobot_vm.ml b/info/control2011/src/tools/krobot_vm.ml
index 2ee0244..3a44da9 100644
--- a/info/control2011/src/tools/krobot_vm.ml
+++ b/info/control2011/src/tools/krobot_vm.ml
@@ -89,6 +89,8 @@ type robot = {
mutable init_time : float option;
+ mutable delayed_action : (float * Krobot_action.t list) option;
+
ax12_front_low_left : ax12;
ax12_front_low_right : ax12;
ax12_front_high_left : ax12;
@@ -673,6 +675,13 @@ let rec exec robot actions =
else
exec robot actions
end
+
+ | Start_timer(delay,action) :: rest ->
+ let current_time = Unix.gettimeofday () in
+ robot.init_time <- Some (current_time);
+ robot.delayed_action <- Some (current_time +. delay, action);
+ (rest, Wait)
+
| End :: rest ->
([], Send_bus [Strategy_finished])
| _ :: rest ->
@@ -686,6 +695,19 @@ let run robot =
while_lwt true do
let timestamp = Unix.gettimeofday () in
+ (* check if the current time is higher than the delayed action *)
+ begin
+ match robot.delayed_action with
+ | Some (timeout,l) ->
+ if timeout >= timestamp
+ then begin
+ robot.delayed_action <- None;
+ robot.strategy <- l
+ end
+ | None ->
+ ()
+ end;
+
(* Check if a program asked for the strategy to change. *)
begin
match robot.change_strategy with
@@ -821,6 +843,7 @@ lwt () =
ax12_back_high_right = { ax12_position = 0; ax12_speed = 0; ax12_torque = 0 };
replace = false;
init_time = None;
+ delayed_action = None;
} in
(* Handle krobot message. *)
hooks/post-receive
--
krobot
|
|
From: Xavier L. <Ba...@us...> - 2013-04-25 22:50:04
|
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 8795043d51140b4e632d493acef16078577dca49 (commit)
from 04302e1607bd38ec213b2012d1af04fdc1e50433 (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 8795043d51140b4e632d493acef16078577dca49
Author: Xavier Lagorce <Xav...@cr...>
Date: Fri Apr 26 00:50:23 2013 +0200
[krobot-viewer] Added callback on new rotational speed limit field in ui
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/src/tools/krobot_viewer.ml b/info/control2011/src/tools/krobot_viewer.ml
index 897b658..4492bab 100644
--- a/info/control2011/src/tools/krobot_viewer.ml
+++ b/info/control2011/src/tools/krobot_viewer.ml
@@ -867,6 +867,9 @@ lwt () =
(ui#v_max#connect#value_changed
(fun () -> send_motor_limit ()));
ignore
+ (ui#omega_max#connect#value_changed
+ (fun () -> send_motor_limit ()));
+ ignore
(ui#a_tan_max#connect#value_changed
(fun () -> send_motor_limit ()));
ignore
hooks/post-receive
--
krobot
|
|
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
|
|
From: Pierre C. <Ba...@us...> - 2013-04-24 23:43:30
|
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 4ea1cf29d38266053fb64d3a0279213d3f6e02d3 (commit)
from 7e53c27bb7cc49869dcc77be44a3197065722367 (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 4ea1cf29d38266053fb64d3a0279213d3f6e02d3
Author: Pierre Chambart <pie...@oc...>
Date: Thu Apr 25 01:42:07 2013 +0200
Better robot radius handling in pathfinder
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/src/lib/krobot_config.ml b/info/control2011/src/lib/krobot_config.ml
index 127b63e..be24feb 100644
--- a/info/control2011/src/lib/krobot_config.ml
+++ b/info/control2011/src/lib/krobot_config.ml
@@ -11,16 +11,20 @@ let sqr x = x *. x
let world_height = 2.
let world_width = 3.
-let robot_length = 0.39
+let robot_length = 0.22
let robot_width = 0.33
let wheels_diameter = 0.098
let wheels_distance = 0.224
let wheels_position = 0.04
+let robot_radius =
+ let l1 = robot_length -. wheels_position in
+ let l2 = robot_width /. 2. in
+ sqrt (l1 *. l1 +. l2 *. l2)
let rotary_beacon_index_pos = 0.
let safety_margin = 0.01
-let beacon_radius = 0.4
+let beacon_radius = 0.2
let coin_radius = 0.06
diff --git a/info/control2011/src/lib/krobot_config.mli b/info/control2011/src/lib/krobot_config.mli
index 159f59f..1f88fa2 100644
--- a/info/control2011/src/lib/krobot_config.mli
+++ b/info/control2011/src/lib/krobot_config.mli
@@ -32,6 +32,9 @@ val wheels_position : float
(** The distance between the axe of the wheels and the back of the
robot. *)
+val robot_radius : float
+ (** distance between the point between the wheel and the farthest point *)
+
val rotary_beacon_index_pos : float
(** The angle of the rotary beacon index angle with respect to the
robot's front *)
diff --git a/info/control2011/src/lib/krobot_path.ml b/info/control2011/src/lib/krobot_path.ml
index 679581a..49b2311 100644
--- a/info/control2011/src/lib/krobot_path.ml
+++ b/info/control2011/src/lib/krobot_path.ml
@@ -21,12 +21,12 @@ let rec prev_last = function
let find ~src ~dst ~beacon =
let fixed_objects = List.map (fun { pos; size } -> pos,
- size +. Krobot_config.robot_width /. 2. +. 0.01)
+ size +. Krobot_config.robot_radius +. 0.01)
Krobot_config.fixed_obstacles in
(* do that in a better way when we have time... *)
let init_coins = List.map (fun pos -> pos,
- Krobot_config.coin_radius +. Krobot_config.robot_width /. 2. +. 0.01)
+ Krobot_config.coin_radius +. Krobot_config.robot_radius +. 0.01)
Krobot_config.initial_coins in
let l = fixed_objects @ init_coins in
@@ -35,22 +35,23 @@ let find ~src ~dst ~beacon =
| (Some v, None)
| (None, Some v) ->
ignore (Lwt_log.info_f "One beacon %f %f" v.x v.y);
- (v, beacon_radius +. safety_margin) :: l
+ (v, beacon_radius +. safety_margin +. Krobot_config.robot_radius) :: l
| (Some v1, Some v2) ->
ignore (Lwt_log.info_f "Two beacons (%f,%f) (%f,%f)" v1.x v1.y v2.x v2.y);
- (v1, beacon_radius +. safety_margin)
- :: (v2, beacon_radius +. safety_margin)
+ (v1, beacon_radius +. safety_margin +. Krobot_config.robot_radius)
+ :: (v2, beacon_radius +. safety_margin +. Krobot_config.robot_radius)
:: l
| (None, None) ->
ignore (Lwt_log.info_f "no beacon");
l
in
let l = List.map (fun (v,s) -> (v, min s (distance v src -. 0.1))) l in
+ let min_distance = Krobot_config.robot_radius +. safety_margin in
Krobot_pathfinding.find_path ~src ~dst
- ({ x = safety_margin;
- y = safety_margin},
- { x = world_width -. safety_margin;
- y = world_height -. safety_margin})
+ ({ x = min_distance;
+ y = min_distance},
+ { x = world_width -. min_distance;
+ y = world_height -. min_distance})
l
(*
let goto_object ~src ~dst ~beacon =
diff --git a/info/control2011/src/tools/krobot_viewer.ml b/info/control2011/src/tools/krobot_viewer.ml
index 81c0ef4..900d9b3 100644
--- a/info/control2011/src/tools/krobot_viewer.ml
+++ b/info/control2011/src/tools/krobot_viewer.ml
@@ -287,6 +287,13 @@ let draw viewer =
Krobot_config.fixed_obstacles
in
+ (* Draw the robot bounding circle *)
+
+ Cairo.arc ctx viewer.state.pos.x viewer.state.pos.y
+ Krobot_config.robot_radius 0. (2. *. pi);
+ Cairo.set_source_rgba ctx 1. 1. 1. 0.5;
+ Cairo.fill ctx;
+
(* Draw the robot and the ghost *)
List.iter
(fun (state, (r,g,b,alpha)) ->
hooks/post-receive
--
krobot
|
|
From: Pierre C. <Ba...@us...> - 2013-04-24 21:35:23
|
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 7e53c27bb7cc49869dcc77be44a3197065722367 (commit)
from 1958da2ab5fcb90d724dbe3f44b5c5a0e6843998 (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 7e53c27bb7cc49869dcc77be44a3197065722367
Author: Pierre Chambart <pie...@oc...>
Date: Wed Apr 24 23:34:25 2013 +0200
Remove symetry from Goto and Follow_path actions
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/src/lib/krobot_action.ml b/info/control2011/src/lib/krobot_action.ml
index 8009c90..8507d79 100644
--- a/info/control2011/src/lib/krobot_action.ml
+++ b/info/control2011/src/lib/krobot_action.ml
@@ -14,9 +14,9 @@ type t =
| Node of t option * t list
| Stop
| Think
- | Goto of bool * vertice * vector option
+ | Goto of vertice * vector option
| Set_limits of float * float * float
- | Follow_path of bool * vertice list * vector option * bool
+ | Follow_path of vertice list * vector option * bool
| Bezier of float * vertice * vertice * vertice * vertice * float
| Set_curve of Bezier.curve option
| Wait_for_jack of bool
@@ -64,13 +64,13 @@ let rec to_string = function
"Stop"
| Think ->
"Think"
- | Goto (reverted,v,vect) ->
- sprintf "Goto %b %s %s" reverted (string_of_vertice v) (string_of_option string_of_vector vect)
+ | 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_led (_,_) -> "Set_led"
- | Follow_path (reverted,l,vect, correct) ->
- sprintf "Follow_path [%b, %s, %s, %b]" reverted
+ | Follow_path (l,vect, correct) ->
+ sprintf "Follow_path [%s, %s, %b]"
(String.concat "; " (List.map string_of_vertice l))
(string_of_option string_of_vector vect)
correct
diff --git a/info/control2011/src/lib/krobot_action.mli b/info/control2011/src/lib/krobot_action.mli
index 9b6711e..e5c9f04 100644
--- a/info/control2011/src/lib/krobot_action.mli
+++ b/info/control2011/src/lib/krobot_action.mli
@@ -20,7 +20,7 @@ type t =
| Think
(** This is the highest-level action. It instruct the VM to
think about a new strategy. *)
- | Goto of bool * vertice * vector option
+ | Goto of vertice * vector option
(** Go to the given point.
if the bool parameter is true, the path is inverted according
to the robot team *)
@@ -31,10 +31,8 @@ type t =
(* TODO: en faire un node pour pouvoir revenir a des limites normales en sortant
d'une serie d'actions *)
- | Follow_path of bool * vertice list * vector option * bool
+ | Follow_path of vertice list * vector option * bool
(** Follow the given path. It does not check for obstacles.
- if the bool parameter is true, the path is inverted according
- to the robot team.
the last boolean tells to correct the bezier curves
to avoid obstacles.
Use only if the lines between vertices does not colide obstacles *)
diff --git a/info/control2011/src/tools/krobot_homologation.ml b/info/control2011/src/tools/krobot_homologation.ml
index 556c957..c0d7dcf 100644
--- a/info/control2011/src/tools/krobot_homologation.ml
+++ b/info/control2011/src/tools/krobot_homologation.ml
@@ -43,10 +43,10 @@ let strat_loop =
None,
Some (pi) );
- Goto (false, { x = 1.5; y = 0.5 }, None);
- Goto (false, { x = 1.5; y = 1.5 }, None);
- Goto (false, { x = 1.5; y = 0.5 }, None);
- Goto (false, { x = 1.5; y = 1.5 }, None);
+ Goto ({ x = 1.5; y = 0.5 }, None);
+ Goto ({ x = 1.5; y = 1.5 }, None);
+ Goto ({ x = 1.5; y = 0.5 }, None);
+ Goto ({ x = 1.5; y = 1.5 }, None);
Wait_for 2.;
End;
]
diff --git a/info/control2011/src/tools/krobot_planner.ml b/info/control2011/src/tools/krobot_planner.ml
index c12861a..6b8e8d5 100644
--- a/info/control2011/src/tools/krobot_planner.ml
+++ b/info/control2011/src/tools/krobot_planner.ml
@@ -148,7 +148,7 @@ let handle_message planner (timestamp, message) =
| Trajectory_go ->
let path = planner.vertices in
- ignore (Krobot_bus.send planner.bus (Unix.gettimeofday (), Strategy_set [Krobot_action.Follow_path (false,path,None,false)]));
+ ignore (Krobot_bus.send planner.bus (Unix.gettimeofday (), Strategy_set [Krobot_action.Follow_path (path,None,false)]));
set_vertices planner []
| Trajectory_find_path -> begin
diff --git a/info/control2011/src/tools/krobot_viewer.ml b/info/control2011/src/tools/krobot_viewer.ml
index 140b292..81c0ef4 100644
--- a/info/control2011/src/tools/krobot_viewer.ml
+++ b/info/control2011/src/tools/krobot_viewer.ml
@@ -615,10 +615,10 @@ let handle_message viewer (timestamp, message) =
queue_draw viewer
| Set_simulation_mode m ->
- match m with
- | Sim_HIL -> viewer.ui#menu_mode_hil#set_active true
- | _ -> viewer.ui#menu_mode_normal#set_active true
-
+ begin match m with
+ | Sim_HIL -> viewer.ui#menu_mode_hil#set_active true
+ | _ -> viewer.ui#menu_mode_normal#set_active true
+ end
| _ ->
()
end
@@ -795,7 +795,7 @@ lwt () =
if GdkEvent.Button.button ev = 1 then begin
match viewer.planner_path with
| curve :: _ ->
- ignore (Krobot_bus.send bus (Unix.gettimeofday (), Strategy_set [Krobot_action.Goto(false,Bezier.dst curve,None)]))
+ ignore (Krobot_bus.send bus (Unix.gettimeofday (), Strategy_set [Krobot_action.Goto(Bezier.dst curve,None)]))
| _ ->
()
end;
diff --git a/info/control2011/src/tools/krobot_vm.ml b/info/control2011/src/tools/krobot_vm.ml
index c88eca8..015573d 100644
--- a/info/control2011/src/tools/krobot_vm.ml
+++ b/info/control2011/src/tools/krobot_vm.ml
@@ -262,12 +262,6 @@ let string_of_test = function
| `Lt -> "Lt"
| `Le -> "Le"
-let revert_vertice v = { v with x = Krobot_config.world_width -. v.x }
-let revert_vector_opt v =
- match v with
- | None -> None
- | Some v -> Some { v with vx = -. v.vx }
-
let bezier_collide objects curve c1 c2 shift_vector curve_parameter =
let curve = Bezier.mul_d1 curve c1 in
let curve = Bezier.mul_d2 curve c2 in
@@ -424,12 +418,8 @@ let rec exec robot actions =
| Set_curve(Some curve) :: rest ->
robot.curve <- Some curve;
exec robot rest
- | Goto (revert,v,last_vector) :: rest -> begin
+ | Goto (v,last_vector) :: rest -> begin
ignore (Lwt_log.info_f "Goto");
- let v,last_vector = if revert && robot.team = `Blue
- then revert_vertice v, revert_vector_opt last_vector
- else v, last_vector
- in
(* Try to find a path to the destination. *)
match Krobot_path.find ~src:robot.position ~dst:v ~beacon:robot.beacon with
| Some vertices ->
@@ -438,11 +428,11 @@ let rec exec robot actions =
(Node (
Some
(Node (None, [Stop; Wait_for 0.05;
- Goto (false,v,last_vector)])),
- [Follow_path (false,vertices,last_vector, true)]) :: rest)
+ Goto (v,last_vector)])),
+ [Follow_path (vertices,last_vector, true)]) :: rest)
| None ->
- ([Stop; Wait_for 0.05; Goto (revert,v,last_vector)] @ rest,
+ ([Stop; Wait_for 0.05; Goto (v,last_vector)] @ rest,
Wait)
end
| Can c ::rest ->
@@ -458,12 +448,8 @@ let rec exec robot actions =
| `Green -> 6
| `Yellow -> 5 in
(rest, Send[Switch_request(led,value)])
- | Follow_path (revert,vertices,last_vector, correct_curve ) :: rest -> begin
+ | Follow_path (vertices,last_vector, correct_curve ) :: rest -> begin
ignore (Lwt_log.info_f "Follow_path");
- let vertices,vector = if revert && robot.team = `Blue
- then List.map revert_vertice vertices, revert_vector_opt last_vector
- else vertices, last_vector
- in
(* Compute bezier curves. *)
let vector = { vx = cos robot.orientation; vy = sin robot.orientation } in
(*
@@ -651,7 +637,7 @@ let rec exec robot actions =
| Calibrate ( approach_position, approach_orientation, distance,
supposed_x, supposed_y, supposed_orientation )::rest ->
Node (None,
- ([ Goto (false,approach_position,
+ ([ Goto (approach_position,
Some { vx = cos approach_orientation;
vy = cos approach_orientation });
Set_orientation approach_orientation;
hooks/post-receive
--
krobot
|
|
From: Xavier L. <Ba...@us...> - 2013-04-19 19:35:09
|
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 1958da2ab5fcb90d724dbe3f44b5c5a0e6843998 (commit)
via 69bced4234e5d3993c500e7c7a8e64a485db542a (commit)
from 269bb49a5e69a782d3da876587a11164b77b0915 (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 1958da2ab5fcb90d724dbe3f44b5c5a0e6843998
Author: Xavier Lagorce <Xav...@cr...>
Date: Fri Apr 19 21:35:01 2013 +0200
[ControllerMotorSTM32] Bugfixes and removed specificities for beacon motor
commit 69bced4234e5d3993c500e7c7a8e64a485db542a
Author: Xavier Lagorce <Xav...@cr...>
Date: Fri Apr 19 21:30:12 2013 +0200
[SensorActuator] Corrected bugs in firmware
-----------------------------------------------------------------------
Changes:
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 ee2d2b1..f9c90c2 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
@@ -192,7 +192,7 @@ static void NORETURN canMonitorListen_process(void) {
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;
+ simulation_mode_can_msg_t simulation_mode_msg;
bezier_can_msg_t bezier_msg;
bezier_limits_can_msg_t bezier_limits_msg;
motor_command_can_msg_t motor_command_msg;
@@ -312,10 +312,10 @@ static void NORETURN canMonitorListen_process(void) {
odo_setState(CONTROL_ODOMETRY, &odometry);
}
break;
- case CAN_MSG_CONTROLLER_MODE:
- controller_mode_msg.data32[0] = frame.data32[0];
- controller_mode_msg.data32[1] = frame.data32[0];
- switch (controller_mode_msg.data.mode) {
+ case CAN_MSG_SIMULATION_MODE:
+ simulation_mode_msg.data32[0] = frame.data32[0];
+ simulation_mode_msg.data32[1] = frame.data32[0];
+ switch (simulation_mode_msg.data.mode) {
case SIMULATION_MODE_NORMAL:
mc_change_mode(MOTOR3, CONTROLLER_MODE_HIL);
mc_change_mode(MOTOR4, CONTROLLER_MODE_HIL);
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 66e9f8c..ce2ad8c 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
@@ -95,9 +95,6 @@ static void init(void)
ENCODER1, ENCODER2,
2.0*M_PI/1024.0, -2.0*M_PI/1024.0);
- // Init beacon motor
- enableMotor(MOTOR2);
-
// Blink to say we are ready
for (uint8_t i=0; i < 5; i++) {
LED1_ON();
diff --git a/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/sensor_actuator/can/can_monitor.c b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/sensor_actuator/can/can_monitor.c
index 3f3e670..f867ecb 100644
--- a/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/sensor_actuator/can/can_monitor.c
+++ b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/sensor_actuator/can/can_monitor.c
@@ -29,6 +29,8 @@ PROC_DEFINE_STACK(stack_recv, KERN_MINSTACKSIZE * 4)
static void NORETURN can_sender_process(void);
static void NORETURN can_receiver_process(void);
+volatile uint8_t sim_mode = SIMULATION_MODE_NO;
+
void can_processes_init(void) {
can_config cfg;
@@ -73,9 +75,6 @@ static void NORETURN can_sender_process(void) {
ax12_state ax12_st;
- simulation_mode sim_mode_msg;
- uint8_t sim_mode = SIMULATION_MODE_NO;
-
int i = 0;
/* Initialize can frame */
@@ -209,10 +208,10 @@ static void NORETURN can_receiver_process(void) {
}
// General messages
switch (f.eid) {
- case CAN_MSG_SIMULATIOn_MODE:
+ case CAN_MSG_SIMULATION_MODE:
do {
- GET_PACKET(simulation_mode, sim_mode_msg, f);
- sim_mode = sim_mode_msg.mode;
+ GET_PACKET(simulation_mode, mode_req, f);
+ sim_mode = mode_req.p.mode;
} while (0);
break;
}
hooks/post-receive
--
krobot
|
|
From: Xavier L. <Ba...@us...> - 2013-04-18 16:31: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 269bb49a5e69a782d3da876587a11164b77b0915 (commit)
from 06a1b0b57845e008428267506e5d70a34da92261 (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 269bb49a5e69a782d3da876587a11164b77b0915
Author: Xavier Lagorce <Xav...@cr...>
Date: Thu Apr 18 18:34:47 2013 +0200
[krobot-simulator] Added detection of fixed objects in Urg emulation
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/src/tools/krobot_simulator.ml b/info/control2011/src/tools/krobot_simulator.ml
index ce5ab48..f32d5cf 100644
--- a/info/control2011/src/tools/krobot_simulator.ml
+++ b/info/control2011/src/tools/krobot_simulator.ml
@@ -445,6 +445,42 @@ let min_border_distance x y theta =
| [] -> None
| l -> Some (List.fold_left min max_float l)
+let distance_to_obj x y theta obj =
+ let x2, y2 = x +. cos theta, y +. sin theta in
+ let x3, y3, r = obj.pos.Krobot_geom.x, obj.pos.Krobot_geom.y, obj.size in
+ let a = (x2 -. x) ** 2. +. (y2 -. y) ** 2. in
+ let b = 2. *. ( (x2-.x)*.(x-.x3)+.(y2-.y)*.(y-.y3) ) in
+ let c = x3**2. +. y3**2. +. x**2. +. y**2. -. 2. *. (x*.x3+.y*.y3) -. r**2. in
+ let delta = b**2. -. 4. *.a*.c in
+ if abs_float delta < 0.01 then
+ Some (-.b/.(2.*.a))
+ else if delta > 0. then
+ let d1 = (-.b -. sqrt (delta)) /. (2.*.a) in
+ let d2 = (-.b +. sqrt (delta)) /. (2.*.a) in
+ if d1 < 0. && d2 < 0. then
+ None
+ else if d1 < 0. && d2 > 0. then
+ Some d2
+ else if d1 > 0. && d2 < 0. then
+ Some d1
+ else
+ Some (min d1 d2)
+ else
+ None
+
+let closest_obstacle x y theta objs =
+ let border = min_border_distance x y theta in
+ let dist_to_objs = List.map (distance_to_obj x y theta) objs in
+ List.fold_left
+ (fun min_dist dist -> match dist with
+ | Some dist ->
+ (match min_dist with
+ | Some min_dist -> Some (min min_dist dist)
+ | None -> Some dist)
+ | None -> min_dist)
+ border
+ dist_to_objs
+
(*let gen_data robot =
let dim = Array.length Krobot_config.urg_angles in
let {Krobot_geom.x=urg_rel_x;
@@ -475,7 +511,7 @@ let gen_data robot =
let l = ref [] in
for i = 0 to dim - 1 do
let angle = Krobot_config.urg_angles.(i) in
- match min_border_distance robot.x robot.y (robot.theta +. angle) with
+ match closest_obstacle robot.x robot.y (robot.theta +. angle) Krobot_config.fixed_obstacles with
| Some dist ->
let x = dist *. cos angle in
let y = dist *. sin angle in
hooks/post-receive
--
krobot
|
|
From: Xavier L. <Ba...@us...> - 2013-04-18 15:21:12
|
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 06a1b0b57845e008428267506e5d70a34da92261 (commit)
via 68147d413ca6994b81ac03b00beac5a52c77c325 (commit)
via 2366595d8df4e04ad9ff413a31af4cc705f1fc30 (commit)
from a10ecb5f43d6247ced5b9ee5ac1ef8909e9f92a7 (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 06a1b0b57845e008428267506e5d70a34da92261
Author: Xavier Lagorce <Xav...@cr...>
Date: Thu Apr 18 17:24:27 2013 +0200
[SensorActuator] Take into account new simulation mode message
commit 68147d413ca6994b81ac03b00beac5a52c77c325
Author: Xavier Lagorce <Xav...@cr...>
Date: Thu Apr 18 17:11:10 2013 +0200
[ControllerMotorSTM32] Take into account new simulation mode message
commit 2366595d8df4e04ad9ff413a31af4cc705f1fc30
Author: Xavier Lagorce <Xav...@cr...>
Date: Thu Apr 18 16:55:27 2013 +0200
[info/krobot-simulator] Changed mode message to include more simulation modes
-----------------------------------------------------------------------
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 156bf47..0b2cc5d 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,7 +31,7 @@
#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_SIMULATION_MODE 205 // simulation_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
#define CAN_MSG_MOTOR_COMMAND 208 // motor_command_can_msg_t
@@ -42,6 +42,13 @@
#define CAN_MSG_DRIVE_TORQUE_LIMIT 213 // drive_torque_limit_can_msg_t
/* +-----------------------------------------------------------------+
+ | Constants for messages |
+ +-----------------------------------------------------------------+ */
+#define SIMULATION_MODE_NO 0
+#define SIMULATION_MODE_NORMAL 1
+#define SIMULATION_MODE_HIL 2
+
+/* +-----------------------------------------------------------------+
| CAN messages data structures |
+-----------------------------------------------------------------+ */
@@ -148,10 +155,10 @@ typedef struct {
float rot_acc __attribute__((__packed__)); // Rotational acceleration for braking
} stop_msg_t;
-// Select robot mode (normal or HIL)
+// Select robot mode (normal, simulation or HIL)
typedef struct {
uint8_t mode;
-} controller_mode_msg_t;
+} simulation_mode_msg_t;
// Command the speed of a particular motor
typedef struct {
@@ -239,9 +246,9 @@ typedef union {
} stop_can_msg_t;
typedef union {
- controller_mode_msg_t data;
+ simulation_mode_msg_t data;
uint32_t data32[2];
-} controller_mode_can_msg_t;
+} simulation_mode_can_msg_t;
typedef union {
motor_command_msg_t data;
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 3d83b64..ee2d2b1 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
@@ -10,9 +10,10 @@
#include "can_monitor.h"
#include "hw/hw_led.h"
-#define ROBOT_MODE_NORMAL 0
-#define ROBOT_MODE_HIL 1
-#define ROBOT_MODE_FAULT 2
+#define ROBOT_MODE_NORMAL 0
+#define ROBOT_MODE_HIL 1
+#define ROBOT_MODE_FAULT 2
+#define ROBOT_MODE_SIMULATION 3
#define CONTROL_ODOMETRY 0
@@ -89,9 +90,9 @@ static void NORETURN canMonitor_process(void) {
txm.eid = CAN_MSG_ENCODERS34;
can_transmit(CAND1, &txm, ms_to_ticks(10));*/
- // Sending odometry data if not in HIL mode or motor commands if in HIL mode
- if (mode != ROBOT_MODE_HIL ||
- (mode == ROBOT_MODE_FAULT && old_mode == ROBOT_MODE_HIL)) {
+ // Sending odometry data if not in simulation mode or motor commands if in HIL mode
+ if (mode != ROBOT_MODE_HIL || mode != ROBOT_MODE_SIMULATION ||
+ (mode == ROBOT_MODE_FAULT && (old_mode == ROBOT_MODE_HIL || old_mode == ROBOT_MODE_SIMULATION))) {
odo_getState(0, &odometry);
msg_odo.data.x = (int16_t)(odometry.x * 1000.0);
msg_odo.data.y = (int16_t)(odometry.y * 1000.0);
@@ -121,7 +122,7 @@ static void NORETURN canMonitor_process(void) {
txm.data32[1] = msg_odo.data32[1];
txm.eid = CAN_MSG_ODOMETRY_INDEP;
can_transmit(CAND1, &txm, ms_to_ticks(10));
- } else {
+ } else if (mode == ROBOT_MODE_HIL || (mode == ROBOT_MODE_FAULT && old_mode == ROBOT_MODE_HIL)){
// Sending MOTOR3 data
msg_mot.data.position = mc_getPosition(MOTOR3);
msg_mot.data.speed = mc_getSpeed(MOTOR3);
@@ -146,6 +147,7 @@ static void NORETURN canMonitor_process(void) {
timer_add(&timer_can);
// Sending ghost state
+ if (mode != ROBOT_MODE_SIMULATION || (mode == ROBOT_MODE_FAULT && old_mode != ROBOT_MODE_SIMULATION))
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);
@@ -313,12 +315,20 @@ static void NORETURN canMonitorListen_process(void) {
case CAN_MSG_CONTROLLER_MODE:
controller_mode_msg.data32[0] = frame.data32[0];
controller_mode_msg.data32[1] = frame.data32[0];
- if (controller_mode_msg.data.mode == 1) {
+ switch (controller_mode_msg.data.mode) {
+ case SIMULATION_MODE_NORMAL:
+ mc_change_mode(MOTOR3, CONTROLLER_MODE_HIL);
+ mc_change_mode(MOTOR4, CONTROLLER_MODE_HIL);
+ odo_disable(CONTROL_ODOMETRY);
+ mode = ROBOT_MODE_SIMULATION;
+ break;
+ case SIMULATION_MODE_HIL:
mc_change_mode(MOTOR3, CONTROLLER_MODE_HIL);
mc_change_mode(MOTOR4, CONTROLLER_MODE_HIL);
odo_disable(CONTROL_ODOMETRY);
mode = ROBOT_MODE_HIL;
- } else {
+ break;
+ default:
mc_change_mode(MOTOR3, CONTROLLER_MODE_NORMAL);
mc_change_mode(MOTOR4, CONTROLLER_MODE_NORMAL);
odo_restart(CONTROL_ODOMETRY);
diff --git a/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/sensor_actuator/can/can_messages.h b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/sensor_actuator/can/can_messages.h
index 48c3b90..057833f 100644
--- a/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/sensor_actuator/can/can_messages.h
+++ b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/sensor_actuator/can/can_messages.h
@@ -59,6 +59,13 @@
//#define CAN_AX12_RESET 344 // Not used anymore
#define CAN_AX12_SET_TORQUE_ENABLE 345 // ax12_set_torque_enable
+// Simulation control
+#define CAN_MSG_SIMULATION_MODE 205 // simulation mode
+#define SIMULATION_MODE_NO 0
+#define SIMULATION_MODE_NORMAL 1
+#define SIMULATION_MODE_HIL 2
+
+
/****************************************************************************/
/**
@@ -159,6 +166,13 @@ struct ax12_set_torque_enable_pkt {
uint8_t enable;
};
+/**
+ * Simulation mode control
+ */
+struct simulation_mode_pkt {
+ uint8_t mode;
+} __attribute__((packed));
+
/****************************************************************************/
/**
@@ -220,4 +234,9 @@ typedef union {
uint32_t d[2];
} ax12_set_torque_enable;
+typedef union {
+ struct simulation_mode_pkt p;
+ uint32_t d[2];
+} simulation_mode;
+
#endif
diff --git a/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/sensor_actuator/can/can_monitor.c b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/sensor_actuator/can/can_monitor.c
index f8cf675..3f3e670 100644
--- a/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/sensor_actuator/can/can_monitor.c
+++ b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/sensor_actuator/can/can_monitor.c
@@ -73,6 +73,9 @@ static void NORETURN can_sender_process(void) {
ax12_state ax12_st;
+ simulation_mode sim_mode_msg;
+ uint8_t sim_mode = SIMULATION_MODE_NO;
+
int i = 0;
/* Initialize can frame */
@@ -86,51 +89,53 @@ static void NORETURN can_sender_process(void) {
for (;;) {
timer_add(&timer_send);
- /* Beacon */
- get_beacon_positions(&pos, &pos_ll);
+ if (sim_mode == SIMULATION_MODE_NO) {
+ /* Beacon */
+ get_beacon_positions(&pos, &pos_ll);
- SET_PACKET(f, CAN_BEACON_POSITION, pos);
- can_transmit(CAND1, &f, ms_to_ticks(10));
+ SET_PACKET(f, CAN_BEACON_POSITION, pos);
+ can_transmit(CAND1, &f, ms_to_ticks(10));
- SET_PACKET(f, CAN_BEACON_LOWLEVEL_POSITION, pos_ll);
- can_transmit(CAND1, &f, ms_to_ticks(10));
+ SET_PACKET(f, CAN_BEACON_LOWLEVEL_POSITION, pos_ll);
+ can_transmit(CAND1, &f, ms_to_ticks(10));
- /* Switches */
+ /* Switches */
- get_switch_status(&st1, &st2);
+ get_switch_status(&st1, &st2);
- SET_PACKET(f, CAN_SWITCH_STATUS_1, st1);
- can_transmit(CAND1, &f, ms_to_ticks(10));
+ SET_PACKET(f, CAN_SWITCH_STATUS_1, st1);
+ can_transmit(CAND1, &f, ms_to_ticks(10));
- SET_PACKET(f, CAN_SWITCH_STATUS_2, st2);
- can_transmit(CAND1, &f, ms_to_ticks(10));
+ SET_PACKET(f, CAN_SWITCH_STATUS_2, st2);
+ can_transmit(CAND1, &f, ms_to_ticks(10));
- /* ADC */
+ /* ADC */
- get_adc_values(&adc1, &adc2);
+ get_adc_values(&adc1, &adc2);
- SET_PACKET(f, CAN_ADC_VALUES_1, adc1);
- can_transmit(CAND1, &f, ms_to_ticks(10));
+ SET_PACKET(f, CAN_ADC_VALUES_1, adc1);
+ can_transmit(CAND1, &f, ms_to_ticks(10));
- SET_PACKET(f, CAN_ADC_VALUES_2, adc2);
- can_transmit(CAND1, &f, ms_to_ticks(10));
+ SET_PACKET(f, CAN_ADC_VALUES_2, adc2);
+ can_transmit(CAND1, &f, ms_to_ticks(10));
- /* Battery monitoring */
+ /* Battery monitoring */
- if (get_battery_monitoring(&battery1, &battery2) == 0) {
+ if (get_battery_monitoring(&battery1, &battery2) == 0) {
- SET_PACKET(f, CAN_BATTERY_STATUS_1, battery1);
- can_transmit(CAND1, &f, ms_to_ticks(10));
+ SET_PACKET(f, CAN_BATTERY_STATUS_1, battery1);
+ can_transmit(CAND1, &f, ms_to_ticks(10));
- SET_PACKET(f, CAN_BATTERY_STATUS_2, battery2);
- can_transmit(CAND1, &f, ms_to_ticks(10));
- }
+ SET_PACKET(f, CAN_BATTERY_STATUS_2, battery2);
+ can_transmit(CAND1, &f, ms_to_ticks(10));
+ }
- /* AX-12 */
+ /* AX-12 */
- while (ax12_dequeue_state(&ax12_st) == 0) {
- SET_PACKET(f, CAN_AX12_STATE, ax12_st);
- can_transmit(CAND1, &f, ms_to_ticks(10));
+ while (ax12_dequeue_state(&ax12_st) == 0) {
+ SET_PACKET(f, CAN_AX12_STATE, ax12_st);
+ can_transmit(CAND1, &f, ms_to_ticks(10));
+ }
}
if (i)
@@ -158,46 +163,58 @@ static void NORETURN can_receiver_process(void) {
if (!ret || f.ide != 1)
continue;
- switch (f.eid) {
+ // These messages are to be interpreted only when not in simulation mode
+ if (sim_mode == SIMULATION_MODE_NO) {
+ switch (f.eid) {
case CAN_SWITCH_SET:
do {
- GET_PACKET(switch_request, req, f);
- set_switch(&req);
+ GET_PACKET(switch_request, req, f);
+ set_switch(&req);
} while (0);
break;
case CAN_AX12_REQUEST_STATE:
do {
- struct ax12_hl_command hlc;
- GET_PACKET(ax12_request_state, ax12_req, f);
- hlc.command = AX12_HL_GET_STATE;
- hlc.address = ax12_req.p.address;
- ax12_queue_command(&hlc);
+ struct ax12_hl_command hlc;
+ GET_PACKET(ax12_request_state, ax12_req, f);
+ hlc.command = AX12_HL_GET_STATE;
+ hlc.address = ax12_req.p.address;
+ ax12_queue_command(&hlc);
} while (0);
break;
case CAN_AX12_GOTO:
do {
- struct ax12_hl_command hlc;
- GET_PACKET(ax12_goto, ax12_g, f);
- hlc.command = AX12_HL_GOTO;
- hlc.address = ax12_g.p.address;
- hlc.args[0] = ax12_g.p.position;
- hlc.args[1] = ax12_g.p.speed;
- ax12_queue_command(&hlc);
+ struct ax12_hl_command hlc;
+ GET_PACKET(ax12_goto, ax12_g, f);
+ hlc.command = AX12_HL_GOTO;
+ hlc.address = ax12_g.p.address;
+ hlc.args[0] = ax12_g.p.position;
+ hlc.args[1] = ax12_g.p.speed;
+ ax12_queue_command(&hlc);
} while (0);
break;
case CAN_AX12_SET_TORQUE_ENABLE:
do {
- struct ax12_hl_command hlc;
- GET_PACKET(ax12_set_torque_enable, ax12_ste, f);
- hlc.command = AX12_HL_SET_TORQUE_ENABLE;
- hlc.address = ax12_ste.p.address;
- hlc.args[0] = ax12_ste.p.enable;
- ax12_queue_command(&hlc);
+ struct ax12_hl_command hlc;
+ GET_PACKET(ax12_set_torque_enable, ax12_ste, f);
+ hlc.command = AX12_HL_SET_TORQUE_ENABLE;
+ hlc.address = ax12_ste.p.address;
+ hlc.args[0] = ax12_ste.p.enable;
+ ax12_queue_command(&hlc);
} while (0);
break;
default:
break;
+ }
+ }
+ // General messages
+ switch (f.eid) {
+ case CAN_MSG_SIMULATIOn_MODE:
+ do {
+ GET_PACKET(simulation_mode, sim_mode_msg, f);
+ sim_mode = sim_mode_msg.mode;
+ } while (0);
+ break;
}
}
diff --git a/info/control2011/src/lib/krobot_message.ml b/info/control2011/src/lib/krobot_message.ml
index 85a32c6..358c399 100644
--- a/info/control2011/src/lib/krobot_message.ml
+++ b/info/control2011/src/lib/krobot_message.ml
@@ -18,6 +18,8 @@ open Lwt_react
type direction = Forward | Backward
+type simulation_mode = Sim_no | Sim_normal | Sim_HIL
+
type t =
| Battery1_voltages of float * float * float * float
| Battery2_voltages of float * float * float * float
@@ -54,7 +56,7 @@ type t =
| Odometry_ghost of float * float * float * int * bool
| Set_odometry of float * float * float
| Set_odometry_indep of float * float * float
- | Set_controller_mode of bool
+ | Set_simulation_mode of simulation_mode
| Elevator of float * float
| Req_motor_status
| Unknown of frame
@@ -67,6 +69,11 @@ let string_of_direction = function
| Forward -> "Forward"
| Backward -> "Backward"
+let string_of_simulation_mode = function
+ | Sim_no -> "Normal mode"
+ | Sim_normal -> "Simulation mode"
+ | Sim_HIL -> "HIL mode"
+
let to_string = function
| Battery1_voltages(elem1, elem2, elem3, elem4) ->
sprintf
@@ -222,10 +229,10 @@ let to_string = function
sprintf
"Set_odometry_indep(%f, %f, %f)"
x y theta
- | Set_controller_mode b ->
+ | Set_simulation_mode m ->
sprintf
- "Set_controller_mode(%B)"
- b
+ "Set_simulation_mode(%s)"
+ (string_of_simulation_mode m)
| Elevator(p1, p2) ->
sprintf
"Elevator(%f, %f)"
@@ -641,13 +648,16 @@ let encode = function
~remote:false
~format:F29bits
~data
- | Set_controller_mode b ->
+ | Set_simulation_mode m ->
frame
~identifier:205
~kind:Data
~remote:false
~format:F29bits
- ~data:(if b then "\x01" else "\x00")
+ ~data:(match m with
+ | Sim_no -> "\x00"
+ | Sim_normal -> "\x01"
+ | Sim_HIL -> "\x02")
| Elevator(p1, p2) ->
let data = String.create 8 in
put_float32 data 0 p1;
@@ -761,8 +771,9 @@ let decode frame =
(get_float32 frame.data 0,
get_float32 frame.data 4)
| 205 ->
- Set_controller_mode
- (get_uint8 frame.data 0 <> 0)
+ Set_simulation_mode
+ (let v = get_uint8 frame.data 0 in
+ if v == 1 then Sim_normal else if v == 2 then Sim_HIL else Sim_no)
| 206 ->
let x, y, d1, d2, theta, v = decode_bezier frame.data in
Motor_bezier(float x /. 1000.,
diff --git a/info/control2011/src/lib/krobot_message.mli b/info/control2011/src/lib/krobot_message.mli
index f55a58f..5486768 100644
--- a/info/control2011/src/lib/krobot_message.mli
+++ b/info/control2011/src/lib/krobot_message.mli
@@ -12,6 +12,9 @@
type direction = Forward | Backward
(** Type of directions. *)
+type simulation_mode = Sim_no | Sim_normal | Sim_HIL
+ (** Type of simulation. *)
+
(** Type of messages. *)
type t =
| Battery1_voltages of float * float * float * float
@@ -117,8 +120,8 @@ type t =
| Set_odometry_indep of float * float * float
(** [set_odometry_ident(x, y, theta)] sets the parameters of the
independant odometry to the given ones. *)
- | Set_controller_mode of bool
- (** Put the card into simulation mode or not. *)
+ | Set_simulation_mode of simulation_mode
+ (** Put the cards into a given simulation mode. *)
| Elevator of float * float
(** Set the position of the elevators, between 0 and 1. Negative
positions are ignored. *)
diff --git a/info/control2011/src/tools/krobot_simulator.ml b/info/control2011/src/tools/krobot_simulator.ml
index 2ba335f..ce5ab48 100644
--- a/info/control2011/src/tools/krobot_simulator.ml
+++ b/info/control2011/src/tools/krobot_simulator.ml
@@ -27,12 +27,19 @@ let fork = ref true
let hil = ref false
let sensors_emu = ref true
let robot_sim = ref true
+let go_normal = ref false
+let go_simu = ref false
+let go_hil = ref false
let options = Arg.align [
"-no-fork", Arg.Clear fork, " Run in foreground";
"-no-sensor", Arg.Clear sensors_emu, " Don't emulate sensor inputs";
"-no-simulation", Arg.Clear robot_sim, " Don't simulate the robot";
"-hil", Arg.Set hil, " Run in hardware in the loop mode";
+ "-go-normal", Arg.Set go_normal, " Put the card in normal mode and exit";
+ "-go-simulation", Arg.Set go_simu, " Put the cards in simulation mode and exit";
+ "-go-normal", Arg.Set go_normal, " Put the cards in normal mode and exit";
+ "-go-hil", Arg.Set go_hil, " Put the cards in hardware in the loop mode and exit";
]
let usage = "\
@@ -500,6 +507,17 @@ lwt () =
(* Open the krobot bus. *)
lwt bus = Krobot_bus.get () in
+ if (!go_hil or !go_simu or !go_normal) then begin
+ if !go_normal then
+ ignore(Krobot_message.send bus (Unix.gettimeofday (), Set_simulation_mode Sim_no))
+ else if !go_hil then
+ ignore(Krobot_message.send bus (Unix.gettimeofday (), Set_simulation_mode Sim_HIL))
+ else if !go_simu then
+ ignore(Krobot_message.send bus (Unix.gettimeofday (), Set_simulation_mode Sim_normal))
+ else
+ exit 0
+ end;
+
(* Fork if not prevented. *)
if !fork then Krobot_daemon.daemonize bus;
@@ -531,13 +549,19 @@ lwt () =
} in
sim := Some local_sim;
+ (* Set the correct simulation mode *)
+ if !robot_sim || !sensors_emu then
+ (if !hil then
+ ignore(Krobot_message.send bus (Unix.gettimeofday (), Set_simulation_mode Sim_HIL))
+ else
+ ignore(Krobot_message.send bus (Unix.gettimeofday (), Set_simulation_mode Sim_normal))
+ );
+ (* Launch robot simulation *)
if !robot_sim then begin
- (* Set the motor_controller card in HIL mode if necessary *)
- if !hil then
- ignore (Krobot_message.send bus (Unix.gettimeofday (), Set_controller_mode true)) ;
ignore(send_CAN_messages local_sim bus);
ignore(loop bus local_sim)
end;
+ (* Launch sensor simulation *)
if !sensors_emu then begin
ignore(loop_urg local_sim bus)
end;
diff --git a/info/control2011/src/tools/krobot_viewer.ml b/info/control2011/src/tools/krobot_viewer.ml
index 564a27c..140b292 100644
--- a/info/control2011/src/tools/krobot_viewer.ml
+++ b/info/control2011/src/tools/krobot_viewer.ml
@@ -614,11 +614,10 @@ let handle_message viewer (timestamp, message) =
viewer.ui#beacon_period#set_text (string_of_int period);
queue_draw viewer
- | Set_controller_mode hil ->
- if hil then
- viewer.ui#menu_mode_hil#set_active true
- else
- viewer.ui#menu_mode_normal#set_active true
+ | Set_simulation_mode m ->
+ match m with
+ | Sim_HIL -> viewer.ui#menu_mode_hil#set_active true
+ | _ -> viewer.ui#menu_mode_normal#set_active true
| _ ->
()
@@ -830,7 +829,7 @@ lwt () =
ignore (
Krobot_message.send bus
(Unix.gettimeofday (),
- Set_controller_mode false)
+ Set_simulation_mode Sim_no)
);
false));
@@ -841,7 +840,7 @@ lwt () =
ignore_result (
Krobot_message.send bus
(Unix.gettimeofday (),
- Set_controller_mode true)
+ Set_simulation_mode Sim_HIL)
);
false));
hooks/post-receive
--
krobot
|
|
From: Xavier L. <Ba...@us...> - 2013-04-18 13:43:12
|
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 a10ecb5f43d6247ced5b9ee5ac1ef8909e9f92a7 (commit)
from 0140e38a6bc4281a82ece8fac326d852bcc9377c (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 a10ecb5f43d6247ced5b9ee5ac1ef8909e9f92a7
Author: Xavier Lagorce <Xav...@cr...>
Date: Thu Apr 18 15:29:59 2013 +0200
[info/krobot-simulator] Added options to select what to simulate
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/src/tools/krobot_simulator.ml b/info/control2011/src/tools/krobot_simulator.ml
index c307a52..2ba335f 100644
--- a/info/control2011/src/tools/krobot_simulator.ml
+++ b/info/control2011/src/tools/krobot_simulator.ml
@@ -25,14 +25,18 @@ let time_step = 0.001
let fork = ref true
let hil = ref false
+let sensors_emu = ref true
+let robot_sim = ref true
let options = Arg.align [
"-no-fork", Arg.Clear fork, " Run in foreground";
+ "-no-sensor", Arg.Clear sensors_emu, " Don't emulate sensor inputs";
+ "-no-simulation", Arg.Clear robot_sim, " Don't simulate the robot";
"-hil", Arg.Set hil, " Run in hardware in the loop mode";
]
let usage = "\
-Usage: krobot-mc-simulator [options]
+Usage: krobot-simulator [options]
options are:"
(* +-----------------------------------------------------------------+
@@ -349,7 +353,7 @@ let send_CAN_messages sim bus =
let handle_message bus (timestamp, message) =
match message with
- | Kill "mc_simulator" ->
+ | Kill "simulator" ->
exit 0
| CAN(_, frame) -> begin
match !sim with
@@ -502,16 +506,12 @@ lwt () =
(* Handle krobot message. *)
E.keep (E.map (handle_message bus) (Krobot_bus.recv bus));
- (* Kill any running mc_simulator. *)
- lwt () = Krobot_bus.send bus (Unix.gettimeofday (), Krobot_bus.Kill "mc_simulator") in
+ (* Kill any running simulator. *)
+ lwt () = Krobot_bus.send bus (Unix.gettimeofday (), Krobot_bus.Kill "simulator") in
(* Wait a bit to let the other handler release the connection *)
lwt () = Lwt_unix.sleep 0.4 in
- (* Set the motor_controller card in HIL mode if necessary *)
- if !hil then
- ignore (Krobot_message.send bus (Unix.gettimeofday (), Set_controller_mode true)) ;
-
(* Initial state of the simulator *)
let local_sim = {
state = { x = 0.; y = 0.; theta = 0. };
@@ -531,8 +531,17 @@ lwt () =
} in
sim := Some local_sim;
- ignore(send_CAN_messages local_sim bus);
- ignore(loop_urg local_sim bus);
-
- (* Loop forever. *)
- Lwt_unix.run (loop bus local_sim)
+ if !robot_sim then begin
+ (* Set the motor_controller card in HIL mode if necessary *)
+ if !hil then
+ ignore (Krobot_message.send bus (Unix.gettimeofday (), Set_controller_mode true)) ;
+ ignore(send_CAN_messages local_sim bus);
+ ignore(loop bus local_sim)
+ end;
+ if !sensors_emu then begin
+ ignore(loop_urg local_sim bus)
+ end;
+
+ (* Run forever *)
+ let t, _ = Lwt.wait () in
+ t
hooks/post-receive
--
krobot
|
|
From: Xavier L. <Ba...@us...> - 2013-04-18 12:13:34
|
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 0140e38a6bc4281a82ece8fac326d852bcc9377c (commit)
from e7d6c3175ae17c43e72df1fc2e7b56c1b9bfdff9 (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 0140e38a6bc4281a82ece8fac326d852bcc9377c
Author: Xavier Lagorce <Xav...@cr...>
Date: Thu Apr 18 14:16:49 2013 +0200
[info/krobot-simulator] Only one application regrouping all simulators from now on.
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/_oasis b/info/control2011/_oasis
index be457fb..94c60bf 100644
--- a/info/control2011/_oasis
+++ b/info/control2011/_oasis
@@ -169,27 +169,6 @@ Executable "krobot-simulator"
MainIs: krobot_simulator.ml
BuildDepends: krobot, lwt.syntax
-Executable "krobot-hil-simulator"
- Path: src/tools
- Install: true
- CompiledObject: best
- MainIs: krobot_hil_simulator.ml
- BuildDepends: krobot, lwt.syntax
-
-Executable "krobot-mc-simulator"
- Path: src/tools
- Install: true
- CompiledObject: best
- MainIs: krobot_mc_simulator.ml
- BuildDepends: krobot, lwt.syntax
-
-Executable "krobot-io-simulator"
- Path: src/tools
- Install: true
- CompiledObject: best
- MainIs: krobot_io_simulator.ml
- BuildDepends: krobot, lwt.syntax
-
Executable "krobot-planner"
Path: src/tools
Install: true
diff --git a/info/control2011/_tags b/info/control2011/_tags
index 9af45e4..8609920 100644
--- a/info/control2011/_tags
+++ b/info/control2011/_tags
@@ -2,7 +2,7 @@
<src/interfaces/*.ml>: -syntax_camlp4o
# OASIS_START
-# DO NOT EDIT (digest: 44c1536155bea156ff4e324744cffda8)
+# DO NOT EDIT (digest: 9a283fb86531fb8de178da3ad5f872b1)
# Library krobot
"src/lib": include
<src/lib/krobot.{cma,cmxa}>: use_libkrobot
@@ -84,11 +84,6 @@
<src/tools/krobot_monitor.{native,byte}>: pkg_lwt.unix
<src/tools/krobot_monitor.{native,byte}>: pkg_lwt.syntax
<src/tools/krobot_monitor.{native,byte}>: pkg_lwt.react
-# Executable krobot-hil-simulator
-<src/tools/krobot_hil_simulator.{native,byte}>: use_krobot
-<src/tools/krobot_hil_simulator.{native,byte}>: pkg_lwt.unix
-<src/tools/krobot_hil_simulator.{native,byte}>: pkg_lwt.syntax
-<src/tools/krobot_hil_simulator.{native,byte}>: pkg_lwt.react
# Executable krobot-hub
<src/tools/krobot_hub.{native,byte}>: pkg_lwt.unix
<src/tools/krobot_hub.{native,byte}>: pkg_lwt.syntax
@@ -192,11 +187,6 @@
<src/tools/krobot_dump_ax12.{native,byte}>: pkg_lwt.syntax
<src/tools/krobot_dump_ax12.{native,byte}>: pkg_lwt.react
<src/tools/krobot_dump_ax12.{native,byte}>: pkg_bitstring
-# Executable krobot-mc-simulator
-<src/tools/krobot_mc_simulator.{native,byte}>: use_krobot
-<src/tools/krobot_mc_simulator.{native,byte}>: pkg_lwt.unix
-<src/tools/krobot_mc_simulator.{native,byte}>: pkg_lwt.syntax
-<src/tools/krobot_mc_simulator.{native,byte}>: pkg_lwt.react
# Executable krobot-replay
<src/tools/krobot_replay.{native,byte}>: use_krobot
<src/tools/krobot_replay.{native,byte}>: pkg_lwt.unix
@@ -260,11 +250,6 @@
<src/tools/krobot_homologation.{native,byte}>: pkg_lwt.unix
<src/tools/krobot_homologation.{native,byte}>: pkg_lwt.syntax
<src/tools/krobot_homologation.{native,byte}>: pkg_lwt.react
-# Executable krobot-io-simulator
-<src/tools/krobot_io_simulator.{native,byte}>: use_krobot
-<src/tools/krobot_io_simulator.{native,byte}>: pkg_lwt.unix
-<src/tools/krobot_io_simulator.{native,byte}>: pkg_lwt.syntax
-<src/tools/krobot_io_simulator.{native,byte}>: pkg_lwt.react
# Executable krobot-pet
<src/tools/krobot_pet.{native,byte}>: use_krobot
<src/tools/krobot_pet.{native,byte}>: pkg_lwt.unix
diff --git a/info/control2011/setup.ml b/info/control2011/setup.ml
index 2b5c6f0..e0cc01c 100644
--- a/info/control2011/setup.ml
+++ b/info/control2011/setup.ml
@@ -8,7 +8,7 @@
*)
(* OASIS_START *)
-(* DO NOT EDIT (digest: bf059af587069cdcfbcb6e820bb05831) *)
+(* DO NOT EDIT (digest: a04a83a7ce5a2eba466a7d0057270c5b) *)
(*
Regenerated by OASIS v0.2.0
Visit http://oasis.forge.ocamlcore.org for more information and
@@ -5375,36 +5375,6 @@ let setup_t =
});
Executable
({
- cs_name = "krobot-hil-simulator";
- cs_data = PropList.Data.create ();
- cs_plugin_data = [];
- },
- {
- bs_build = [(OASISExpr.EBool true, true)];
- bs_install = [(OASISExpr.EBool true, true)];
- bs_path = "src/tools";
- bs_compiled_object = Best;
- bs_build_depends =
- [
- InternalLibrary "krobot";
- FindlibPackage ("lwt.syntax", None)
- ];
- bs_build_tools = [ExternalTool "ocamlbuild"];
- bs_c_sources = [];
- bs_data_files = [];
- bs_ccopt = [(OASISExpr.EBool true, [])];
- bs_cclib = [(OASISExpr.EBool true, [])];
- bs_dlllib = [(OASISExpr.EBool true, [])];
- bs_dllpath = [(OASISExpr.EBool true, [])];
- bs_byteopt = [(OASISExpr.EBool true, [])];
- bs_nativeopt = [(OASISExpr.EBool true, [])];
- },
- {
- exec_custom = false;
- exec_main_is = "krobot_hil_simulator.ml";
- });
- Executable
- ({
cs_name = "krobot-hub";
cs_data = PropList.Data.create ();
cs_plugin_data = [];
@@ -5977,36 +5947,6 @@ let setup_t =
});
Executable
({
- cs_name = "krobot-mc-simulator";
- cs_data = PropList.Data.create ();
- cs_plugin_data = [];
- },
- {
- bs_build = [(OASISExpr.EBool true, true)];
- bs_install = [(OASISExpr.EBool true, true)];
- bs_path = "src/tools";
- bs_compiled_object = Best;
- bs_build_depends =
- [
- InternalLibrary "krobot";
- FindlibPackage ("lwt.syntax", None)
- ];
- bs_build_tools = [ExternalTool "ocamlbuild"];
- bs_c_sources = [];
- bs_data_files = [];
- bs_ccopt = [(OASISExpr.EBool true, [])];
- bs_cclib = [(OASISExpr.EBool true, [])];
- bs_dlllib = [(OASISExpr.EBool true, [])];
- bs_dllpath = [(OASISExpr.EBool true, [])];
- bs_byteopt = [(OASISExpr.EBool true, [])];
- bs_nativeopt = [(OASISExpr.EBool true, [])];
- },
- {
- exec_custom = false;
- exec_main_is = "krobot_mc_simulator.ml";
- });
- Executable
- ({
cs_name = "krobot-replay";
cs_data = PropList.Data.create ();
cs_plugin_data = [];
@@ -6226,36 +6166,6 @@ let setup_t =
});
Executable
({
- cs_name = "krobot-io-simulator";
- cs_data = PropList.Data.create ();
- cs_plugin_data = [];
- },
- {
- bs_build = [(OASISExpr.EBool true, true)];
- bs_install = [(OASISExpr.EBool true, true)];
- bs_path = "src/tools";
- bs_compiled_object = Best;
- bs_build_depends =
- [
- InternalLibrary "krobot";
- FindlibPackage ("lwt.syntax", None)
- ];
- bs_build_tools = [ExternalTool "ocamlbuild"];
- bs_c_sources = [];
- bs_data_files = [];
- bs_ccopt = [(OASISExpr.EBool true, [])];
- bs_cclib = [(OASISExpr.EBool true, [])];
- bs_dlllib = [(OASISExpr.EBool true, [])];
- bs_dllpath = [(OASISExpr.EBool true, [])];
- bs_byteopt = [(OASISExpr.EBool true, [])];
- bs_nativeopt = [(OASISExpr.EBool true, [])];
- },
- {
- exec_custom = false;
- exec_main_is = "krobot_io_simulator.ml";
- });
- Executable
- ({
cs_name = "krobot-pet";
cs_data = PropList.Data.create ();
cs_plugin_data = [];
diff --git a/info/control2011/src/tools/krobot_hil_simulator.ml b/info/control2011/src/tools/krobot_hil_simulator.ml
deleted file mode 100644
index 9fcf5cd..0000000
--- a/info/control2011/src/tools/krobot_hil_simulator.ml
+++ /dev/null
@@ -1,142 +0,0 @@
-(*
- * krobot_hil_simulator.ml
- * -----------------------
- * Copyright : (c) 2011, Jeremie Dimino <je...@di...>
- * Licence : BSD3
- *
- * This file is a part of [kro]bot.
- *)
-
-(* Simulate the robot. *)
-
-open Lwt
-open Lwt_react
-open Krobot_config
-open Krobot_message
-open Krobot_geom
-
-(* +-----------------------------------------------------------------+
- | Types |
- +-----------------------------------------------------------------+ *)
-
-(* State of the robot. *)
-type state = {
- x : float;
- y : float;
- theta : float;
-}
-
-type internal_state = {
- theta_l : float;
- theta_r : float;
-}
-
-type command =
- | Idle
- (* The robot is doint nothing. *)
- | Speed of float * float
- (* [Speed(left_velocity, right_velocity)] *)
- | Turn of float * float
- (* [Turn(t_acc, velocity)] *)
- | Move of float * float
- (* [Move(t_acc, velocity)] *)
-
-(* Type of simulators. *)
-type simulator = {
- mutable state : state;
- (* The state of the robot. *)
- mutable internal_state : internal_state;
- (* The state of the wheels. *)
- mutable velocity_l : float;
- (* Velocity of the left motor (read from the CAN). *)
- mutable velocity_r : float;
- (* Velocity of the right motor (read from the CAN). *)
- mutable time : float;
- (* The current time. *)
-}
-
-(* +-----------------------------------------------------------------+
- | Entry point |
- +-----------------------------------------------------------------+ *)
-
-let print sim =
- Lwt_log.debug_f "
-time = %f
-state:
- x = %f
- y = %f
- theta = %f
-internal_state:
- theta_l = %f
- theta_r = %f
-velocities:
- left = %f
- right = %f
-"
- sim.time
- sim.state.x
- sim.state.y
- sim.state.theta
- sim.internal_state.theta_l
- sim.internal_state.theta_r
- sim.velocity_l
- sim.velocity_r
-
-lwt () =
- lwt bus = Krobot_bus.get () in
-
- let sim = {
- state = { x = 0.; y = 0.; theta = 0. };
- internal_state = { theta_l = 0.; theta_r = 0. };
- velocity_l = 0.;
- velocity_r = 0.;
- time = Unix.gettimeofday ();
- } in
-
- (* Handle commands. *)
- E.keep
- (E.map_s
- (fun (ts, msg) ->
- match msg with
- | Encoder_position_speed_3(pos, speed) ->
- sim.velocity_r <- speed;
- return ()
- | Encoder_position_speed_4(pos, speed) ->
- sim.velocity_l <- speed;
- return ()
- | Set_odometry(x, y, theta) ->
- sim.state <- { x; y; theta };
- return ()
- | _ ->
- return ())
- (Krobot_message.recv bus));
-
- lwt () = Krobot_message.send bus (Unix.gettimeofday (), Set_controller_mode true) in
-
- while_lwt true do
- let time = Unix.gettimeofday () in
- let delta = time -. sim.time in
- sim.time <- time;
-
- (* Sends the state of the robot. *)
- lwt () = Krobot_message.send bus (sim.time, Odometry(sim.state.x, sim.state.y, sim.state.theta)) in
-
- lwt () = print sim in
-
- let u1 = (sim.velocity_l +. sim.velocity_r) *. wheels_diameter /. 4.
- and u2 = (sim.velocity_l -. sim.velocity_r) *. wheels_diameter /. wheels_distance /. 2. in
- let dx = u1 *. (cos sim.state.theta)
- and dy = u1 *. (sin sim.state.theta)
- and dtheta = u2 in
- sim.state <- {
- x = sim.state.x +. dx *. delta;
- y = sim.state.y +. dy *. delta;
- theta = math_mod_float (sim.state.theta +. dtheta *. delta) (2. *. pi);
- };
- sim.internal_state <- {
- theta_l = sim.internal_state.theta_l +. delta *. (u1 *. 4. +. u2 *. wheels_distance) /. (2. *. wheels_diameter);
- theta_r = sim.internal_state.theta_r +. delta *. (u1 *. 4. -. u2 *. wheels_distance) /. (2. *. wheels_diameter);
- };
-
- Lwt_unix.sleep 0.01
- done
diff --git a/info/control2011/src/tools/krobot_io_simulator.ml b/info/control2011/src/tools/krobot_io_simulator.ml
deleted file mode 100644
index dbaf6e6..0000000
--- a/info/control2011/src/tools/krobot_io_simulator.ml
+++ /dev/null
@@ -1,149 +0,0 @@
-(*
- * krobot_io_simulator.ml
- * ----------------
- * Copyright : (c) 2013, Xavier Lagorce <Xav...@cr...>
- * Licence : BSD3
- *
- * This file is a part of [kro]bot.
- *)
-
-(* Service simulating all inputs/outputs from the robot *)
-
-open Lwt
-open Lwt_react
-open Lwt_preemptive
-open Krobot_bus
-open Krobot_message
-open Krobot_geom
-
-let section = Lwt_log.Section.make "krobot(io_simulator)"
-
-(* +-----------------------------------------------------------------+
- | Types |
- +-----------------------------------------------------------------+ *)
-
-(* State of the robot. *)
-type state = {
- s_x : float;
- s_y : float;
- theta : float;
-}
-
-let robot = ref { s_x = 0.; s_y = 0.; theta = 0.}
-
-(* +-----------------------------------------------------------------+
- | read/send loop |
- +-----------------------------------------------------------------+ *)
-
-let dist_x x y theta obstacle =
- let y_int = y +. (obstacle -. x) *. (tan theta) in
- if (y_int > 0. && y_int < Krobot_config.world_height) then
- let d = (obstacle -. x) /. (cos theta) in
- if d >= 0. then
- Some d
- else
- None
- else
- None
-
-let dist_y x y theta obstacle =
- let x_int = x +. (obstacle -. y) /. (tan theta) in
- if (x_int > 0. && x_int < Krobot_config.world_width) then
- let d = (obstacle -. y) /. (sin theta) in
- if d >= 0. then
- Some d
- else
- None
- else
- None
-
-let min_border_distance x y theta =
- let l = match dist_x x y theta 0. with Some v -> [v] | None -> [] in
- let l = match dist_x x y theta Krobot_config.world_width with Some v -> v::l | None -> l in
- let l = match dist_y x y theta 0. with Some v -> v::l | None -> l in
- let l = match dist_y x y theta Krobot_config.world_height with Some v -> v::l | None -> l in
- match l with
- | [] -> assert false
- | l -> List.fold_left min max_float l
- | _ -> assert false
-
-let gen_data robot =
- let dim = Array.length Krobot_config.urg_angles in
- let l = ref [] in
- for i = 0 to dim - 1 do
- let angle = Krobot_config.urg_angles.(i) in
- let dist = min_border_distance robot.s_x robot.s_y (robot.theta +. angle) in
- let x = dist *. cos angle in
- let y = dist *. sin angle in
- l := {x;y} :: !l
- done;
- Array.of_list !l
-
-let loop bus =
- let rec aux () =
- let time = Unix.gettimeofday () in
- let msg = Urg (gen_data !robot) in
- lwt () = Krobot_bus.send bus (time, msg) in
- lwt () = Lwt_unix.sleep 0.01 in
- aux () in
- aux ()
-
-(* +-----------------------------------------------------------------+
- | Message handling |
- +-----------------------------------------------------------------+ *)
-
-let handle_message (timestamp, message) =
- match message with
- | Kill "io_simulator" ->
- exit 0
- | CAN(_, frame) -> begin
- match decode frame with
- | Odometry(x, y, theta) ->
- robot := { s_x = x; s_y = y; theta }
- | _ ->
- ()
- end
- | _ ->
- ()
-
-(* +-----------------------------------------------------------------+
- | Command-line arguments |
- +-----------------------------------------------------------------+ *)
-
-let fork = ref true
-
-let options = Arg.align [
- "-no-fork", Arg.Clear fork, " Run in foreground";
-]
-
-let usage = "\
-Usage: krobot-io-simulator [options]
-options are:"
-
-(* +-----------------------------------------------------------------+
- | Entry point |
- +-----------------------------------------------------------------+ *)
-
-lwt () =
- Arg.parse options ignore usage;
-
- (* Display all informative messages. *)
- Lwt_log.append_rule "*" Lwt_log.Info;
-
- (* Open the krobot bus. *)
- lwt bus = Krobot_bus.get () in
-
- (* Fork if not prevented. *)
- if !fork then Krobot_daemon.daemonize bus;
-
- (* Handle krobot message. *)
- E.keep (E.map (handle_message) (Krobot_bus.recv bus));
-
- (* Kill any running io_simulator. *)
- lwt () = Krobot_bus.send bus (Unix.gettimeofday (), Krobot_bus.Kill "io_simulator") in
-
- (* Wait a bit to let the other handler release the connection *)
- lwt () = Lwt_unix.sleep 0.4 in
-
- (* Loop forever. *)
- Lwt_unix.run (loop bus)
diff --git a/info/control2011/src/tools/krobot_mc_simulator.ml b/info/control2011/src/tools/krobot_mc_simulator.ml
deleted file mode 100644
index c307a52..0000000
--- a/info/control2011/src/tools/krobot_mc_simulator.ml
+++ /dev/null
@@ -1,538 +0,0 @@
-(*
- * krobot_simulator.ml
- * -----------------------
- * Copyright : (c) 2013, Xavier Lagorce <Xav...@cr...>
- * Licence : BSD3
- *
- * This file is a part of [kro]bot.
- *)
-
-(* Simulate the robot. *)
-
-open Lwt
-open Lwt_react
-open Krobot_config
-open Krobot_bus
-open Krobot_message
-open Krobot_geom
-
-let section = Lwt_log.Section.make "krobot(simulator)"
-let time_step = 0.001
-
-(* +-----------------------------------------------------------------+
- | Command-line arguments |
- +-----------------------------------------------------------------+ *)
-
-let fork = ref true
-let hil = ref false
-
-let options = Arg.align [
- "-no-fork", Arg.Clear fork, " Run in foreground";
- "-hil", Arg.Set hil, " Run in hardware in the loop mode";
-]
-
-let usage = "\
-Usage: krobot-mc-simulator [options]
-options are:"
-
-(* +-----------------------------------------------------------------+
- | Types |
- +-----------------------------------------------------------------+ *)
-
-(* State of the robot. *)
-type state = {
- x : float;
- y : float;
- theta : float;
-}
-
-type internal_state = {
- theta_l : float;
- theta_r : float;
-}
-
-type command =
- | Idle
- (* The robot is doing nothing. *)
- | Speed of float * float
- (* [Speed(left_velocity, right_velocity)] *)
- | Turn of float * float
- (* [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)] *)
-
-(* Type of simulators. *)
-type simulator = {
- mutable state : state;
- (* The state of the robot. *)
- mutable state_indep : state;
- (* The state of the robot for second set of encoders. *)
- mutable internal_state : internal_state;
- (* The state of the wheels. *)
- mutable velocity_l : float;
- (* Velocity of the left motor. *)
- mutable velocity_r : float;
- (* Velocity of the right motor. *)
- mutable ghost : state;
- (* The state of the ghost. *)
- mutable bezier_u : float;
- (* position on the Bezier's curve*)
- mutable bezier_curve : Bezier.curve option;
- (* Bezier's curve currently being followed if existing *)
- mutable bezier_next : (Bezier.curve * bool) option;
- (* Next Bezier's curve to follow *)
- mutable time : float;
- (* The current time. *)
- mutable command : command;
- (* The current command. *)
- mutable command_start : float;
- (* The start time of the current command. *)
- mutable command_end : float;
- (* The end time of the current command. *)
- mutable bezier_limits : float * float * float;
-}
-
-(* +-----------------------------------------------------------------+
- | Utility functions |
- +-----------------------------------------------------------------+ *)
-
-let print sim =
- Lwt_log.debug_f "
-time = %f
-state:
- x = %f
- y = %f
- theta = %f
-internal_state:
- theta_l = %f
- theta_r = %f
-velocities:
- left = %f
- right = %f
-"
- sim.time
- sim.state.x
- sim.state.y
- sim.state.theta
- sim.internal_state.theta_l
- sim.internal_state.theta_r
- sim.velocity_l
- sim.velocity_r
-
-
-(* +-----------------------------------------------------------------+
- | Trajectory generation |
- +-----------------------------------------------------------------+ *)
-
-let velocities sim dt =
- (* Put the robot into idle if the last command is terminated. *)
- (match sim.command with
- | Bezier_cmd (_,cur_dir) ->
- if sim.bezier_u >= 1. then begin
- match sim.bezier_next with
- | None ->
- sim.command <- Idle
- | Some (curve,dir) ->
- sim.command <- Bezier_cmd (sim.bezier_limits, cur_dir);
- sim.bezier_curve <- Some curve;
- sim.bezier_next <- None;
- sim.bezier_u <- 0.
- end
- | _ -> if sim.time > sim.command_end then sim.command <- Idle);
- match sim.command with
- | Idle ->
- (0., 0.)
- | Speed(l_v, r_v) ->
- ((l_v +. r_v) *. wheels_diameter /. 4., (l_v -. r_v) *. wheels_diameter /. wheels_distance)
- | Turn(t_acc, vel) ->
- if sim.time < (sim.command_start +. t_acc) then
- (0., vel *. (sim.time -. sim.command_start) /. t_acc)
- else if sim.time < (sim.command_end -. t_acc) then
- (0., vel)
- else
- (0., vel *. (sim.command_end -. sim.time) /. t_acc)
- | Move(t_acc, vel) ->
- if sim.time < (sim.command_start +. t_acc) then
- (vel *. (sim.time -. sim.command_start) /. t_acc, 0.)
- else if sim.time < (sim.command_end -. t_acc) then
- (vel, 0.)
- else
- (vel *. (sim.command_end -. sim.time) /. t_acc, 0.)
- | Bezier_cmd (limits, dir) ->
- (match sim.bezier_curve with
- | None ->
- sim.command <- Idle;
- (0., 0.)
- | Some curve ->
- let (v_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
- sim.bezier_u <- sim.bezier_u +. vel /. s' *. dt;
- if dir then
- (vel, theta' *. vel /. s')
- else
- (-.vel, theta' *. vel /. s'))
-
-let bezier sim (x_end, y_end, d1, d2, theta_end, v_end) =
- let p,theta_start = match sim.bezier_curve with
- | None ->
- {Krobot_geom.x = sim.state.x; Krobot_geom.y = sim.state.y},
- sim.state.theta
- | Some curve ->
- let _,_,r,s = Bezier.pqrs curve in
- s,
- (angle (vector r s))
- in
- let s = {Krobot_geom.x = x_end; Krobot_geom.y = y_end} in
- let q = translate p (vector_of_polar d1 theta_start) in
- let r = translate s (vector_of_polar (-.d2) theta_end) in
- let dir = d1 >= 0. in
- match sim.bezier_curve with
- | None ->
- sim.command <- Bezier_cmd (sim.bezier_limits,dir);
- sim.bezier_u <- 0.;
- sim.bezier_curve <- Some (Bezier.of_vertices p q r s);
- | Some _ ->
- sim.bezier_next <- Some ((Bezier.of_vertices p q r s),dir)
-
-let move sim distance velocity acceleration =
- if distance <> 0. && velocity > 0. && acceleration > 0. then begin
- let t_acc = velocity /. acceleration in
- let t_end = (velocity *. velocity +. distance *. acceleration) /. (velocity *. acceleration) in
- if t_end > 2. *. t_acc then begin
- if t_acc <> 0. then begin
- sim.command <- Move(t_acc, velocity);
- sim.command_start <- sim.time;
- sim.command_end <- sim.time +. t_end
- end
- end else begin
- if t_acc <> 0. then begin
- let t_acc = sqrt (abs_float (distance) /. acceleration) in
- let t_end = 2. *. t_acc in
- let sign = if distance >= 0. then 1. else -1. in
- let velocity = sign *. acceleration *. t_acc in
- sim.command <- Move(t_acc, velocity);
- sim.command_start <- sim.time;
- sim.command_end <- sim.time +. t_end
- end
- end
- end
-
-let turn sim angle velocity acceleration =
- if angle <> 0. && velocity > 0. && acceleration > 0. then begin
- let t_acc = velocity /. acceleration in
- let t_end = (velocity *. velocity +. angle *. acceleration) /. (velocity *. acceleration) in
- if t_end > 2. *. t_acc then begin
- if t_acc <> 0. then begin
- sim.command <- Turn(t_acc, velocity);
- sim.command_start <- sim.time;
- sim.command_end <- sim.time +. t_end
- end
- end else begin
- let t_acc = sqrt (abs_float (angle) /. acceleration) in
- let t_end = 2. *. t_acc in
- let sign = if angle >= 0. then 1. else -1. in
- let velocity = sign *. acceleration *. t_acc in
- if t_acc <> 0. then begin
- sim.command <- Turn(t_acc, velocity);
- sim.command_start <- sim.time;
- sim.command_end <- sim.time +. t_end
- end
- end
- end
-
-let set_velocities sim left_velocity right_velocity duration =
- sim.command <- Speed(left_velocity, right_velocity);
- sim.command_start <- sim.time;
- sim.command_end <- sim.time +. duration
-
-let get_velocities sim dt =
- let (u1, u2) = velocities sim dt in
- let l_v = (4. *. u1 +. wheels_distance *. u2) /. (2. *. wheels_diameter)
- and r_v = (4. *. u1 -. wheels_distance *. u2) /. (2. *. wheels_diameter) in
- (l_v, r_v)
-
-let get_state sim =
- sim.state
-
-let get_encoders sim =
- let (theta_l, theta_r) = (sim.internal_state.theta_l, sim.internal_state.theta_r) in
- (theta_l *. wheels_diameter /. 2., theta_r *. wheels_diameter /. 2.)
-
-
-(* +-----------------------------------------------------------------+
- | Main loops |
- +-----------------------------------------------------------------+ *)
-
-let sim = ref None
-
-let loop bus sim =
- let rec aux () =
- let time = Unix.gettimeofday () in
- let delta = time -. sim.time in
- sim.time <- time;
-
- lwt () = print sim in
-
- let u1, u2 = if !hil then
- (sim.velocity_l +. sim.velocity_r) *. wheels_diameter /. 4. ,
- (sim.velocity_l -. sim.velocity_r) *. wheels_diameter /. wheels_distance /. 2.
- else
- velocities sim delta
- in
- let dx = u1 *. (cos sim.state.theta)
- and dy = u1 *. (sin sim.state.theta)
- and dtheta = u2 in
- let theta = sim.state.theta +. dtheta *. delta in
- let theta =
- if theta > pi then
- theta -. 2. *. pi
- else if theta < -.pi then
- theta +. 2. *. pi
- else
- theta
- in
- sim.state <- {
- x = sim.state.x +. dx *. delta;
- y = sim.state.y +. dy *. delta;
- theta = theta;
- };
- sim.internal_state <- {
- theta_l = sim.internal_state.theta_l +. delta *. (u1 *. 4. +. u2 *. wheels_distance) /. (2. *. wheels_diameter);
- theta_r = sim.internal_state.theta_r +. delta *. (u1 *. 4. -. u2 *. wheels_distance) /. (2. *. wheels_diameter);
- };
- (match sim.command with
- | Bezier_cmd _ -> sim.ghost <- sim.state
- | _ -> ());
- lwt () = Lwt_unix.sleep time_step in
- aux () in
- aux ()
-
-let send_CAN_messages sim bus =
- let rec aux () =
- (* Sends the state of the robot. *)
- lwt () = Krobot_message.send bus (sim.time, Odometry(sim.state.x, sim.state.y, sim.state.theta)) in
- lwt () = Krobot_message.send bus (sim.time, Odometry_indep(sim.state.x, sim.state.y, sim.state.theta)) in
- (* Wait before next batch of packets (emulate the electronic board behavior) *)
- lwt () = Lwt_unix.sleep 0.005 in
- (* Sends the state of the ghost. *)
- lwt () = Krobot_message.send bus (sim.time,
- Odometry_ghost(sim.ghost.x,
- sim.ghost.y,
- sim.ghost.theta,
- int_of_float (255. *. sim.bezier_u),
- match sim.command with Bezier_cmd _ -> true | _ -> false)) in
- (* Sends the state of the motors. *)
- lwt () = match sim.command with
- | Turn(a, b) ->
- Krobot_message.send bus (Unix.gettimeofday (), Motor_status(false, true, false, false))
- | Move(a, b) ->
- Krobot_message.send bus (Unix.gettimeofday (), Motor_status(true, false, false, false))
- | Bezier_cmd _ ->
- (* Motor State to be verifyed on the real Motor Controller card *)
- Krobot_message.send bus (Unix.gettimeofday (), Motor_status(true, true, false, false))
- | _ ->
- Krobot_message.send bus (Unix.gettimeofday (), Motor_status(false, false, false, false)) in
- lwt () = Lwt_unix.sleep 0.005 in
- aux () in
- aux ()
-
-(* +-----------------------------------------------------------------+
- | Message handling |
- +-----------------------------------------------------------------+ *)
-
-let handle_message bus (timestamp, message) =
- match message with
- | Kill "mc_simulator" ->
- exit 0
- | CAN(_, frame) -> begin
- match !sim with
- | Some sim -> begin
- (* Generic messages *)
- (match decode frame with
- | Set_odometry(x, y, theta) ->
- sim.state <- { x; y; theta }
- | _ ->
- ());
- (* Messages related to HIL mode *)
- (if !hil then begin
- match decode frame with
- | Encoder_position_speed_3(pos, speed) ->
- sim.velocity_r <- speed
- | Encoder_position_speed_4(pos, speed) ->
- sim.velocity_l <- speed
- | _ ->
- () end
- (* Message related to full software simulation mode *)
- else begin
- match decode frame with
- | Motor_move(dist, speed, acc) ->
- Lwt_unix.run (Lwt_log.info_f "received: move(%f, %f, %f)" dist speed acc);
- move sim dist speed acc
- | Motor_turn(angle, speed, acc) ->
- Lwt_unix.run (Lwt_log.info_f "received: turn(%f, %f, %f)" angle speed acc);
- turn sim angle speed acc
- | Motor_stop(lin_acc, rot_acc) ->
- sim.command <- Idle;
- sim.bezier_curve <- None;
- sim.bezier_next <- None;
- | Set_odometry(x, y, theta) ->
- 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(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)
- | _ ->
- () end);
- Lwt.return () end
- | None ->
- Lwt.return ()
- end
- | _ -> Lwt.return ()
-
-(* +-----------------------------------------------------------------+
- | Hokuyo emulation |
- +-----------------------------------------------------------------+ *)
-
-let dist_x x y theta obstacle =
- let y_int = y +. (obstacle -. x) *. (tan theta) in
- if (y_int > 0. && y_int < Krobot_config.world_height) then
- let d = (obstacle -. x) /. (cos theta) in
- if d >= 0. then
- Some d
- else
- None
- else
- None
-
-let dist_y x y theta obstacle =
- let x_int = x +. (obstacle -. y) /. (tan theta) in
- if (x_int > 0. && x_int < Krobot_config.world_width) then
- let d = (obstacle -. y) /. (sin theta) in
- if d >= 0. then
- Some d
- else
- None
- else
- None
-
-let min_border_distance x y theta =
- let l = match dist_x x y theta 0. with Some v -> [v] | None -> [] in
- let l = match dist_x x y theta Krobot_config.world_width with Some v -> v::l | None -> l in
- let l = match dist_y x y theta 0. with Some v -> v::l | None -> l in
- let l = match dist_y x y theta Krobot_config.world_height with Some v -> v::l | None -> l in
- match l with
- | [] -> None
- | l -> Some (List.fold_left min max_float l)
-
-(*let gen_data robot =
- let dim = Array.length Krobot_config.urg_angles in
- let {Krobot_geom.x=urg_rel_x;
- Krobot_geom.y=urg_rel_y } = Krobot_config.urg_position in
- let urg_pos = [| urg_rel_x; urg_rel_y; 1. |] in
- let rot = rot_mat robot.theta in
- let urg_pos = mult rot urg_pos in
- let pos = Krobot_geom.translate
- { Krobot_geom.x=robot.x; Krobot_geom.y=robot.y }
- { Krobot_geom.vx = urg_pos.(0); Krobot_geom.vy = urg_pos.(1) } in
- let {Krobot_geom.x=cen_x;
- Krobot_geom.y=cen_y } = pos in
- let l = ref [] in
- for i = 0 to dim - 1 do
- let angle = Krobot_config.urg_angles.(i) in
- match min_border_distance cen_x cen_y (robot.theta +. angle) with
- | Some dist ->
- let x = dist *. cos angle +. urg_pos.(0) in
- let y = dist *. sin angle +. urg_pos.(1) in
- l := {Krobot_geom.x;Krobot_geom.y} :: !l
- | None ->
- ()
- done;
- Array.of_list !l*)
-
-let gen_data robot =
- let dim = Array.length Krobot_config.urg_angles in
- let l = ref [] in
- for i = 0 to dim - 1 do
- let angle = Krobot_config.urg_angles.(i) in
- match min_border_distance robot.x robot.y (robot.theta +. angle) with
- | Some dist ->
- let x = dist *. cos angle in
- let y = dist *. sin angle in
- l := {Krobot_geom.x=x;Krobot_geom.y=y} :: !l
- | None ->
- ()
- done;
- Array.of_list !l
-
-let loop_urg sim bus =
- let rec aux () =
- let time = Unix.gettimeofday () in
- let msg = Urg (gen_data sim.state) in
- lwt () = Krobot_bus.send bus (time, msg) in
- lwt () = Lwt_unix.sleep 0.01 in
- aux () in
- aux ()
-
-(* +-----------------------------------------------------------------+
- | Entry point |
- +-----------------------------------------------------------------+ *)
-
-lwt () =
- Arg.parse options ignore usage;
-
- (* Display all informative messages. *)
- Lwt_log.append_rule "*" Lwt_log.Info;
-
- (* Open the krobot bus. *)
- lwt bus = Krobot_bus.get () in
-
- (* Fork if not prevented. *)
- if !fork then Krobot_daemon.daemonize bus;
-
- (* Handle krobot message. *)
- E.keep (E.map (handle_message bus) (Krobot_bus.recv bus));
-
- (* Kill any running mc_simulator. *)
- lwt () = Krobot_bus.send bus (Unix.gettimeofday (), Krobot_bus.Kill "mc_simulator") in
-
- (* Wait a bit to let the other handler release the connection *)
- lwt () = Lwt_unix.sleep 0.4 in
-
- (* Set the motor_controller card in HIL mode if necessary *)
- if !hil then
- ignore (Krobot_message.send bus (Unix.gettimeofday (), Set_controller_mode true)) ;
-
- (* Initial state of the simulator *)
- let local_sim = {
- state = { x = 0.; y = 0.; theta = 0. };
- state_indep = { x = 0.; y = 0.; theta = 0. };
- internal_state = { theta_l = 0.; theta_r = 0. };
- velocity_l = 0.;
- velocity_r = 0.;
- ghost = { x = 0.; y = 0.; theta = 0. };
- bezier_u = 0.;
- bezier_curve = None;
- bezier_next = None;
- command = Idle;
- command_start = 0.;
- command_end = 0.;
- time = Unix.gettimeofday ();
- bezier_limits = 1., 1., 1.;
- } in
- sim := Some local_sim;
-
- ignore(send_CAN_messages local_sim bus);
- ignore(loop_urg local_sim bus);
-
- (* Loop forever. *)
- Lwt_unix.run (loop bus local_sim)
diff --git a/info/control2011/src/tools/krobot_simulator.ml b/info/control2011/src/tools/krobot_simulator.ml
index f40885e..c307a52 100644
--- a/info/control2011/src/tools/krobot_simulator.ml
+++ b/info/control2011/src/tools/krobot_simulator.ml
@@ -1,7 +1,7 @@
(*
* krobot_simulator.ml
- * -------------------
- * Copyright : (c) 2011, Jeremie Dimino <je...@di...>
+ * -----------------------
+ * Copyright : (c) 2013, Xavier Lagorce <Xav...@cr...>
* Licence : BSD3
*
* This file is a part of [kro]bot.
@@ -12,9 +12,29 @@
open Lwt
open Lwt_react
open Krobot_config
+open Krobot_bus
open Krobot_message
open Krobot_geom
+let section = Lwt_log.Section.make "krobot(simulator)"
+let time_step = 0.001
+
+(* +-----------------------------------------------------------------+
+ | Command-line arguments |
+ +-----------------------------------------------------------------+ *)
+
+let fork = ref true
+let hil = ref false
+
+let options = Arg.align [
+ "-no-fork", Arg.Clear fork, " Run in foreground";
+ "-hil", Arg.Set hil, " Run in hardware in the loop mode";
+]
+
+let usage = "\
+Usage: krobot-mc-simulator [options]
+options are:"
+
(* +-----------------------------------------------------------------+
| Types |
+-----------------------------------------------------------------+ *)
@@ -33,72 +53,153 @@ type internal_state = {
type command =
| Idle
- (* The robot is doint nothing. *)
+ (* The robot is doing nothing. *)
| Speed of float * float
(* [Speed(left_velocity, right_velocity)] *)
| Turn of float * float
(* [Turn(t_acc, velocity)] *)
| Move of float * float
(* [Move(t_acc, velocity)] *)
- | Bezier of
- ( float * float * float * float * float * float ) *
- ( float * float * float )
- (** [Motor_bezier(x_end, y_end, d1, d2, theta_end, v_end)]
- [Motor_bezier_limits(v_max, a_tan_max, a_rad_max)] *)
+ | Bezier_cmd of (float * float * float) * bool
+ (** [Motor_bezier_limits(v_max, a_tan_max, a_rad_max)] *)
(* Type of simulators. *)
type simulator = {
mutable state : state;
(* The state of the robot. *)
+ mutable state_indep : state;
+ (* The state of the robot for second set of encoders. *)
mutable internal_state : internal_state;
(* The state of the wheels. *)
+ mutable velocity_l : float;
+ (* Velocity of the left motor. *)
+ mutable velocity_r : float;
+ (* Velocity of the right motor. *)
+ mutable ghost : state;
+ (* The state of the ghost. *)
+ mutable bezier_u : float;
+ (* position on the Bezier's curve*)
+ mutable bezier_curve : Bezier.curve option;
+ (* Bezier's curve currently being followed if existing *)
+ mutable bezier_next : (Bezier.curve * bool) option;
+ (* Next Bezier's curve to follow *)
+ mutable time : float;
+ (* The current time. *)
mutable command : command;
(* The current command. *)
mutable command_start : float;
(* The start time of the current command. *)
mutable command_end : float;
(* The end time of the current command. *)
- mutable time : float;
- (* The current time. *)
mutable bezier_limits : float * float * float;
}
(* +-----------------------------------------------------------------+
- | Simulation |
+ | Utility functions |
+-----------------------------------------------------------------+ *)
-let sim_step = 1e-3
+let print sim =
+ Lwt_log.debug_f "
+time = %f
+state:
+ x = %f
+ y = %f
+ theta = %f
+internal_state:
+ theta_l = %f
+ theta_r = %f
+velocities:
+ left = %f
+ right = %f
+"
+ sim.time
+ sim.state.x
+ sim.state.y
+ sim.state.theta
+ sim.internal_state.theta_l
+ sim.internal_state.theta_r
+ sim.velocity_l
+ sim.velocity_r
+
-let velocities sim =
+(* +-----------------------------------------------------------------+
+ | Trajectory generation |
+ +-----------------------------------------------------------------+ *)
+
+let velocities sim dt =
+ (* Put the robot into idle if the last command is terminated. *)
+ (match sim.command with
+ | Bezier_cmd (_,cur_dir) ->
+ if sim.bezier_u >= 1. then begin
+ match sim.bezier_next with
+ | None ->
+ sim.command <- Idle
+ | Some (curve,dir) ->
+ sim.command <- Bezier_cmd (sim.bezier_limits, cur_dir);
+ sim.bezier_curve <- Some curve;
+ sim.bezier_next <- None;
+ sim.bezier_u <- 0.
+ end
+ | _ -> if sim.time > sim.command_end then sim.command <- Idle);
match sim.command with
| Idle ->
- (0., 0.)
+ (0., 0.)
| Speed(l_v, r_v) ->
- ((l_v +. r_v) *. wheels_diameter /. 4., (l_v -. r_v) *. wheels_diameter /. wheels_distance)
+ ((l_v +. r_v) *. wheels_diameter /. 4., (l_v -. r_v) *. wheels_diameter /. wheels_distance)
| Turn(t_acc, vel) ->
- if sim.time < (sim.command_start +. t_acc) then
- (0., vel *. (sim.time -. sim.command_start) /. t_acc)
- else if sim.time < (sim.command_end -. t_acc) then
- (0., vel)
- else
- (0., vel *. (sim.command_end -. sim.time) /. t_acc)
+ if sim.time < (sim.command_start +. t_acc) then
+ (0., vel *. (sim.time -. sim.command_start) /. t_acc)
+ else if sim.time < (sim.command_end -. t_acc) then
+ (0., vel)
+ else
+ (0., vel *. (sim.command_end -. sim.time) /. t_acc)
| Move(t_acc, vel) ->
- if sim.time < (sim.command_start +. t_acc) then
- (vel *. (sim.time -. sim.command_start) /. t_acc, 0.)
- else if sim.time < (sim.command_end -. t_acc) then
- (vel, 0.)
- else
- (vel *. (sim.command_end -. sim.time) /. t_acc, 0.)
- | Bezier(_,_) -> failwith "todo"
+ if sim.time < (sim.command_start +. t_acc) then
+ (vel *. (sim.time -. sim.command_start) /. t_acc, 0.)
+ else if sim.time < (sim.command_end -. t_acc) then
+ (vel, 0.)
+ else
+ (vel *. (sim.command_end -. sim.time) /. t_acc, 0.)
+ | Bezier_cmd (limits, dir) ->
+ (match sim.bezier_curve with
+ | None ->
+ sim.command <- Idle;
+ (0., 0.)
+ | Some curve ->
+ let (v_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
+ sim.bezier_u <- sim.bezier_u +. vel /. s' *. dt;
+ if dir then
+ (vel, theta' *. vel /. s')
+ else
+ (-.vel, theta' *. vel /. s'))
let bezier sim (x_end, y_end, d1, d2, theta_end, v_end) =
- assert false
-(*
- sim.command <- Bezier((x_end, y_end, d1, d2, theta_end, v_end),
- sim.bezier_limits);
- sim.command_start <- sim.time;
- sim.command_end <- sim.time +. t_end
-*)
+ let p,theta_start = match sim.bezier_curve with
+ | None ->
+ {Krobot_geom.x = sim.state.x; Krobot_geom.y = sim.state.y},
+ sim.state.theta
+ | Some curve ->
+ let _,_,r,s = Bezier.pqrs curve in
+ s,
+ (angle (vector r s))
+ in
+ let s = {Krobot_geom.x = x_end; Krobot_geom.y = y_end} in
+ let q = translate p (vector_of_polar d1 theta_start) in
+ let r = translate s (vector_of_polar (-.d2) theta_end) in
+ let dir = d1 >= 0. in
+ match sim.bezier_curve with
+ | None ->
+ sim.command <- Bezier_cmd (sim.bezier_limits,dir);
+ sim.bezier_u <- 0.;
+ sim.bezier_curve <- Some (Bezier.of_vertices p q r s);
+ | Some _ ->
+ sim.bezier_next <- Some ((Bezier.of_vertices p q r s),dir)
let move sim distance velocity acceleration =
if distance <> 0. && velocity > 0. && acceleration > 0. then begin
@@ -151,8 +252,8 @@ let set_velocities sim left_velocity right_velocity duration =
sim.command_start <- sim.time;
sim.command_end <- sim.time +. duration
-let get_velocities sim =
- let (u1, u2) = velocities sim in
+let get_velocities sim dt =
+ let (u1, u2) = velocities sim dt in
let l_v = (4. *. u1 +. wheels_distance *. u2) /. (2. *. wheels_diameter)
and r_v = (4. *. u1 -. wheels_distance *. u2) /. (2. *. wheels_diameter) in
(l_v, r_v)
@@ -164,138 +265,274 @@ let get_encoders sim =
let (theta_l, theta_r) = (sim.internal_state.theta_l, sim.internal_state.theta_r) in
(theta_l *. wheels_diameter /. 2., theta_r *. wheels_diameter /. 2.)
+
(* +-----------------------------------------------------------------+
- | Entry point |
+ | Main loops |
+-----------------------------------------------------------------+ *)
-let print sim =
- Lwt_log.debug_f "
-time = %f
-state:
- x = %f
- y = %f
- theta = %f
-internal_state:
- theta_l = %f
- theta_r = %f
-command:
- start = %f
- end = %f
- kind = %s
-"
- sim.time
- sim.state.x
- sim.state.y
- sim.state.theta
- sim.internal_state.theta_l
- sim.internal_state.theta_r
- sim.command_start
- sim.command_end
- (match sim.command with
- | Idle ->
- "Idle"
- | Speed(l, r) ->
- Printf.sprintf "Speed(%f, %f)" l r
- | Move(t, a) ->
- Printf.sprintf "Move(%f, %f)" t a
- | Turn(t, a) ->
- Printf.sprintf "Turn(%f, %f)" t a
- | Bezier((x_end, y_end, d1, d2, theta_end, v_end), (v_max, a_tan_max, a_rad_max)) ->
- Printf.sprintf "Bezier((%f, %f, %f, %f, %f, %f),(%f, %f, %f))"
- x_end y_end d1 d2 theta_end v_end v_max a_tan_max a_rad_max)
-
-lwt () =
- lwt bus = Krobot_bus.get () in
+let sim = ref None
- let sim = {
- state = { x = 0.; y = 0.; theta = 0. };
- internal_state = { theta_l = 0.; theta_r = 0. };
- command = Idle;
- command_start = 0.;
- command_end = 0.;
- time = Unix.gettimeofday ();
- bezier_limits = 1., 1., 1.;
- } in
-
- (* Handle commands. *)
- E.keep
- (E.map_s
- (fun (ts, msg) ->
- match msg with
- | Motor_move(dist, speed, acc) ->
- lwt () = Lwt_log.info_f "received: move(%f, %f, %f)" dist speed acc in
- move sim dist speed acc;
- return ()
- | Motor_turn(angle, speed, acc) ->
- lwt () = Lwt_log.info_f "received: turn(%f, %f, %f)" angle speed acc in
- turn sim angle speed acc;
- return ()
- | Motor_stop(lin_acc, rot_acc) ->
- sim.command <- Idle;
- return ()
- | Req_motor_status ->
- begin
- match sim.command with
- | Turn(a, b) ->
- Krobot_message.send bus (Unix.gettimeofday (), Motor_status(false, true, false, false))
- | Move(a, b) ->
- Krobot_message.send bus (Unix.gettimeofday (), Motor_status(true, false, false, false))
- | _ ->
- Krobot_message.send bus (Unix.gettimeofday (), Motor_status(false, false, false, false))
- end
- | Set_odometry(x, y, theta) ->
- sim.state <- { x; y; theta };
- return ()
- | Set_odometry_indep(x, y, theta) ->
- sim.state <- { x; y; theta };
- return ()
- | Motor_bezier_limits(v_max, a_tan_max, a_rad_max) ->
- sim.bezier_limits <- (v_max, a_tan_max, a_rad_max);
- return ()
- | Motor_bezier(x_end, y_end, d1, d2, theta_end, v_end) ->
- bezier sim (x_end, y_end, d1, d2, theta_end, v_end)
- | _ ->
- return ())
- (Krobot_message.recv bus));
-
- while_lwt true do
+let loop bus sim =
+ let rec aux () =
let time = Unix.gettimeofday () in
let delta = time -. sim.time in
sim.time <- time;
- (* Put the robot into idle if the last command is terminated. *)
- if sim.time > sim.command_end then sim.command <- Idle;
-
- (* Sends the state of the robot. *)
- lwt () = Krobot_message.send bus (sim.time, Odometry(sim.state.x, sim.state.y, sim.state.theta)) in
- lwt () = Krobot_message.send bus (sim.time, Odometry_indep(sim.state.x, sim.state.y, sim.state.theta)) in
-
- (* Sends the state of the motors. *)
- lwt () =
- match sim.command with
- | Turn(a, b) ->
- Krobot_message.send bus (Unix.gettimeofday (), Motor_status(false, true, false, false))
- | Move(a, b) ->
- Krobot_message.send bus (Unix.gettimeofday (), Motor_status(true, false, false, false))
- | _ ->
- Krobot_message.send bus (Unix.gettimeofday (), Motor_status(false, false, false, false))
- in
-
lwt () = print sim in
- let (u1, u2) = velocities sim in
+ let u1, u2 = if !hil then
+ (sim.velocity_l +. sim.velocity_r) *. wheels_diameter /. 4. ,
+ (sim.velocity_l -. sim.velocity_r) *. wheels_diameter /. wheels_distance /. 2.
+ else
+ velocities sim delta
+ in
let dx = u1 *. (cos sim.state.theta)
and dy = u1 *. (sin sim.state.theta)
and dtheta = u2 in
+ let theta = sim.state.theta +. dtheta *. delta in
+ let theta =
+ if theta > pi then
+ theta -. 2. *. pi
+ else if theta < -.pi then
+ theta +. 2. *. pi
+ else
+ theta
+ in
sim.state <- {
x = sim.state.x +. dx *. delta;
y = sim.state.y +. dy *. delta;
- theta = math_mod_float (sim.state.theta +. dtheta *. delta) (2. *. pi);
+ theta = theta;
};
sim.internal_state <- {
theta_l = sim.internal_state.theta_l +. delta *. (u1 *. 4. +. u2 *. wheels_distance) /. (2. *. wheels_diameter);
theta_r = sim.internal_state.theta_r +. delta *. (u1 *. 4. -. u2 *. wheels_distance) /. (2. *. wheels_diameter);
};
+ (match sim.command with
+ | Bezier_cmd _ -> sim.ghost <- sim.state
+ | _ -> ());
+ lwt () = Lwt_unix.sleep time_step in
+ aux () in
+ aux ()
+
+let send_CAN_messages sim bus =
+ let rec aux () =
+ (* Sends the state of the robot. *)
+ lwt () = Krobot_message.send bus (sim.time, Odometry(sim.state.x, sim.state.y, sim.state.theta)) in
+ lwt () = Krobot_message.send bus (sim.time, Odometry_indep(sim.state.x, sim.state.y, sim.state.theta)) in
+ (* Wait before next batch of packets (emulate the electronic board behavior) *)
+ lwt () = Lwt_unix.sleep 0.005 in
+ (* Sends the state of the ghost. *)
+ lwt () = Krobot_message.send bus (sim.time,
+ Odometry_ghost(sim.ghost.x,
+ sim.ghost.y,
+ sim.ghost.theta,
+ int_of_float (255. *. sim.bezier_u),
+ match sim.command with Bezier_cmd _ -> true | _ -> false)) in
+ (* Sends the state of the motors. *)
+ lwt () = match sim.command with
+ | Turn(a, b) ->
+ Krobot_message.send bus (Unix.gettimeofday (), Motor_status(false, true, false, false))
+ | Move(a, b) ->
+ Krobot_message.send bus (Unix.gettimeofday (), Motor_status(true, false, false, false))
+ | Bezier_cmd _ ->
+ (* Motor State to be verifyed on the real Motor Controller card *)
+ Krobot_message.send bus (Unix.gettimeofday (), Motor_status(true, true, false, false))
+ | _ ->
+ Krobot_message.send bus (Unix.gettimeofday (), Motor_status(false, false, false, false)) in
+ lwt () = Lwt_unix.sleep 0.005 in
+ aux () in
+ aux ()
+
+(* +-----------------------------------------------------------------+
+ | Message handling |
+ +-----------------------------------------------------------------+ *)
+
+let handle_message bus (timestamp, message) =
+ match message with
+ | Kill "mc_simulator" ->
+ exit 0
+ | CAN(_, frame) -> begin
+ match !sim with
+ | Some sim -> begin
+ (* Generic messages *)
+ (match decode frame with
+ | Set_odometry(x, y, theta) ->
+ sim.state <- { x; y; theta }
+ | _ ->
+ ());
+ (* Messages related to HIL mode *)
+ (if !hil then begin
+ match decode frame with
+ | Encoder_position_speed_3(pos, speed) ->
+ sim.velocity_r <- speed
+ | Encoder_position_speed_4(pos, speed) ->
+ sim.velocity_l <- speed
+ | _ ->
+ () end
+ (* Message related to full software simulation mode *)
+ else begin
+ match decode frame with
+ | Motor_move(dist, speed, acc) ->
+ Lwt_unix.run (Lwt_log.info_f "received: move(%f, %f, %f)" dist speed acc);
+ move sim dist speed acc
+ | Motor_turn(angle, speed, acc) ->
+ Lwt_unix.run (Lwt_log.info_f "received: turn(%f, %f, %f)" angle speed acc);
+ turn sim angle speed acc
+ | Motor_stop(lin_acc, rot_acc) ->
+ sim.command <- Idle;
+ sim.bezier_curve <- None;
+ sim.bezier_next <- None;
+ | Set_odometry(x, y, theta) ->
+ 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(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)
+ | _ ->
+ () end);
+ Lwt.return () end
+ | None ->
+ Lwt.return ()
+ end
+ | _ -> Lwt.return ()
+
+(* +-----------------------------------------------------------------+
+ | Hokuyo emulation |
+ +-----------------------------------------------------------------+ *)
+
+let dist_x x y theta obstacle =
+ let y_int = y +. (obstacle -. x) *. (tan theta) in
+ if (y_int > 0. && y_int < Krobot_config.world_height) then
+ let d = (obstacle -. x) /. (cos theta) in
+ if d >= 0. then
+ Some d
+ else
+ None
+ else
+ None
+
+let dist_y x y theta obstacle =
+ let x_int = x +. (obstacle -. y) /. (tan theta) in
+ if (x_int > 0. && x_int < Krobot_config.world_width) then
+ let d = (obstacle -. y) /. (sin theta) in
+ if d >= 0. then
+ Some d
+ else
+ None
+ else
+ None
+
+let min_border_distance x y theta =
+ let l = match dist_x x y theta 0. with Some v -> [v] | None -> [] in
+ let l = match dist_x x y theta Krobot_config.world_width with Some v -> v::l | None -> l in
+ let l = match dist_y x y theta 0. with Some v -> v::l | None -> l in
+ let l = match dist_y x y theta Krobot_config.world_height with Some v -> v::l | None -> l in
+ match l with
+ | [] -> None
+ | l -> Some (List.fold_left min max_float l)
+
+(*let gen_data robot =
+ let dim = Array.length Krobot_config.urg_angles in
+ let {Krobot_geom.x=urg_rel_x;
+ Krobot_geom.y=urg_rel_y } = Krobot_config.urg_position in
+ let urg_pos = [| urg_rel_x; urg_rel_y; 1. |] in
+ let rot = rot_mat robot.theta in
+ let urg_pos = mult rot urg_pos in
+ let pos = Krobot_geom.translate
+ { Krobot_geom.x=robot.x; Krobot_geom.y=robot.y }
+ { Krobot_geom.vx = urg_pos.(0); Krobot_geom.vy = urg_pos.(1) } in
+ let {Krobot_geom.x=cen_x;
+ Krobot_geom.y=cen_y } = pos in
+ let l = ref [] in
+ for i = 0 to dim - 1 do
+ let angle = Krobot_config.urg_angles.(i) in
+ match min_border_distance cen_x cen_y (robot.theta +. angle) with
+ | Some dist ->
+ let x = dist *. cos angle +. urg_pos.(0) in
+ let y = dist *. sin angle +. urg_pos.(1) in
+ l := {Krobot_geom.x;Krobot_geom.y} :: !l
+ | None ->
+ ()
+ done;
+ Array.of_list !l*)
+
+let gen_data robot =
+ let dim = Array.length Krobot_config.urg_angles in
+ let l = ref [] in
+ for i = 0 to dim - 1 do
+ let angle = Krobot_config.urg_angles.(i) in
+ match min_border_distance robot.x robot.y (robot.theta +. angle) with
+ | Some dist ->
+ let x = dist *. cos angle in
+ let y = dist *. sin angle in
+ l := {Krobot_geom.x=x;Krobot_geom.y=y} :: !l
+ | None ->
+ ()
+ done;
+ Array.of_list !l
+
+let loop_urg sim bus =
+ let rec aux () =
+ let time = Unix.gettimeofday () in
+ let msg = Urg (gen_data sim.state) in
+ lwt () = Krobot_bus.send bus (time, msg) in
+ lwt () = Lwt_unix.sleep 0.01 in
+ aux () in
+ aux ()
+
+(* +-----------------------------------------------------------------+
+ | Entry point |
+ +-----------------------------------------------------------------+ *)
+
+lwt () =
+ Arg.parse options ignore usage;
+
+ (* Display all informative messages. *)
+ Lwt_log.append_rule "*" Lwt_log.Info;
+
+ (* Open the krobot bus. *)
+ lwt bus = Krobot_bus.get () in
+
+ (* Fork if not prevented. *)
+ if !fork then Krobot_daemon.daemonize bus;
+
+ (* Handle krobot message. *)
+ E.keep (E.map (handle_message bus) (Krobot_bus.recv bus));
+
+ (* Kill any running mc_simulator. *)
+ lwt () = Krobot_bus.send bus (Unix.gettimeofday (), Krobot_bus.Kill "mc_simulator") in
+
+ (* Wait a bit to let the other handler release the connection *)
+ lwt () = Lwt_unix.sleep 0.4 in
+
+ (* Set the motor_controller card in HIL mode if necessary *)
+ if !hil then
+ ignore (Krobot_message.send bus (Unix.gettimeofday (), Set_controller_mode true)) ;
+
+ (* Initial state of the simulator *)
+ let local_sim = {
+ state = { x = 0.; y = 0.; theta = 0. };
+ state_indep = { x = 0.; y = 0.; theta = 0. };
+ internal_state = { theta_l = 0.; theta_r = 0. };
+ velocity_l = 0.;
+ velocity_r = 0.;
+ ghost = { x = 0.; y = 0.; theta = 0. };
+ bezier_u = 0.;
+ bezier_curve = None;
+ bezier_next = None;
+ command = Idle;
+ command_start = 0.;
+ command_end = 0.;
+ time = Unix.gettimeofday ();
+ ...
[truncated message content] |
|
From: Xavier L. <Ba...@us...> - 2013-04-17 23:34:51
|
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 e7d6c3175ae17c43e72df1fc2e7b56c1b9bfdff9 (commit)
from db4524176b7945fb99beaf8d3f7c306d1bd461cf (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 e7d6c3175ae17c43e72df1fc2e7b56c1b9bfdff9
Author: Xavier Lagorce <Xav...@cr...>
Date: Thu Apr 18 01:34:41 2013 +0200
[info/simulation] More Urg emulation
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/src/tools/krobot_io_simulator.ml b/info/control2011/src/tools/krobot_io_simulator.ml
index 9c37c01..dbaf6e6 100644
--- a/info/control2011/src/tools/krobot_io_simulator.ml
+++ b/info/control2011/src/tools/krobot_io_simulator.ml
@@ -35,13 +35,46 @@ let robot = ref { s_x = 0.; s_y = 0.; theta = 0.}
| read/send loop |
+-----------------------------------------------------------------+ *)
+let dist_x x y theta obstacle =
+ let y_int = y +. (obstacle -. x) *. (tan theta) in
+ if (y_int > 0. && y_int < Krobot_config.world_height) then
+ let d = (obstacle -. x) /. (cos theta) in
+ if d >= 0. then
+ Some d
+ else
+ None
+ else
+ None
+
+let dist_y x y theta obstacle =
+ let x_int = x +. (obstacle -. y) /. (tan theta) in
+ if (x_int > 0. && x_int < Krobot_config.world_width) then
+ let d = (obstacle -. y) /. (sin theta) in
+ if d >= 0. then
+ Some d
+ else
+ None
+ else
+ None
+
+let min_border_distance x y theta =
+ let l = match dist_x x y theta 0. with Some v -> [v] | None -> [] in
+ let l = match dist_x x y theta Krobot_config.world_width with Some v -> v::l | None -> l in
+ let l = match dist_y x y theta 0. with Some v -> v::l | None -> l in
+ let l = match dist_y x y theta Krobot_config.world_height with Some v -> v::l | None -> l in
+ match l with
+ | [] -> assert false
+ | l -> List.fold_left min max_float l
+ | _ -> assert false
+
let gen_data robot =
let dim = Array.length Krobot_config.urg_angles in
let l = ref [] in
for i = 0 to dim - 1 do
let angle = Krobot_config.urg_angles.(i) in
- let x = 1. *. cos angle in
- let y = 1. *. sin angle in
+ let dist = min_border_distance robot.s_x robot.s_y (robot.theta +. angle) in
+ let x = dist *. cos angle in
+ let y = dist *. sin angle in
l := {x;y} :: !l
done;
Array.of_list !l
@@ -49,7 +82,7 @@ let gen_data robot =
let loop bus =
let rec aux () =
let time = Unix.gettimeofday () in
- let msg = Urg (gen_data robot) in
+ let msg = Urg (gen_data !robot) in
lwt () = Krobot_bus.send bus (time, msg) in
lwt () = Lwt_unix.sleep 0.01 in
aux () in
diff --git a/info/control2011/src/tools/krobot_mc_simulator.ml b/info/control2011/src/tools/krobot_mc_simulator.ml
index 307551c..c307a52 100644
--- a/info/control2011/src/tools/krobot_mc_simulator.ml
+++ b/info/control2011/src/tools/krobot_mc_simulator.ml
@@ -1,5 +1,5 @@
(*
- * krobot_motor_controller_simulator.ml
+ * krobot_simulator.ml
* -----------------------
* Copyright : (c) 2013, Xavier Lagorce <Xav...@cr...>
* Licence : BSD3
@@ -16,7 +16,7 @@ open Krobot_bus
open Krobot_message
open Krobot_geom
-let section = Lwt_log.Section.make "krobot(mc_simulator)"
+let section = Lwt_log.Section.make "krobot(simulator)"
let time_step = 0.001
(* +-----------------------------------------------------------------+
@@ -400,6 +400,90 @@ let handle_message bus (timestamp, message) =
| _ -> Lwt.return ()
(* +-----------------------------------------------------------------+
+ | Hokuyo emulation |
+ +-----------------------------------------------------------------+ *)
+
+let dist_x x y theta obstacle =
+ let y_int = y +. (obstacle -. x) *. (tan theta) in
+ if (y_int > 0. && y_int < Krobot_config.world_height) then
+ let d = (obstacle -. x) /. (cos theta) in
+ if d >= 0. then
+ Some d
+ else
+ None
+ else
+ None
+
+let dist_y x y theta obstacle =
+ let x_int = x +. (obstacle -. y) /. (tan theta) in
+ if (x_int > 0. && x_int < Krobot_config.world_width) then
+ let d = (obstacle -. y) /. (sin theta) in
+ if d >= 0. then
+ Some d
+ else
+ None
+ else
+ None
+
+let min_border_distance x y theta =
+ let l = match dist_x x y theta 0. with Some v -> [v] | None -> [] in
+ let l = match dist_x x y theta Krobot_config.world_width with Some v -> v::l | None -> l in
+ let l = match dist_y x y theta 0. with Some v -> v::l | None -> l in
+ let l = match dist_y x y theta Krobot_config.world_height with Some v -> v::l | None -> l in
+ match l with
+ | [] -> None
+ | l -> Some (List.fold_left min max_float l)
+
+(*let gen_data robot =
+ let dim = Array.length Krobot_config.urg_angles in
+ let {Krobot_geom.x=urg_rel_x;
+ Krobot_geom.y=urg_rel_y } = Krobot_config.urg_position in
+ let urg_pos = [| urg_rel_x; urg_rel_y; 1. |] in
+ let rot = rot_mat robot.theta in
+ let urg_pos = mult rot urg_pos in
+ let pos = Krobot_geom.translate
+ { Krobot_geom.x=robot.x; Krobot_geom.y=robot.y }
+ { Krobot_geom.vx = urg_pos.(0); Krobot_geom.vy = urg_pos.(1) } in
+ let {Krobot_geom.x=cen_x;
+ Krobot_geom.y=cen_y } = pos in
+ let l = ref [] in
+ for i = 0 to dim - 1 do
+ let angle = Krobot_config.urg_angles.(i) in
+ match min_border_distance cen_x cen_y (robot.theta +. angle) with
+ | Some dist ->
+ let x = dist *. cos angle +. urg_pos.(0) in
+ let y = dist *. sin angle +. urg_pos.(1) in
+ l := {Krobot_geom.x;Krobot_geom.y} :: !l
+ | None ->
+ ()
+ done;
+ Array.of_list !l*)
+
+let gen_data robot =
+ let dim = Array.length Krobot_config.urg_angles in
+ let l = ref [] in
+ for i = 0 to dim - 1 do
+ let angle = Krobot_config.urg_angles.(i) in
+ match min_border_distance robot.x robot.y (robot.theta +. angle) with
+ | Some dist ->
+ let x = dist *. cos angle in
+ let y = dist *. sin angle in
+ l := {Krobot_geom.x=x;Krobot_geom.y=y} :: !l
+ | None ->
+ ()
+ done;
+ Array.of_list !l
+
+let loop_urg sim bus =
+ let rec aux () =
+ let time = Unix.gettimeofday () in
+ let msg = Urg (gen_data sim.state) in
+ lwt () = Krobot_bus.send bus (time, msg) in
+ lwt () = Lwt_unix.sleep 0.01 in
+ aux () in
+ aux ()
+
+(* +-----------------------------------------------------------------+
| Entry point |
+-----------------------------------------------------------------+ *)
@@ -448,6 +532,7 @@ lwt () =
sim := Some local_sim;
ignore(send_CAN_messages local_sim bus);
+ ignore(loop_urg local_sim bus);
(* Loop forever. *)
Lwt_unix.run (loop bus local_sim)
hooks/post-receive
--
krobot
|
|
From: Xavier L. <Ba...@us...> - 2013-04-17 16:05:56
|
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 db4524176b7945fb99beaf8d3f7c306d1bd461cf (commit)
from 155f88832102bf2e0dccf85c195cfefa13887571 (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 db4524176b7945fb99beaf8d3f7c306d1bd461cf
Author: Xavier Lagorce <Xav...@cr...>
Date: Wed Apr 17 18:06:38 2013 +0200
[info/krobot-io-simulator] First minimal versio of the input/output simulator
(with a skeleton for Urg data simulation)
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/_oasis b/info/control2011/_oasis
index fb0111d..be457fb 100644
--- a/info/control2011/_oasis
+++ b/info/control2011/_oasis
@@ -183,6 +183,13 @@ Executable "krobot-mc-simulator"
MainIs: krobot_mc_simulator.ml
BuildDepends: krobot, lwt.syntax
+Executable "krobot-io-simulator"
+ Path: src/tools
+ Install: true
+ CompiledObject: best
+ MainIs: krobot_io_simulator.ml
+ BuildDepends: krobot, lwt.syntax
+
Executable "krobot-planner"
Path: src/tools
Install: true
diff --git a/info/control2011/_tags b/info/control2011/_tags
index 4b665fc..9af45e4 100644
--- a/info/control2011/_tags
+++ b/info/control2011/_tags
@@ -2,7 +2,7 @@
<src/interfaces/*.ml>: -syntax_camlp4o
# OASIS_START
-# DO NOT EDIT (digest: 68e14c8d43423b2050fb1cf67d1938cc)
+# DO NOT EDIT (digest: 44c1536155bea156ff4e324744cffda8)
# Library krobot
"src/lib": include
<src/lib/krobot.{cma,cmxa}>: use_libkrobot
@@ -260,6 +260,11 @@
<src/tools/krobot_homologation.{native,byte}>: pkg_lwt.unix
<src/tools/krobot_homologation.{native,byte}>: pkg_lwt.syntax
<src/tools/krobot_homologation.{native,byte}>: pkg_lwt.react
+# Executable krobot-io-simulator
+<src/tools/krobot_io_simulator.{native,byte}>: use_krobot
+<src/tools/krobot_io_simulator.{native,byte}>: pkg_lwt.unix
+<src/tools/krobot_io_simulator.{native,byte}>: pkg_lwt.syntax
+<src/tools/krobot_io_simulator.{native,byte}>: pkg_lwt.react
# Executable krobot-pet
<src/tools/krobot_pet.{native,byte}>: use_krobot
<src/tools/krobot_pet.{native,byte}>: pkg_lwt.unix
diff --git a/info/control2011/setup.ml b/info/control2011/setup.ml
index 18934ac..2b5c6f0 100644
--- a/info/control2011/setup.ml
+++ b/info/control2011/setup.ml
@@ -8,7 +8,7 @@
*)
(* OASIS_START *)
-(* DO NOT EDIT (digest: 868e08593bf53c96a1f79923d193b872) *)
+(* DO NOT EDIT (digest: bf059af587069cdcfbcb6e820bb05831) *)
(*
Regenerated by OASIS v0.2.0
Visit http://oasis.forge.ocamlcore.org for more information and
@@ -6226,6 +6226,36 @@ let setup_t =
});
Executable
({
+ cs_name = "krobot-io-simulator";
+ cs_data = PropList.Data.create ();
+ cs_plugin_data = [];
+ },
+ {
+ bs_build = [(OASISExpr.EBool true, true)];
+ bs_install = [(OASISExpr.EBool true, true)];
+ bs_path = "src/tools";
+ bs_compiled_object = Best;
+ bs_build_depends =
+ [
+ InternalLibrary "krobot";
+ FindlibPackage ("lwt.syntax", None)
+ ];
+ bs_build_tools = [ExternalTool "ocamlbuild"];
+ bs_c_sources = [];
+ bs_data_files = [];
+ bs_ccopt = [(OASISExpr.EBool true, [])];
+ bs_cclib = [(OASISExpr.EBool true, [])];
+ bs_dlllib = [(OASISExpr.EBool true, [])];
+ bs_dllpath = [(OASISExpr.EBool true, [])];
+ bs_byteopt = [(OASISExpr.EBool true, [])];
+ bs_nativeopt = [(OASISExpr.EBool true, [])];
+ },
+ {
+ exec_custom = false;
+ exec_main_is = "krobot_io_simulator.ml";
+ });
+ Executable
+ ({
cs_name = "krobot-pet";
cs_data = PropList.Data.create ();
cs_plugin_data = [];
diff --git a/info/control2011/src/tools/krobot_io_simulator.ml b/info/control2011/src/tools/krobot_io_simulator.ml
new file mode 100644
index 0000000..9c37c01
--- /dev/null
+++ b/info/control2011/src/tools/krobot_io_simulator.ml
@@ -0,0 +1,116 @@
+(*
+ * krobot_io_simulator.ml
+ * ----------------
+ * Copyright : (c) 2013, Xavier Lagorce <Xav...@cr...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+(* Service simulating all inputs/outputs from the robot *)
+
+open Lwt
+open Lwt_react
+open Lwt_preemptive
+open Krobot_bus
+open Krobot_message
+open Krobot_geom
+
+let section = Lwt_log.Section.make "krobot(io_simulator)"
+
+(* +-----------------------------------------------------------------+
+ | Types |
+ +-----------------------------------------------------------------+ *)
+
+(* State of the robot. *)
+type state = {
+ s_x : float;
+ s_y : float;
+ theta : float;
+}
+
+let robot = ref { s_x = 0.; s_y = 0.; theta = 0.}
+
+(* +-----------------------------------------------------------------+
+ | read/send loop |
+ +-----------------------------------------------------------------+ *)
+
+let gen_data robot =
+ let dim = Array.length Krobot_config.urg_angles in
+ let l = ref [] in
+ for i = 0 to dim - 1 do
+ let angle = Krobot_config.urg_angles.(i) in
+ let x = 1. *. cos angle in
+ let y = 1. *. sin angle in
+ l := {x;y} :: !l
+ done;
+ Array.of_list !l
+
+let loop bus =
+ let rec aux () =
+ let time = Unix.gettimeofday () in
+ let msg = Urg (gen_data robot) in
+ lwt () = Krobot_bus.send bus (time, msg) in
+ lwt () = Lwt_unix.sleep 0.01 in
+ aux () in
+ aux ()
+
+(* +-----------------------------------------------------------------+
+ | Message handling |
+ +-----------------------------------------------------------------+ *)
+
+let handle_message (timestamp, message) =
+ match message with
+ | Kill "io_simulator" ->
+ exit 0
+ | CAN(_, frame) -> begin
+ match decode frame with
+ | Odometry(x, y, theta) ->
+ robot := { s_x = x; s_y = y; theta }
+ | _ ->
+ ()
+ end
+ | _ ->
+ ()
+
+(* +-----------------------------------------------------------------+
+ | Command-line arguments |
+ +-----------------------------------------------------------------+ *)
+
+let fork = ref true
+
+let options = Arg.align [
+ "-no-fork", Arg.Clear fork, " Run in foreground";
+]
+
+let usage = "\
+Usage: krobot-io-simulator [options]
+options are:"
+
+(* +-----------------------------------------------------------------+
+ | Entry point |
+ +-----------------------------------------------------------------+ *)
+
+lwt () =
+ Arg.parse options ignore usage;
+
+ (* Display all informative messages. *)
+ Lwt_log.append_rule "*" Lwt_log.Info;
+
+ (* Open the krobot bus. *)
+ lwt bus = Krobot_bus.get () in
+
+ (* Fork if not prevented. *)
+ if !fork then Krobot_daemon.daemonize bus;
+
+ (* Handle krobot message. *)
+ E.keep (E.map (handle_message) (Krobot_bus.recv bus));
+
+ (* Kill any running io_simulator. *)
+ lwt () = Krobot_bus.send bus (Unix.gettimeofday (), Krobot_bus.Kill "io_simulator") in
+
+ (* Wait a bit to let the other handler release the connection *)
+ lwt () = Lwt_unix.sleep 0.4 in
+
+ (* Loop forever. *)
+ Lwt_unix.run (loop bus)
hooks/post-receive
--
krobot
|
|
From: Xavier L. <Ba...@us...> - 2013-04-17 15:19:19
|
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 155f88832102bf2e0dccf85c195cfefa13887571 (commit)
from 6f13724d38ac3ed56ef1575dce5e68cb6f127bd2 (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 155f88832102bf2e0dccf85c195cfefa13887571
Author: Xavier Lagorce <Xav...@cr...>
Date: Wed Apr 17 17:22:37 2013 +0200
[info/krobot-mc-simulator] Minimum emulation of Bezier curves
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/src/tools/krobot_mc_simulator.ml b/info/control2011/src/tools/krobot_mc_simulator.ml
index dd0dc88..307551c 100644
--- a/info/control2011/src/tools/krobot_mc_simulator.ml
+++ b/info/control2011/src/tools/krobot_mc_simulator.ml
@@ -60,11 +60,8 @@ type command =
(* [Turn(t_acc, velocity)] *)
| Move of float * float
(* [Move(t_acc, velocity)] *)
- | Bezier_cmd of
- ( float * float * float * float * float * float ) *
- ( float * float * float )
- (** [Motor_bezier(x_end, y_end, d1, d2, theta_end, v_end)]
- [Motor_bezier_limits(v_max, a_tan_max, a_rad_max)] *)
+ | Bezier_cmd of (float * float * float) * bool
+ (** [Motor_bezier_limits(v_max, a_tan_max, a_rad_max)] *)
(* Type of simulators. *)
type simulator = {
@@ -84,6 +81,8 @@ type simulator = {
(* position on the Bezier's curve*)
mutable bezier_curve : Bezier.curve option;
(* Bezier's curve currently being followed if existing *)
+ mutable bezier_next : (Bezier.curve * bool) option;
+ (* Next Bezier's curve to follow *)
mutable time : float;
(* The current time. *)
mutable command : command;
@@ -127,10 +126,20 @@ velocities:
| Trajectory generation |
+-----------------------------------------------------------------+ *)
-let velocities sim =
+let velocities sim dt =
(* Put the robot into idle if the last command is terminated. *)
(match sim.command with
- | Bezier_cmd _ -> ()
+ | Bezier_cmd (_,cur_dir) ->
+ if sim.bezier_u >= 1. then begin
+ match sim.bezier_next with
+ | None ->
+ sim.command <- Idle
+ | Some (curve,dir) ->
+ sim.command <- Bezier_cmd (sim.bezier_limits, cur_dir);
+ sim.bezier_curve <- Some curve;
+ sim.bezier_next <- None;
+ sim.bezier_u <- 0.
+ end
| _ -> if sim.time > sim.command_end then sim.command <- Idle);
match sim.command with
| Idle ->
@@ -151,31 +160,46 @@ let velocities sim =
(vel, 0.)
else
(vel *. (sim.command_end -. sim.time) /. t_acc, 0.)
- | Bezier_cmd(_,_) ->
- sim.command <- Idle;
- sim.bezier_curve <- None;
- (0., 0.)
-
-(* val of_vertices : vertice -> vertice -> vertice -> vertice -> curve
- (** [of_vertices p q r s] creates a bezier curve from the given
- four control points. [p] and [s] are the first and end point
- of the curve. *)*)
+ | Bezier_cmd (limits, dir) ->
+ (match sim.bezier_curve with
+ | None ->
+ sim.command <- Idle;
+ (0., 0.)
+ | Some curve ->
+ let (v_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
+ sim.bezier_u <- sim.bezier_u +. vel /. s' *. dt;
+ if dir then
+ (vel, theta' *. vel /. s')
+ else
+ (-.vel, theta' *. vel /. s'))
let bezier sim (x_end, y_end, d1, d2, theta_end, v_end) =
- sim.bezier_u <- 0.;
let p,theta_start = match sim.bezier_curve with
| None ->
{Krobot_geom.x = sim.state.x; Krobot_geom.y = sim.state.y},
sim.state.theta
| Some curve ->
- let p,_,r,s = Bezier.pqrs curve in
+ let _,_,r,s = Bezier.pqrs curve in
s,
(angle (vector r s))
in
let s = {Krobot_geom.x = x_end; Krobot_geom.y = y_end} in
let q = translate p (vector_of_polar d1 theta_start) in
let r = translate s (vector_of_polar (-.d2) theta_end) in
- sim.bezier_curve <- Some (Bezier.of_vertices p q r s)
+ let dir = d1 >= 0. in
+ match sim.bezier_curve with
+ | None ->
+ sim.command <- Bezier_cmd (sim.bezier_limits,dir);
+ sim.bezier_u <- 0.;
+ sim.bezier_curve <- Some (Bezier.of_vertices p q r s);
+ | Some _ ->
+ sim.bezier_next <- Some ((Bezier.of_vertices p q r s),dir)
let move sim distance velocity acceleration =
if distance <> 0. && velocity > 0. && acceleration > 0. then begin
@@ -228,8 +252,8 @@ let set_velocities sim left_velocity right_velocity duration =
sim.command_start <- sim.time;
sim.command_end <- sim.time +. duration
-let get_velocities sim =
- let (u1, u2) = velocities sim in
+let get_velocities sim dt =
+ let (u1, u2) = velocities sim dt in
let l_v = (4. *. u1 +. wheels_distance *. u2) /. (2. *. wheels_diameter)
and r_v = (4. *. u1 -. wheels_distance *. u2) /. (2. *. wheels_diameter) in
(l_v, r_v)
@@ -260,20 +284,32 @@ let loop bus sim =
(sim.velocity_l +. sim.velocity_r) *. wheels_diameter /. 4. ,
(sim.velocity_l -. sim.velocity_r) *. wheels_diameter /. wheels_distance /. 2.
else
- velocities sim
+ velocities sim delta
in
let dx = u1 *. (cos sim.state.theta)
and dy = u1 *. (sin sim.state.theta)
and dtheta = u2 in
+ let theta = sim.state.theta +. dtheta *. delta in
+ let theta =
+ if theta > pi then
+ theta -. 2. *. pi
+ else if theta < -.pi then
+ theta +. 2. *. pi
+ else
+ theta
+ in
sim.state <- {
x = sim.state.x +. dx *. delta;
y = sim.state.y +. dy *. delta;
- theta = math_mod_float (sim.state.theta +. dtheta *. delta) (2. *. pi);
+ theta = theta;
};
sim.internal_state <- {
theta_l = sim.internal_state.theta_l +. delta *. (u1 *. 4. +. u2 *. wheels_distance) /. (2. *. wheels_diameter);
theta_r = sim.internal_state.theta_r +. delta *. (u1 *. 4. -. u2 *. wheels_distance) /. (2. *. wheels_diameter);
};
+ (match sim.command with
+ | Bezier_cmd _ -> sim.ghost <- sim.state
+ | _ -> ());
lwt () = Lwt_unix.sleep time_step in
aux () in
aux ()
@@ -343,7 +379,9 @@ let handle_message bus (timestamp, message) =
Lwt_unix.run (Lwt_log.info_f "received: turn(%f, %f, %f)" angle speed acc);
turn sim angle speed acc
| Motor_stop(lin_acc, rot_acc) ->
- sim.command <- Idle
+ sim.command <- Idle;
+ sim.bezier_curve <- None;
+ sim.bezier_next <- None;
| Set_odometry(x, y, theta) ->
sim.state_indep <- { x; y; theta }
| Set_odometry_indep(x, y, theta) ->
@@ -351,6 +389,7 @@ let handle_message bus (timestamp, message) =
| Motor_bezier_limits(v_max, a_tan_max, a_rad_max) ->
sim.bezier_limits <- (v_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)
| _ ->
() end);
@@ -399,6 +438,7 @@ lwt () =
ghost = { x = 0.; y = 0.; theta = 0. };
bezier_u = 0.;
bezier_curve = None;
+ bezier_next = None;
command = Idle;
command_start = 0.;
command_end = 0.;
hooks/post-receive
--
krobot
|
|
From: Xavier L. <Ba...@us...> - 2013-04-12 16:13:43
|
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 6f13724d38ac3ed56ef1575dce5e68cb6f127bd2 (commit)
via 64ffe837a0148b58e3afbe1122650a73252e5088 (commit)
from 629207b3e494c0a6c38c3085c3c637ee74d74770 (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 6f13724d38ac3ed56ef1575dce5e68cb6f127bd2
Author: Xavier Lagorce <Xav...@cr...>
Date: Fri Apr 12 18:15:07 2013 +0200
[info/krobot-mc-simulator] Several improvements
commit 64ffe837a0148b58e3afbe1122650a73252e5088
Author: Xavier Lagorce <Xav...@cr...>
Date: Fri Apr 12 18:14:42 2013 +0200
[info/Krobot_geom] Added a function to get the angle of a vector
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/src/lib/krobot_geom.ml b/info/control2011/src/lib/krobot_geom.ml
index dfcd56d..cb4f683 100644
--- a/info/control2011/src/lib/krobot_geom.ml
+++ b/info/control2011/src/lib/krobot_geom.ml
@@ -64,6 +64,7 @@ let ( *| ) = mul
let ( /| ) = div
let norm v = sqrt (sqr v.vx +. sqr v.vy)
+let angle v = atan2 v.vy v.vx
(* +-----------------------------------------------------------------+
| Vertices |
diff --git a/info/control2011/src/lib/krobot_geom.mli b/info/control2011/src/lib/krobot_geom.mli
index 5a87c19..7e2355d 100644
--- a/info/control2011/src/lib/krobot_geom.mli
+++ b/info/control2011/src/lib/krobot_geom.mli
@@ -45,6 +45,7 @@ val vector_of_polar : norm : float -> angle : float -> vector
(** [vector_of_polar norm angle] *)
val norm : vector -> float
+val angle : vector -> float
val distance : vertice -> vertice -> float
val normalize : vector -> vector
diff --git a/info/control2011/src/tools/krobot_mc_simulator.ml b/info/control2011/src/tools/krobot_mc_simulator.ml
index d1bd8de..dd0dc88 100644
--- a/info/control2011/src/tools/krobot_mc_simulator.ml
+++ b/info/control2011/src/tools/krobot_mc_simulator.ml
@@ -17,6 +17,7 @@ open Krobot_message
open Krobot_geom
let section = Lwt_log.Section.make "krobot(mc_simulator)"
+let time_step = 0.001
(* +-----------------------------------------------------------------+
| Command-line arguments |
@@ -59,7 +60,7 @@ type command =
(* [Turn(t_acc, velocity)] *)
| Move of float * float
(* [Move(t_acc, velocity)] *)
- | Bezier of
+ | Bezier_cmd of
( float * float * float * float * float * float ) *
( float * float * float )
(** [Motor_bezier(x_end, y_end, d1, d2, theta_end, v_end)]
@@ -69,12 +70,20 @@ type command =
type simulator = {
mutable state : state;
(* The state of the robot. *)
+ mutable state_indep : state;
+ (* The state of the robot for second set of encoders. *)
mutable internal_state : internal_state;
(* The state of the wheels. *)
mutable velocity_l : float;
(* Velocity of the left motor. *)
mutable velocity_r : float;
(* Velocity of the right motor. *)
+ mutable ghost : state;
+ (* The state of the ghost. *)
+ mutable bezier_u : float;
+ (* position on the Bezier's curve*)
+ mutable bezier_curve : Bezier.curve option;
+ (* Bezier's curve currently being followed if existing *)
mutable time : float;
(* The current time. *)
mutable command : command;
@@ -119,35 +128,54 @@ velocities:
+-----------------------------------------------------------------+ *)
let velocities sim =
+ (* Put the robot into idle if the last command is terminated. *)
+ (match sim.command with
+ | Bezier_cmd _ -> ()
+ | _ -> if sim.time > sim.command_end then sim.command <- Idle);
match sim.command with
| Idle ->
- (0., 0.)
+ (0., 0.)
| Speed(l_v, r_v) ->
- ((l_v +. r_v) *. wheels_diameter /. 4., (l_v -. r_v) *. wheels_diameter /. wheels_distance)
+ ((l_v +. r_v) *. wheels_diameter /. 4., (l_v -. r_v) *. wheels_diameter /. wheels_distance)
| Turn(t_acc, vel) ->
- if sim.time < (sim.command_start +. t_acc) then
- (0., vel *. (sim.time -. sim.command_start) /. t_acc)
- else if sim.time < (sim.command_end -. t_acc) then
- (0., vel)
- else
- (0., vel *. (sim.command_end -. sim.time) /. t_acc)
+ if sim.time < (sim.command_start +. t_acc) then
+ (0., vel *. (sim.time -. sim.command_start) /. t_acc)
+ else if sim.time < (sim.command_end -. t_acc) then
+ (0., vel)
+ else
+ (0., vel *. (sim.command_end -. sim.time) /. t_acc)
| Move(t_acc, vel) ->
- if sim.time < (sim.command_start +. t_acc) then
- (vel *. (sim.time -. sim.command_start) /. t_acc, 0.)
- else if sim.time < (sim.command_end -. t_acc) then
- (vel, 0.)
- else
- (vel *. (sim.command_end -. sim.time) /. t_acc, 0.)
- | Bezier(_,_) -> failwith "todo"
+ if sim.time < (sim.command_start +. t_acc) then
+ (vel *. (sim.time -. sim.command_start) /. t_acc, 0.)
+ else if sim.time < (sim.command_end -. t_acc) then
+ (vel, 0.)
+ else
+ (vel *. (sim.command_end -. sim.time) /. t_acc, 0.)
+ | Bezier_cmd(_,_) ->
+ sim.command <- Idle;
+ sim.bezier_curve <- None;
+ (0., 0.)
+
+(* val of_vertices : vertice -> vertice -> vertice -> vertice -> curve
+ (** [of_vertices p q r s] creates a bezier curve from the given
+ four control points. [p] and [s] are the first and end point
+ of the curve. *)*)
let bezier sim (x_end, y_end, d1, d2, theta_end, v_end) =
- assert false
-(*
- sim.command <- Bezier((x_end, y_end, d1, d2, theta_end, v_end),
- sim.bezier_limits);
- sim.command_start <- sim.time;
- sim.command_end <- sim.time +. t_end
-*)
+ sim.bezier_u <- 0.;
+ let p,theta_start = match sim.bezier_curve with
+ | None ->
+ {Krobot_geom.x = sim.state.x; Krobot_geom.y = sim.state.y},
+ sim.state.theta
+ | Some curve ->
+ let p,_,r,s = Bezier.pqrs curve in
+ s,
+ (angle (vector r s))
+ in
+ let s = {Krobot_geom.x = x_end; Krobot_geom.y = y_end} in
+ let q = translate p (vector_of_polar d1 theta_start) in
+ let r = translate s (vector_of_polar (-.d2) theta_end) in
+ sim.bezier_curve <- Some (Bezier.of_vertices p q r s)
let move sim distance velocity acceleration =
if distance <> 0. && velocity > 0. && acceleration > 0. then begin
@@ -215,7 +243,7 @@ let get_encoders sim =
(* +-----------------------------------------------------------------+
- | Main loop |
+ | Main loops |
+-----------------------------------------------------------------+ *)
let sim = ref None
@@ -226,21 +254,6 @@ let loop bus sim =
let delta = time -. sim.time in
sim.time <- time;
- (* Put the robot into idle if the last command is terminated. *)
- if sim.time > sim.command_end then sim.command <- Idle;
-
- (* Sends the state of the robot. *)
- lwt () = Krobot_message.send bus (sim.time, Odometry(sim.state.x, sim.state.y, sim.state.theta)) in
- lwt () = Krobot_message.send bus (sim.time, Odometry_indep(sim.state.x, sim.state.y, sim.state.theta)) in
- (* Sends the state of the motors. *)
- lwt () = match sim.command with
- | Turn(a, b) ->
- Krobot_message.send bus (Unix.gettimeofday (), Motor_status(false, true, false, false))
- | Move(a, b) ->
- Krobot_message.send bus (Unix.gettimeofday (), Motor_status(true, false, false, false))
- | _ ->
- Krobot_message.send bus (Unix.gettimeofday (), Motor_status(false, false, false, false))
- in
lwt () = print sim in
let u1, u2 = if !hil then
@@ -261,7 +274,36 @@ let loop bus sim =
theta_l = sim.internal_state.theta_l +. delta *. (u1 *. 4. +. u2 *. wheels_distance) /. (2. *. wheels_diameter);
theta_r = sim.internal_state.theta_r +. delta *. (u1 *. 4. -. u2 *. wheels_distance) /. (2. *. wheels_diameter);
};
- lwt () = Lwt_unix.sleep 0.01 in
+ lwt () = Lwt_unix.sleep time_step in
+ aux () in
+ aux ()
+
+let send_CAN_messages sim bus =
+ let rec aux () =
+ (* Sends the state of the robot. *)
+ lwt () = Krobot_message.send bus (sim.time, Odometry(sim.state.x, sim.state.y, sim.state.theta)) in
+ lwt () = Krobot_message.send bus (sim.time, Odometry_indep(sim.state.x, sim.state.y, sim.state.theta)) in
+ (* Wait before next batch of packets (emulate the electronic board behavior) *)
+ lwt () = Lwt_unix.sleep 0.005 in
+ (* Sends the state of the ghost. *)
+ lwt () = Krobot_message.send bus (sim.time,
+ Odometry_ghost(sim.ghost.x,
+ sim.ghost.y,
+ sim.ghost.theta,
+ int_of_float (255. *. sim.bezier_u),
+ match sim.command with Bezier_cmd _ -> true | _ -> false)) in
+ (* Sends the state of the motors. *)
+ lwt () = match sim.command with
+ | Turn(a, b) ->
+ Krobot_message.send bus (Unix.gettimeofday (), Motor_status(false, true, false, false))
+ | Move(a, b) ->
+ Krobot_message.send bus (Unix.gettimeofday (), Motor_status(true, false, false, false))
+ | Bezier_cmd _ ->
+ (* Motor State to be verifyed on the real Motor Controller card *)
+ Krobot_message.send bus (Unix.gettimeofday (), Motor_status(true, true, false, false))
+ | _ ->
+ Krobot_message.send bus (Unix.gettimeofday (), Motor_status(false, false, false, false)) in
+ lwt () = Lwt_unix.sleep 0.005 in
aux () in
aux ()
@@ -302,18 +344,8 @@ let handle_message bus (timestamp, message) =
turn sim angle speed acc
| Motor_stop(lin_acc, rot_acc) ->
sim.command <- Idle
- | Req_motor_status ->
- begin
- Lwt_unix.run (match sim.command with
- | Turn(a, b) ->
- Krobot_message.send bus (Unix.gettimeofday (), Motor_status(false, true, false, false))
- | Move(a, b) ->
- Krobot_message.send bus (Unix.gettimeofday (), Motor_status(true, false, false, false))
- | _ ->
- Krobot_message.send bus (Unix.gettimeofday (), Motor_status(false, false, false, false)))
- end
| Set_odometry(x, y, theta) ->
- sim.state <- { x; y; theta }
+ 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) ->
@@ -355,14 +387,18 @@ lwt () =
(* Set the motor_controller card in HIL mode if necessary *)
if !hil then
- Lwt_unix.run (Krobot_message.send bus (Unix.gettimeofday (), Set_controller_mode true)) ;
+ ignore (Krobot_message.send bus (Unix.gettimeofday (), Set_controller_mode true)) ;
(* Initial state of the simulator *)
let local_sim = {
state = { x = 0.; y = 0.; theta = 0. };
+ state_indep = { x = 0.; y = 0.; theta = 0. };
internal_state = { theta_l = 0.; theta_r = 0. };
velocity_l = 0.;
velocity_r = 0.;
+ ghost = { x = 0.; y = 0.; theta = 0. };
+ bezier_u = 0.;
+ bezier_curve = None;
command = Idle;
command_start = 0.;
command_end = 0.;
@@ -371,5 +407,7 @@ lwt () =
} in
sim := Some local_sim;
+ ignore(send_CAN_messages local_sim bus);
+
(* Loop forever. *)
- loop bus local_sim
+ Lwt_unix.run (loop bus local_sim)
hooks/post-receive
--
krobot
|
|
From: Xavier L. <Ba...@us...> - 2013-04-12 09:40:56
|
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 629207b3e494c0a6c38c3085c3c637ee74d74770 (commit)
from b8e1025d2b6fe0321b5d6e347e8eb9da65248e0a (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 629207b3e494c0a6c38c3085c3c637ee74d74770
Author: Xavier Lagorce <Xav...@cr...>
Date: Fri Apr 12 11:42:27 2013 +0200
[control2011/krobot-mc-simulator] Merged first version of non hil simulation
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/src/tools/krobot_mc_simulator.ml b/info/control2011/src/tools/krobot_mc_simulator.ml
index 4952685..d1bd8de 100644
--- a/info/control2011/src/tools/krobot_mc_simulator.ml
+++ b/info/control2011/src/tools/krobot_mc_simulator.ml
@@ -19,6 +19,22 @@ open Krobot_geom
let section = Lwt_log.Section.make "krobot(mc_simulator)"
(* +-----------------------------------------------------------------+
+ | Command-line arguments |
+ +-----------------------------------------------------------------+ *)
+
+let fork = ref true
+let hil = ref false
+
+let options = Arg.align [
+ "-no-fork", Arg.Clear fork, " Run in foreground";
+ "-hil", Arg.Set hil, " Run in hardware in the loop mode";
+]
+
+let usage = "\
+Usage: krobot-mc-simulator [options]
+options are:"
+
+(* +-----------------------------------------------------------------+
| Types |
+-----------------------------------------------------------------+ *)
@@ -36,13 +52,18 @@ type internal_state = {
type command =
| Idle
- (* The robot is doint nothing. *)
+ (* The robot is doing nothing. *)
| Speed of float * float
(* [Speed(left_velocity, right_velocity)] *)
| Turn of float * float
(* [Turn(t_acc, velocity)] *)
| Move of float * float
(* [Move(t_acc, velocity)] *)
+ | Bezier of
+ ( float * float * float * float * float * float ) *
+ ( float * float * float )
+ (** [Motor_bezier(x_end, y_end, d1, d2, theta_end, v_end)]
+ [Motor_bezier_limits(v_max, a_tan_max, a_rad_max)] *)
(* Type of simulators. *)
type simulator = {
@@ -51,11 +72,18 @@ type simulator = {
mutable internal_state : internal_state;
(* The state of the wheels. *)
mutable velocity_l : float;
- (* Velocity of the left motor (read from the CAN). *)
+ (* Velocity of the left motor. *)
mutable velocity_r : float;
- (* Velocity of the right motor (read from the CAN). *)
+ (* Velocity of the right motor. *)
mutable time : float;
(* The current time. *)
+ mutable command : command;
+ (* The current command. *)
+ mutable command_start : float;
+ (* The start time of the current command. *)
+ mutable command_end : float;
+ (* The end time of the current command. *)
+ mutable bezier_limits : float * float * float;
}
(* +-----------------------------------------------------------------+
@@ -85,6 +113,107 @@ velocities:
sim.velocity_l
sim.velocity_r
+
+(* +-----------------------------------------------------------------+
+ | Trajectory generation |
+ +-----------------------------------------------------------------+ *)
+
+let velocities sim =
+ match sim.command with
+ | Idle ->
+ (0., 0.)
+ | Speed(l_v, r_v) ->
+ ((l_v +. r_v) *. wheels_diameter /. 4., (l_v -. r_v) *. wheels_diameter /. wheels_distance)
+ | Turn(t_acc, vel) ->
+ if sim.time < (sim.command_start +. t_acc) then
+ (0., vel *. (sim.time -. sim.command_start) /. t_acc)
+ else if sim.time < (sim.command_end -. t_acc) then
+ (0., vel)
+ else
+ (0., vel *. (sim.command_end -. sim.time) /. t_acc)
+ | Move(t_acc, vel) ->
+ if sim.time < (sim.command_start +. t_acc) then
+ (vel *. (sim.time -. sim.command_start) /. t_acc, 0.)
+ else if sim.time < (sim.command_end -. t_acc) then
+ (vel, 0.)
+ else
+ (vel *. (sim.command_end -. sim.time) /. t_acc, 0.)
+ | Bezier(_,_) -> failwith "todo"
+
+let bezier sim (x_end, y_end, d1, d2, theta_end, v_end) =
+ assert false
+(*
+ sim.command <- Bezier((x_end, y_end, d1, d2, theta_end, v_end),
+ sim.bezier_limits);
+ sim.command_start <- sim.time;
+ sim.command_end <- sim.time +. t_end
+*)
+
+let move sim distance velocity acceleration =
+ if distance <> 0. && velocity > 0. && acceleration > 0. then begin
+ let t_acc = velocity /. acceleration in
+ let t_end = (velocity *. velocity +. distance *. acceleration) /. (velocity *. acceleration) in
+ if t_end > 2. *. t_acc then begin
+ if t_acc <> 0. then begin
+ sim.command <- Move(t_acc, velocity);
+ sim.command_start <- sim.time;
+ sim.command_end <- sim.time +. t_end
+ end
+ end else begin
+ if t_acc <> 0. then begin
+ let t_acc = sqrt (abs_float (distance) /. acceleration) in
+ let t_end = 2. *. t_acc in
+ let sign = if distance >= 0. then 1. else -1. in
+ let velocity = sign *. acceleration *. t_acc in
+ sim.command <- Move(t_acc, velocity);
+ sim.command_start <- sim.time;
+ sim.command_end <- sim.time +. t_end
+ end
+ end
+ end
+
+let turn sim angle velocity acceleration =
+ if angle <> 0. && velocity > 0. && acceleration > 0. then begin
+ let t_acc = velocity /. acceleration in
+ let t_end = (velocity *. velocity +. angle *. acceleration) /. (velocity *. acceleration) in
+ if t_end > 2. *. t_acc then begin
+ if t_acc <> 0. then begin
+ sim.command <- Turn(t_acc, velocity);
+ sim.command_start <- sim.time;
+ sim.command_end <- sim.time +. t_end
+ end
+ end else begin
+ let t_acc = sqrt (abs_float (angle) /. acceleration) in
+ let t_end = 2. *. t_acc in
+ let sign = if angle >= 0. then 1. else -1. in
+ let velocity = sign *. acceleration *. t_acc in
+ if t_acc <> 0. then begin
+ sim.command <- Turn(t_acc, velocity);
+ sim.command_start <- sim.time;
+ sim.command_end <- sim.time +. t_end
+ end
+ end
+ end
+
+let set_velocities sim left_velocity right_velocity duration =
+ sim.command <- Speed(left_velocity, right_velocity);
+ sim.command_start <- sim.time;
+ sim.command_end <- sim.time +. duration
+
+let get_velocities sim =
+ let (u1, u2) = velocities sim in
+ let l_v = (4. *. u1 +. wheels_distance *. u2) /. (2. *. wheels_diameter)
+ and r_v = (4. *. u1 -. wheels_distance *. u2) /. (2. *. wheels_diameter) in
+ (l_v, r_v)
+
+let get_state sim =
+ sim.state
+
+let get_encoders sim =
+ let (theta_l, theta_r) = (sim.internal_state.theta_l, sim.internal_state.theta_r) in
+ (theta_l *. wheels_diameter /. 2., theta_r *. wheels_diameter /. 2.)
+
+
(* +-----------------------------------------------------------------+
| Main loop |
+-----------------------------------------------------------------+ *)
@@ -97,12 +226,29 @@ let loop bus sim =
let delta = time -. sim.time in
sim.time <- time;
+ (* Put the robot into idle if the last command is terminated. *)
+ if sim.time > sim.command_end then sim.command <- Idle;
+
(* Sends the state of the robot. *)
lwt () = Krobot_message.send bus (sim.time, Odometry(sim.state.x, sim.state.y, sim.state.theta)) in
+ lwt () = Krobot_message.send bus (sim.time, Odometry_indep(sim.state.x, sim.state.y, sim.state.theta)) in
+ (* Sends the state of the motors. *)
+ lwt () = match sim.command with
+ | Turn(a, b) ->
+ Krobot_message.send bus (Unix.gettimeofday (), Motor_status(false, true, false, false))
+ | Move(a, b) ->
+ Krobot_message.send bus (Unix.gettimeofday (), Motor_status(true, false, false, false))
+ | _ ->
+ Krobot_message.send bus (Unix.gettimeofday (), Motor_status(false, false, false, false))
+ in
lwt () = print sim in
- let u1 = (sim.velocity_l +. sim.velocity_r) *. wheels_diameter /. 4.
- and u2 = (sim.velocity_l -. sim.velocity_r) *. wheels_diameter /. wheels_distance /. 2. in
+ let u1, u2 = if !hil then
+ (sim.velocity_l +. sim.velocity_r) *. wheels_diameter /. 4. ,
+ (sim.velocity_l -. sim.velocity_r) *. wheels_diameter /. wheels_distance /. 2.
+ else
+ velocities sim
+ in
let dx = u1 *. (cos sim.state.theta)
and dy = u1 *. (sin sim.state.theta)
and dtheta = u2 in
@@ -123,47 +269,66 @@ let loop bus sim =
| Message handling |
+-----------------------------------------------------------------+ *)
-let handle_message (timestamp, message) =
+let handle_message bus (timestamp, message) =
match message with
| Kill "mc_simulator" ->
exit 0
| CAN(_, frame) -> begin
match !sim with
| Some sim -> begin
- match decode frame with
- | Encoder_position_speed_3(pos, speed) ->
- sim.velocity_r <- speed;
- Lwt.return ()
- | Encoder_position_speed_4(pos, speed) ->
- sim.velocity_l <- speed;
- Lwt.return ()
+ (* Generic messages *)
+ (match decode frame with
| Set_odometry(x, y, theta) ->
- sim.state <- { x; y; theta };
- Lwt.return ()
+ sim.state <- { x; y; theta }
| _ ->
- Lwt.return () end
+ ());
+ (* Messages related to HIL mode *)
+ (if !hil then begin
+ match decode frame with
+ | Encoder_position_speed_3(pos, speed) ->
+ sim.velocity_r <- speed
+ | Encoder_position_speed_4(pos, speed) ->
+ sim.velocity_l <- speed
+ | _ ->
+ () end
+ (* Message related to full software simulation mode *)
+ else begin
+ match decode frame with
+ | Motor_move(dist, speed, acc) ->
+ Lwt_unix.run (Lwt_log.info_f "received: move(%f, %f, %f)" dist speed acc);
+ move sim dist speed acc
+ | Motor_turn(angle, speed, acc) ->
+ Lwt_unix.run (Lwt_log.info_f "received: turn(%f, %f, %f)" angle speed acc);
+ turn sim angle speed acc
+ | Motor_stop(lin_acc, rot_acc) ->
+ sim.command <- Idle
+ | Req_motor_status ->
+ begin
+ Lwt_unix.run (match sim.command with
+ | Turn(a, b) ->
+ Krobot_message.send bus (Unix.gettimeofday (), Motor_status(false, true, false, false))
+ | Move(a, b) ->
+ Krobot_message.send bus (Unix.gettimeofday (), Motor_status(true, false, false, false))
+ | _ ->
+ Krobot_message.send bus (Unix.gettimeofday (), Motor_status(false, false, false, false)))
+ end
+ | Set_odometry(x, y, theta) ->
+ sim.state <- { 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(x_end, y_end, d1, d2, theta_end, v_end) ->
+ bezier sim (x_end, y_end, d1, d2, theta_end, v_end)
+ | _ ->
+ () end);
+ Lwt.return () end
| None ->
Lwt.return ()
end
| _ -> Lwt.return ()
(* +-----------------------------------------------------------------+
- | Command-line arguments |
- +-----------------------------------------------------------------+ *)
-
-let fork = ref true
-let hil = ref false
-
-let options = Arg.align [
- "-no-fork", Arg.Clear fork, " Run in foreground";
- "-hil", Arg.Set hil, " Run in hardware in the loop mode";
-]
-
-let usage = "\
-Usage: krobot-motor-controller-simulator [options]
-options are:"
-
-(* +-----------------------------------------------------------------+
| Entry point |
+-----------------------------------------------------------------+ *)
@@ -180,7 +345,7 @@ lwt () =
if !fork then Krobot_daemon.daemonize bus;
(* Handle krobot message. *)
- E.keep (E.map handle_message (Krobot_bus.recv bus));
+ E.keep (E.map (handle_message bus) (Krobot_bus.recv bus));
(* Kill any running mc_simulator. *)
lwt () = Krobot_bus.send bus (Unix.gettimeofday (), Krobot_bus.Kill "mc_simulator") in
@@ -189,10 +354,8 @@ lwt () =
lwt () = Lwt_unix.sleep 0.4 in
(* Set the motor_controller card in HIL mode if necessary *)
- lwt () = if !hil then
- Krobot_message.send bus (Unix.gettimeofday (), Set_controller_mode true)
- else
- Krobot_message.send bus (Unix.gettimeofday (), Set_controller_mode false) in
+ if !hil then
+ Lwt_unix.run (Krobot_message.send bus (Unix.gettimeofday (), Set_controller_mode true)) ;
(* Initial state of the simulator *)
let local_sim = {
@@ -200,7 +363,11 @@ lwt () =
internal_state = { theta_l = 0.; theta_r = 0. };
velocity_l = 0.;
velocity_r = 0.;
+ command = Idle;
+ command_start = 0.;
+ command_end = 0.;
time = Unix.gettimeofday ();
+ bezier_limits = 1., 1., 1.;
} in
sim := Some local_sim;
hooks/post-receive
--
krobot
|
|
From: Xavier L. <Ba...@us...> - 2013-04-11 17:07:48
|
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 b8e1025d2b6fe0321b5d6e347e8eb9da65248e0a (commit)
from ce3ff750e287178ea2034d0a683371562d679e9e (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 b8e1025d2b6fe0321b5d6e347e8eb9da65248e0a
Author: Xavier Lagorce <Xav...@cr...>
Date: Thu Apr 11 19:09:02 2013 +0200
[control2011/krobot-mc-simulator] Initial import of new general simulator for motor controller card
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/_oasis b/info/control2011/_oasis
index f90e326..fb0111d 100644
--- a/info/control2011/_oasis
+++ b/info/control2011/_oasis
@@ -176,6 +176,13 @@ Executable "krobot-hil-simulator"
MainIs: krobot_hil_simulator.ml
BuildDepends: krobot, lwt.syntax
+Executable "krobot-mc-simulator"
+ Path: src/tools
+ Install: true
+ CompiledObject: best
+ MainIs: krobot_mc_simulator.ml
+ BuildDepends: krobot, lwt.syntax
+
Executable "krobot-planner"
Path: src/tools
Install: true
diff --git a/info/control2011/_tags b/info/control2011/_tags
index 5ca553f..4b665fc 100644
--- a/info/control2011/_tags
+++ b/info/control2011/_tags
@@ -2,279 +2,276 @@
<src/interfaces/*.ml>: -syntax_camlp4o
# OASIS_START
-# DO NOT EDIT (digest: 9d24891399e4580abf4972e7db50e404)
-# Ignore VCS directories, you can use the same kind of rule outside
-# OASIS_START/STOP if you want to exclude directories that contains
-# useless stuff for the build process
-<**/.svn>: -traverse
-<**/.svn>: not_hygienic
-".bzr": -traverse
-".bzr": not_hygienic
-".hg": -traverse
-".hg": not_hygienic
-".git": -traverse
-".git": not_hygienic
-"_darcs": -traverse
-"_darcs": not_hygienic
+# DO NOT EDIT (digest: 68e14c8d43423b2050fb1cf67d1938cc)
# Library krobot
-"src/lib/krobot.cmxs": use_krobot
-<src/lib/krobot.{cma,cmxa}>: use_libkrobot_stubs
-<src/lib/*.ml{,i}>: pkg_lwt.syntax
+"src/lib": include
+<src/lib/krobot.{cma,cmxa}>: use_libkrobot
<src/lib/*.ml{,i}>: pkg_lwt.unix
+<src/lib/*.ml{,i}>: pkg_lwt.syntax
<src/lib/*.ml{,i}>: pkg_lwt.react
-"src/lib/krobot_message_stubs.c": pkg_lwt.syntax
"src/lib/krobot_message_stubs.c": pkg_lwt.unix
+"src/lib/krobot_message_stubs.c": pkg_lwt.syntax
"src/lib/krobot_message_stubs.c": pkg_lwt.react
# Library krobot-can
-"src/can/krobot-can.cmxs": use_krobot-can
-<src/can/krobot-can.{cma,cmxa}>: use_libkrobot-can_stubs
+"src/can": include
+<src/can/krobot-can.{cma,cmxa}>: use_libkrobot-can
<src/can/*.ml{,i}>: use_krobot
-<src/can/*.ml{,i}>: pkg_lwt.syntax
-<src/can/*.ml{,i}>: pkg_sexplib
<src/can/*.ml{,i}>: pkg_sexplib.syntax
+<src/can/*.ml{,i}>: pkg_sexplib
<src/can/*.ml{,i}>: pkg_lwt.unix
-<src/can/*.ml{,i}>: pkg_bitstring
+<src/can/*.ml{,i}>: pkg_lwt.syntax
<src/can/*.ml{,i}>: pkg_lwt.react
+<src/can/*.ml{,i}>: pkg_bitstring
"src/can/can_stubs.c": use_krobot
-"src/can/can_stubs.c": pkg_lwt.syntax
-"src/can/can_stubs.c": pkg_sexplib
"src/can/can_stubs.c": pkg_sexplib.syntax
+"src/can/can_stubs.c": pkg_sexplib
"src/can/can_stubs.c": pkg_lwt.unix
-"src/can/can_stubs.c": pkg_bitstring
+"src/can/can_stubs.c": pkg_lwt.syntax
"src/can/can_stubs.c": pkg_lwt.react
+"src/can/can_stubs.c": pkg_bitstring
+# Executable krobot-dump
+<src/tools/krobot_dump.{native,byte}>: use_krobot
+<src/tools/krobot_dump.{native,byte}>: pkg_lwt.unix
+<src/tools/krobot_dump.{native,byte}>: pkg_lwt.syntax
+<src/tools/krobot_dump.{native,byte}>: pkg_lwt.react
+# Executable krobot-ax12-control
+<src/tools/krobot_ax12_control.{native,byte}>: use_krobot-can
+<src/tools/krobot_ax12_control.{native,byte}>: use_krobot
+<src/tools/krobot_ax12_control.{native,byte}>: pkg_sexplib.syntax
+<src/tools/krobot_ax12_control.{native,byte}>: pkg_sexplib
+<src/tools/krobot_ax12_control.{native,byte}>: pkg_lwt.unix
+<src/tools/krobot_ax12_control.{native,byte}>: pkg_lwt.syntax
+<src/tools/krobot_ax12_control.{native,byte}>: pkg_lwt.react
+<src/tools/krobot_ax12_control.{native,byte}>: pkg_bitstring
+# Executable krobot-dump-encoders
+<src/tools/krobot_dump_encoders.{native,byte}>: use_krobot
+<src/tools/krobot_dump_encoders.{native,byte}>: pkg_lwt.unix
+<src/tools/krobot_dump_encoders.{native,byte}>: pkg_lwt.syntax
+<src/tools/krobot_dump_encoders.{native,byte}>: pkg_lwt.react
+# Executable krobot-beacon-reader
+<src/tools/krobot_beacon_reader.{native,byte}>: use_krobot
+<src/tools/krobot_beacon_reader.{native,byte}>: pkg_lwt.unix
+<src/tools/krobot_beacon_reader.{native,byte}>: pkg_lwt.syntax
+<src/tools/krobot_beacon_reader.{native,byte}>: pkg_lwt.react
# Executable krobot-driver
<src/driver/krobot_driver.{native,byte}>: use_krobot-can
<src/driver/krobot_driver.{native,byte}>: use_krobot
-<src/driver/krobot_driver.{native,byte}>: pkg_lwt.syntax
-<src/driver/krobot_driver.{native,byte}>: pkg_sexplib
<src/driver/krobot_driver.{native,byte}>: pkg_sexplib.syntax
+<src/driver/krobot_driver.{native,byte}>: pkg_sexplib
<src/driver/krobot_driver.{native,byte}>: pkg_lwt.unix
-<src/driver/krobot_driver.{native,byte}>: pkg_bitstring
+<src/driver/krobot_driver.{native,byte}>: pkg_lwt.syntax
<src/driver/krobot_driver.{native,byte}>: pkg_lwt.react
+<src/driver/krobot_driver.{native,byte}>: pkg_bitstring
<src/driver/*.ml{,i}>: use_krobot-can
<src/driver/*.ml{,i}>: use_krobot
-<src/driver/*.ml{,i}>: pkg_lwt.syntax
-<src/driver/*.ml{,i}>: pkg_sexplib
<src/driver/*.ml{,i}>: pkg_sexplib.syntax
+<src/driver/*.ml{,i}>: pkg_sexplib
<src/driver/*.ml{,i}>: pkg_lwt.unix
-<src/driver/*.ml{,i}>: pkg_bitstring
+<src/driver/*.ml{,i}>: pkg_lwt.syntax
<src/driver/*.ml{,i}>: pkg_lwt.react
-# Executable krobot-hub
-<src/tools/krobot_hub.{native,byte}>: pkg_lwt.syntax
-<src/tools/krobot_hub.{native,byte}>: pkg_lwt.unix
-# Executable krobot-dump
-<src/tools/krobot_dump.{native,byte}>: use_krobot
-<src/tools/krobot_dump.{native,byte}>: pkg_lwt.syntax
-<src/tools/krobot_dump.{native,byte}>: pkg_lwt.unix
-<src/tools/krobot_dump.{native,byte}>: pkg_lwt.react
-# Executable krobot-monitor
-<src/tools/krobot_monitor.{native,byte}>: use_krobot
-<src/tools/krobot_monitor.{native,byte}>: pkg_lwt.syntax
-<src/tools/krobot_monitor.{native,byte}>: pkg_lwt.unix
-<src/tools/krobot_monitor.{native,byte}>: pkg_lwt.react
-# Executable krobot-dump-encoders
-<src/tools/krobot_dump_encoders.{native,byte}>: use_krobot
-<src/tools/krobot_dump_encoders.{native,byte}>: pkg_lwt.syntax
-<src/tools/krobot_dump_encoders.{native,byte}>: pkg_lwt.unix
-<src/tools/krobot_dump_encoders.{native,byte}>: pkg_lwt.react
-# Executable krobot-record
-<src/tools/krobot_record.{native,byte}>: use_krobot
-<src/tools/krobot_record.{native,byte}>: pkg_lwt.syntax
-<src/tools/krobot_record.{native,byte}>: pkg_threads
-<src/tools/krobot_record.{native,byte}>: pkg_lwt.unix
-<src/tools/krobot_record.{native,byte}>: pkg_lwt.react
-# Executable krobot-replay
-<src/tools/krobot_replay.{native,byte}>: use_krobot
-<src/tools/krobot_replay.{native,byte}>: pkg_lwt.syntax
-<src/tools/krobot_replay.{native,byte}>: pkg_lwt.unix
-<src/tools/krobot_replay.{native,byte}>: pkg_lwt.react
-# Executable krobot-read
-<src/tools/krobot_read.{native,byte}>: use_krobot
-<src/tools/krobot_read.{native,byte}>: pkg_lwt.syntax
-<src/tools/krobot_read.{native,byte}>: pkg_lwt.unix
-<src/tools/krobot_read.{native,byte}>: pkg_lwt.react
-# Executable krobot-plot
-<src/tools/krobot_plot.{native,byte}>: use_krobot
-<src/tools/krobot_plot.{native,byte}>: pkg_lwt.syntax
-<src/tools/krobot_plot.{native,byte}>: pkg_cairo.lablgtk2
-<src/tools/krobot_plot.{native,byte}>: pkg_lwt.glib
-<src/tools/krobot_plot.{native,byte}>: pkg_lwt.unix
-<src/tools/krobot_plot.{native,byte}>: pkg_lwt.react
-# Executable krobot-plot-battery
-<src/tools/krobot_plot_battery.{native,byte}>: use_krobot
-<src/tools/krobot_plot_battery.{native,byte}>: pkg_lwt.syntax
-<src/tools/krobot_plot_battery.{native,byte}>: pkg_cairo.lablgtk2
-<src/tools/krobot_plot_battery.{native,byte}>: pkg_lwt.glib
-<src/tools/krobot_plot_battery.{native,byte}>: pkg_lwt.unix
-<src/tools/krobot_plot_battery.{native,byte}>: pkg_lwt.react
+<src/driver/*.ml{,i}>: pkg_bitstring
# Executable krobot-viewer
<src/tools/krobot_viewer.{native,byte}>: use_krobot
+<src/tools/krobot_viewer.{native,byte}>: pkg_lwt.unix
<src/tools/krobot_viewer.{native,byte}>: pkg_lwt.syntax
-<src/tools/krobot_viewer.{native,byte}>: pkg_cairo.lablgtk2
+<src/tools/krobot_viewer.{native,byte}>: pkg_lwt.react
<src/tools/krobot_viewer.{native,byte}>: pkg_lwt.glib
<src/tools/krobot_viewer.{native,byte}>: pkg_lablgtk2.glade
-<src/tools/krobot_viewer.{native,byte}>: pkg_lwt.unix
-<src/tools/krobot_viewer.{native,byte}>: pkg_lwt.react
+<src/tools/krobot_viewer.{native,byte}>: pkg_cairo.lablgtk2
<src/tools/*.ml{,i}>: pkg_lablgtk2.glade
-# Executable krobot-simulator
-<src/tools/krobot_simulator.{native,byte}>: use_krobot
-<src/tools/krobot_simulator.{native,byte}>: pkg_lwt.syntax
-<src/tools/krobot_simulator.{native,byte}>: pkg_lwt.unix
-<src/tools/krobot_simulator.{native,byte}>: pkg_lwt.react
+# Executable krobot-monitor
+<src/tools/krobot_monitor.{native,byte}>: use_krobot
+<src/tools/krobot_monitor.{native,byte}>: pkg_lwt.unix
+<src/tools/krobot_monitor.{native,byte}>: pkg_lwt.syntax
+<src/tools/krobot_monitor.{native,byte}>: pkg_lwt.react
# Executable krobot-hil-simulator
<src/tools/krobot_hil_simulator.{native,byte}>: use_krobot
-<src/tools/krobot_hil_simulator.{native,byte}>: pkg_lwt.syntax
<src/tools/krobot_hil_simulator.{native,byte}>: pkg_lwt.unix
+<src/tools/krobot_hil_simulator.{native,byte}>: pkg_lwt.syntax
<src/tools/krobot_hil_simulator.{native,byte}>: pkg_lwt.react
-# Executable krobot-planner
-<src/tools/krobot_planner.{native,byte}>: use_krobot
-<src/tools/krobot_planner.{native,byte}>: pkg_lwt.syntax
-<src/tools/krobot_planner.{native,byte}>: pkg_lwt.unix
-<src/tools/krobot_planner.{native,byte}>: pkg_lwt.react
+# Executable krobot-hub
+<src/tools/krobot_hub.{native,byte}>: pkg_lwt.unix
+<src/tools/krobot_hub.{native,byte}>: pkg_lwt.syntax
+# Executable krobot-calibrate-sharps
+<src/tools/krobot_calibrate_sharps.{native,byte}>: use_krobot
+<src/tools/krobot_calibrate_sharps.{native,byte}>: pkg_lwt.unix
+<src/tools/krobot_calibrate_sharps.{native,byte}>: pkg_lwt.syntax
+<src/tools/krobot_calibrate_sharps.{native,byte}>: pkg_lwt.react
+# Executable krobot-see-coin
+<src/tools/krobot_see_coin.{native,byte}>: use_krobot
+<src/tools/krobot_see_coin.{native,byte}>: pkg_lwt.unix
+<src/tools/krobot_see_coin.{native,byte}>: pkg_lwt.syntax
+<src/tools/krobot_see_coin.{native,byte}>: pkg_lwt.react
+<src/tools/krobot_see_coin.{native,byte}>: pkg_cv
+<src/tools/*.ml{,i}>: pkg_cv
+# Executable send-can
+<examples/send_can.{native,byte}>: use_krobot-can
+<examples/send_can.{native,byte}>: use_krobot
+<examples/send_can.{native,byte}>: pkg_sexplib.syntax
+<examples/send_can.{native,byte}>: pkg_sexplib
+<examples/send_can.{native,byte}>: pkg_lwt.unix
+<examples/send_can.{native,byte}>: pkg_lwt.syntax
+<examples/send_can.{native,byte}>: pkg_lwt.react
+<examples/send_can.{native,byte}>: pkg_bitstring
+# Executable krobot-vm
+<src/tools/krobot_vm.{native,byte}>: use_krobot
+<src/tools/krobot_vm.{native,byte}>: pkg_lwt.unix
+<src/tools/krobot_vm.{native,byte}>: pkg_lwt.syntax
+<src/tools/krobot_vm.{native,byte}>: pkg_lwt.react
+# Executable krobot-webcam
+<src/tools/krobot_webcam.{native,byte}>: use_krobot
+<src/tools/krobot_webcam.{native,byte}>: pkg_lwt.unix
+<src/tools/krobot_webcam.{native,byte}>: pkg_lwt.syntax
+<src/tools/krobot_webcam.{native,byte}>: pkg_lwt.react
+# Executable krobot-stop-beacon
+<src/tools/krobot_stop_beacon.{native,byte}>: use_krobot
+<src/tools/krobot_stop_beacon.{native,byte}>: pkg_lwt.unix
+<src/tools/krobot_stop_beacon.{native,byte}>: pkg_lwt.syntax
+<src/tools/krobot_stop_beacon.{native,byte}>: pkg_lwt.react
# Executable krobot-fake-beacons
<src/tools/krobot_fake_beacons.{native,byte}>: use_krobot
-<src/tools/krobot_fake_beacons.{native,byte}>: pkg_lwt.syntax
<src/tools/krobot_fake_beacons.{native,byte}>: pkg_lwt.unix
+<src/tools/krobot_fake_beacons.{native,byte}>: pkg_lwt.syntax
<src/tools/krobot_fake_beacons.{native,byte}>: pkg_lwt.react
+# Executable krobot-urg
+<src/tools/krobot_urg.{native,byte}>: use_krobot
+<src/tools/krobot_urg.{native,byte}>: pkg_urg
+<src/tools/krobot_urg.{native,byte}>: pkg_threads
+<src/tools/krobot_urg.{native,byte}>: pkg_lwt.unix
+<src/tools/krobot_urg.{native,byte}>: pkg_lwt.syntax
+<src/tools/krobot_urg.{native,byte}>: pkg_lwt.react
+<src/tools/krobot_urg.{native,byte}>: pkg_lwt.preemptive
+<src/tools/*.ml{,i}>: pkg_urg
+<src/tools/*.ml{,i}>: pkg_lwt.preemptive
+# Executable krobot-read
+<src/tools/krobot_read.{native,byte}>: use_krobot
+<src/tools/krobot_read.{native,byte}>: pkg_lwt.unix
+<src/tools/krobot_read.{native,byte}>: pkg_lwt.syntax
+<src/tools/krobot_read.{native,byte}>: pkg_lwt.react
# Executable krobot-objects
<src/tools/krobot_objects.{native,byte}>: use_krobot
-<src/tools/krobot_objects.{native,byte}>: pkg_lwt.syntax
<src/tools/krobot_objects.{native,byte}>: pkg_lwt.unix
+<src/tools/krobot_objects.{native,byte}>: pkg_lwt.syntax
<src/tools/krobot_objects.{native,byte}>: pkg_lwt.react
-# Executable krobot-webcam
-<src/tools/krobot_webcam.{native,byte}>: use_krobot
-<src/tools/krobot_webcam.{native,byte}>: pkg_lwt.syntax
-<src/tools/krobot_webcam.{native,byte}>: pkg_lwt.unix
-<src/tools/krobot_webcam.{native,byte}>: pkg_lwt.react
+# Executable krobot-ia
+<src/tools/krobot_ia.{native,byte}>: use_krobot
+<src/tools/krobot_ia.{native,byte}>: pkg_lwt.unix
+<src/tools/krobot_ia.{native,byte}>: pkg_lwt.syntax
+<src/tools/krobot_ia.{native,byte}>: pkg_lwt.react
+# Executable krobot-joystick
+<src/tools/krobot_joystick.{native,byte}>: use_krobot
+<src/tools/krobot_joystick.{native,byte}>: pkg_sdl
+<src/tools/krobot_joystick.{native,byte}>: pkg_lwt.unix
+<src/tools/krobot_joystick.{native,byte}>: pkg_lwt.syntax
+<src/tools/krobot_joystick.{native,byte}>: pkg_lwt.react
+<src/tools/*.ml{,i}>: pkg_sdl
# Executable krobot-kill
<src/tools/krobot_kill.{native,byte}>: use_krobot
-<src/tools/krobot_kill.{native,byte}>: pkg_lwt.syntax
<src/tools/krobot_kill.{native,byte}>: pkg_lwt.unix
+<src/tools/krobot_kill.{native,byte}>: pkg_lwt.syntax
<src/tools/krobot_kill.{native,byte}>: pkg_lwt.react
-# Executable krobot-calibrate-sharps
-<src/tools/krobot_calibrate_sharps.{native,byte}>: use_krobot
-<src/tools/krobot_calibrate_sharps.{native,byte}>: pkg_lwt.syntax
-<src/tools/krobot_calibrate_sharps.{native,byte}>: pkg_lwt.unix
-<src/tools/krobot_calibrate_sharps.{native,byte}>: pkg_lwt.react
+# Executable krobot-plot-battery
+<src/tools/krobot_plot_battery.{native,byte}>: use_krobot
+<src/tools/krobot_plot_battery.{native,byte}>: pkg_lwt.unix
+<src/tools/krobot_plot_battery.{native,byte}>: pkg_lwt.syntax
+<src/tools/krobot_plot_battery.{native,byte}>: pkg_lwt.react
+<src/tools/krobot_plot_battery.{native,byte}>: pkg_lwt.glib
+<src/tools/krobot_plot_battery.{native,byte}>: pkg_cairo.lablgtk2
+# Executable krobot-ax12-clean
# Executable krobot-sharps
<src/tools/krobot_sharps.{native,byte}>: use_krobot
-<src/tools/krobot_sharps.{native,byte}>: pkg_lwt.syntax
<src/tools/krobot_sharps.{native,byte}>: pkg_lwt.unix
+<src/tools/krobot_sharps.{native,byte}>: pkg_lwt.syntax
<src/tools/krobot_sharps.{native,byte}>: pkg_lwt.react
-# Executable krobot-vm
-<src/tools/krobot_vm.{native,byte}>: use_krobot
-<src/tools/krobot_vm.{native,byte}>: pkg_lwt.syntax
-<src/tools/krobot_vm.{native,byte}>: pkg_lwt.unix
-<src/tools/krobot_vm.{native,byte}>: pkg_lwt.react
-# Executable krobot-ia
-<src/tools/krobot_ia.{native,byte}>: use_krobot
-<src/tools/krobot_ia.{native,byte}>: pkg_lwt.syntax
-<src/tools/krobot_ia.{native,byte}>: pkg_lwt.unix
-<src/tools/krobot_ia.{native,byte}>: pkg_lwt.react
-# Executable krobot-homologation
-<src/tools/krobot_homologation.{native,byte}>: use_krobot
-<src/tools/krobot_homologation.{native,byte}>: pkg_lwt.syntax
-<src/tools/krobot_homologation.{native,byte}>: pkg_lwt.unix
-<src/tools/krobot_homologation.{native,byte}>: pkg_lwt.react
-# Executable krobot-can-display
-<src/tools/krobot_can_display.{native,byte}>: use_krobot-can
-<src/tools/krobot_can_display.{native,byte}>: use_krobot
-<src/tools/krobot_can_display.{native,byte}>: pkg_lwt.syntax
-<src/tools/krobot_can_display.{native,byte}>: pkg_sexplib
-<src/tools/krobot_can_display.{native,byte}>: pkg_sexplib.syntax
-<src/tools/krobot_can_display.{native,byte}>: pkg_cairo.lablgtk2
-<src/tools/krobot_can_display.{native,byte}>: pkg_lwt.glib
-<src/tools/krobot_can_display.{native,byte}>: pkg_lwt.unix
-<src/tools/krobot_can_display.{native,byte}>: pkg_bitstring
-<src/tools/krobot_can_display.{native,byte}>: pkg_lwt.react
-<src/tools/*.ml{,i}>: pkg_cairo.lablgtk2
-<src/tools/*.ml{,i}>: pkg_lwt.glib
-# Executable krobot-ax12-control
-<src/tools/krobot_ax12_control.{native,byte}>: use_krobot-can
-<src/tools/krobot_ax12_control.{native,byte}>: use_krobot
-<src/tools/krobot_ax12_control.{native,byte}>: pkg_lwt.syntax
-<src/tools/krobot_ax12_control.{native,byte}>: pkg_sexplib
-<src/tools/krobot_ax12_control.{native,byte}>: pkg_sexplib.syntax
-<src/tools/krobot_ax12_control.{native,byte}>: pkg_lwt.unix
-<src/tools/krobot_ax12_control.{native,byte}>: pkg_bitstring
-<src/tools/krobot_ax12_control.{native,byte}>: pkg_lwt.react
# Executable krobot-dump-ax12
<src/tools/krobot_dump_ax12.{native,byte}>: use_krobot-can
<src/tools/krobot_dump_ax12.{native,byte}>: use_krobot
-<src/tools/krobot_dump_ax12.{native,byte}>: pkg_lwt.syntax
-<src/tools/krobot_dump_ax12.{native,byte}>: pkg_sexplib
<src/tools/krobot_dump_ax12.{native,byte}>: pkg_sexplib.syntax
+<src/tools/krobot_dump_ax12.{native,byte}>: pkg_sexplib
<src/tools/krobot_dump_ax12.{native,byte}>: pkg_lwt.unix
-<src/tools/krobot_dump_ax12.{native,byte}>: pkg_bitstring
+<src/tools/krobot_dump_ax12.{native,byte}>: pkg_lwt.syntax
<src/tools/krobot_dump_ax12.{native,byte}>: pkg_lwt.react
-<src/tools/*.ml{,i}>: use_krobot-can
-<src/tools/*.ml{,i}>: pkg_sexplib
-<src/tools/*.ml{,i}>: pkg_sexplib.syntax
-<src/tools/*.ml{,i}>: pkg_bitstring
-# Executable krobot-ax12-clean
-# Executable krobot-see-coin
-<src/tools/krobot_see_coin.{native,byte}>: use_krobot
-<src/tools/krobot_see_coin.{native,byte}>: pkg_lwt.syntax
-<src/tools/krobot_see_coin.{native,byte}>: pkg_cv
-<src/tools/krobot_see_coin.{native,byte}>: pkg_lwt.unix
-<src/tools/krobot_see_coin.{native,byte}>: pkg_lwt.react
-<src/tools/*.ml{,i}>: pkg_cv
-# Executable krobot-stop-beacon
-<src/tools/krobot_stop_beacon.{native,byte}>: use_krobot
-<src/tools/krobot_stop_beacon.{native,byte}>: pkg_lwt.syntax
-<src/tools/krobot_stop_beacon.{native,byte}>: pkg_lwt.unix
-<src/tools/krobot_stop_beacon.{native,byte}>: pkg_lwt.react
-# Executable krobot-joystick
-<src/tools/krobot_joystick.{native,byte}>: use_krobot
-<src/tools/krobot_joystick.{native,byte}>: pkg_lwt.syntax
-<src/tools/krobot_joystick.{native,byte}>: pkg_sdl
-<src/tools/krobot_joystick.{native,byte}>: pkg_lwt.unix
-<src/tools/krobot_joystick.{native,byte}>: pkg_lwt.react
-<src/tools/*.ml{,i}>: pkg_sdl
-# Executable krobot-pet
-<src/tools/krobot_pet.{native,byte}>: use_krobot
-<src/tools/krobot_pet.{native,byte}>: pkg_lwt.syntax
-<src/tools/krobot_pet.{native,byte}>: pkg_lwt.unix
-<src/tools/krobot_pet.{native,byte}>: pkg_lwt.react
-# Executable krobot-urg
-<src/tools/krobot_urg.{native,byte}>: use_krobot
-<src/tools/krobot_urg.{native,byte}>: pkg_lwt.syntax
-<src/tools/krobot_urg.{native,byte}>: pkg_urg
-<src/tools/krobot_urg.{native,byte}>: pkg_lwt.preemptive
-<src/tools/krobot_urg.{native,byte}>: pkg_threads
-<src/tools/krobot_urg.{native,byte}>: pkg_lwt.unix
-<src/tools/krobot_urg.{native,byte}>: pkg_lwt.react
-<src/tools/*.ml{,i}>: use_krobot
-<src/tools/*.ml{,i}>: pkg_lwt.syntax
-<src/tools/*.ml{,i}>: pkg_urg
-<src/tools/*.ml{,i}>: pkg_lwt.preemptive
-<src/tools/*.ml{,i}>: pkg_threads
-<src/tools/*.ml{,i}>: pkg_lwt.unix
-<src/tools/*.ml{,i}>: pkg_lwt.react
+<src/tools/krobot_dump_ax12.{native,byte}>: pkg_bitstring
+# Executable krobot-mc-simulator
+<src/tools/krobot_mc_simulator.{native,byte}>: use_krobot
+<src/tools/krobot_mc_simulator.{native,byte}>: pkg_lwt.unix
+<src/tools/krobot_mc_simulator.{native,byte}>: pkg_lwt.syntax
+<src/tools/krobot_mc_simulator.{native,byte}>: pkg_lwt.react
+# Executable krobot-replay
+<src/tools/krobot_replay.{native,byte}>: use_krobot
+<src/tools/krobot_replay.{native,byte}>: pkg_lwt.unix
+<src/tools/krobot_replay.{native,byte}>: pkg_lwt.syntax
+<src/tools/krobot_replay.{native,byte}>: pkg_lwt.react
# Executable dump-can
<examples/dump_can.{native,byte}>: use_krobot-can
<examples/dump_can.{native,byte}>: use_krobot
-<examples/dump_can.{native,byte}>: pkg_lwt.syntax
-<examples/dump_can.{native,byte}>: pkg_sexplib
<examples/dump_can.{native,byte}>: pkg_sexplib.syntax
+<examples/dump_can.{native,byte}>: pkg_sexplib
<examples/dump_can.{native,byte}>: pkg_lwt.unix
-<examples/dump_can.{native,byte}>: pkg_bitstring
+<examples/dump_can.{native,byte}>: pkg_lwt.syntax
<examples/dump_can.{native,byte}>: pkg_lwt.react
-# Executable send-can
-<examples/send_can.{native,byte}>: use_krobot-can
-<examples/send_can.{native,byte}>: use_krobot
-<examples/send_can.{native,byte}>: pkg_lwt.syntax
-<examples/send_can.{native,byte}>: pkg_sexplib
-<examples/send_can.{native,byte}>: pkg_sexplib.syntax
-<examples/send_can.{native,byte}>: pkg_lwt.unix
-<examples/send_can.{native,byte}>: pkg_bitstring
-<examples/send_can.{native,byte}>: pkg_lwt.react
+<examples/dump_can.{native,byte}>: pkg_bitstring
<examples/*.ml{,i}>: use_krobot-can
<examples/*.ml{,i}>: use_krobot
-<examples/*.ml{,i}>: pkg_lwt.syntax
-<examples/*.ml{,i}>: pkg_sexplib
<examples/*.ml{,i}>: pkg_sexplib.syntax
+<examples/*.ml{,i}>: pkg_sexplib
<examples/*.ml{,i}>: pkg_lwt.unix
-<examples/*.ml{,i}>: pkg_bitstring
+<examples/*.ml{,i}>: pkg_lwt.syntax
<examples/*.ml{,i}>: pkg_lwt.react
+<examples/*.ml{,i}>: pkg_bitstring
+# Executable krobot-can-display
+<src/tools/krobot_can_display.{native,byte}>: use_krobot-can
+<src/tools/krobot_can_display.{native,byte}>: use_krobot
+<src/tools/krobot_can_display.{native,byte}>: pkg_sexplib.syntax
+<src/tools/krobot_can_display.{native,byte}>: pkg_sexplib
+<src/tools/krobot_can_display.{native,byte}>: pkg_lwt.unix
+<src/tools/krobot_can_display.{native,byte}>: pkg_lwt.syntax
+<src/tools/krobot_can_display.{native,byte}>: pkg_lwt.react
+<src/tools/krobot_can_display.{native,byte}>: pkg_lwt.glib
+<src/tools/krobot_can_display.{native,byte}>: pkg_cairo.lablgtk2
+<src/tools/krobot_can_display.{native,byte}>: pkg_bitstring
+<src/tools/*.ml{,i}>: use_krobot-can
+<src/tools/*.ml{,i}>: pkg_sexplib.syntax
+<src/tools/*.ml{,i}>: pkg_sexplib
+<src/tools/*.ml{,i}>: pkg_bitstring
+# Executable krobot-planner
+<src/tools/krobot_planner.{native,byte}>: use_krobot
+<src/tools/krobot_planner.{native,byte}>: pkg_lwt.unix
+<src/tools/krobot_planner.{native,byte}>: pkg_lwt.syntax
+<src/tools/krobot_planner.{native,byte}>: pkg_lwt.react
+# Executable krobot-record
+<src/tools/krobot_record.{native,byte}>: use_krobot
+<src/tools/krobot_record.{native,byte}>: pkg_threads
+<src/tools/krobot_record.{native,byte}>: pkg_lwt.unix
+<src/tools/krobot_record.{native,byte}>: pkg_lwt.syntax
+<src/tools/krobot_record.{native,byte}>: pkg_lwt.react
+<src/tools/*.ml{,i}>: pkg_threads
+# Executable krobot-plot
+<src/tools/krobot_plot.{native,byte}>: use_krobot
+<src/tools/krobot_plot.{native,byte}>: pkg_lwt.unix
+<src/tools/krobot_plot.{native,byte}>: pkg_lwt.syntax
+<src/tools/krobot_plot.{native,byte}>: pkg_lwt.react
+<src/tools/krobot_plot.{native,byte}>: pkg_lwt.glib
+<src/tools/krobot_plot.{native,byte}>: pkg_cairo.lablgtk2
+<src/tools/*.ml{,i}>: pkg_lwt.glib
+<src/tools/*.ml{,i}>: pkg_cairo.lablgtk2
+# Executable krobot-homologation
+<src/tools/krobot_homologation.{native,byte}>: use_krobot
+<src/tools/krobot_homologation.{native,byte}>: pkg_lwt.unix
+<src/tools/krobot_homologation.{native,byte}>: pkg_lwt.syntax
+<src/tools/krobot_homologation.{native,byte}>: pkg_lwt.react
+# Executable krobot-pet
+<src/tools/krobot_pet.{native,byte}>: use_krobot
+<src/tools/krobot_pet.{native,byte}>: pkg_lwt.unix
+<src/tools/krobot_pet.{native,byte}>: pkg_lwt.syntax
+<src/tools/krobot_pet.{native,byte}>: pkg_lwt.react
+# Executable krobot-simulator
+<src/tools/krobot_simulator.{native,byte}>: use_krobot
+<src/tools/krobot_simulator.{native,byte}>: pkg_lwt.unix
+<src/tools/krobot_simulator.{native,byte}>: pkg_lwt.syntax
+<src/tools/krobot_simulator.{native,byte}>: pkg_lwt.react
+<src/tools/*.ml{,i}>: use_krobot
+<src/tools/*.ml{,i}>: pkg_lwt.unix
+<src/tools/*.ml{,i}>: pkg_lwt.syntax
+<src/tools/*.ml{,i}>: pkg_lwt.react
# OASIS_STOP
diff --git a/info/control2011/configure b/info/control2011/configure
index 97ed012..6719c7c 100755
--- a/info/control2011/configure
+++ b/info/control2011/configure
@@ -1,27 +1,8 @@
#!/bin/sh
# OASIS_START
-# DO NOT EDIT (digest: 425187ed8bfdbdd207fd76392dd243a7)
+# DO NOT EDIT (digest: ed33e59fe00e48bc31edf413bbc8b8d6)
set -e
-FST=true
-for i in "$@"; do
- if $FST; then
- set --
- FST=false
- fi
-
- case $i in
- --*=*)
- ARG=${i%%=*}
- VAL=${i##*=}
- set -- "$@" "$ARG" "$VAL"
- ;;
- *)
- set -- "$@" "$i"
- ;;
- esac
-done
-
-ocaml setup.ml -configure "$@"
+ocaml setup.ml -configure $*
# OASIS_STOP
diff --git a/info/control2011/myocamlbuild.ml b/info/control2011/myocamlbuild.ml
index ebd412e..f3e5e22 100644
--- a/info/control2011/myocamlbuild.ml
+++ b/info/control2011/myocamlbuild.ml
@@ -1,39 +1,39 @@
(* OASIS_START *)
-(* DO NOT EDIT (digest: d2ba10e1d203a414b96ccf471790ac22) *)
+(* DO NOT EDIT (digest: 68c025d8d1050b1e4fcf5d0e43496e2b) *)
module OASISGettext = struct
-(* # 21 "/home/chambart/.opam/base/build/oasis.0.3.0/src/oasis/OASISGettext.ml" *)
-
- let ns_ str =
+# 21 "/tmp/buildd/oasis-0.2.0/src/oasis/OASISGettext.ml"
+
+ let ns_ str =
str
-
- let s_ str =
+
+ let s_ str =
str
-
+
let f_ (str : ('a, 'b, 'c, 'd) format4) =
str
-
+
let fn_ fmt1 fmt2 n =
if n = 1 then
fmt1^^""
else
fmt2^^""
-
- let init =
+
+ let init =
[]
-
+
end
module OASISExpr = struct
-(* # 21 "/home/chambart/.opam/base/build/oasis.0.3.0/src/oasis/OASISExpr.ml" *)
-
-
-
+# 21 "/tmp/buildd/oasis-0.2.0/src/oasis/OASISExpr.ml"
+
+
+
open OASISGettext
-
+
type test = string
-
+
type flag = string
-
+
type t =
| EBool of bool
| ENot of t
@@ -42,31 +42,31 @@ module OASISExpr = struct
| EFlag of flag
| ETest of test * string
-
+
type 'a choices = (t * 'a) list
-
+
let eval var_get t =
- let rec eval' =
+ let rec eval' =
function
| EBool b ->
b
-
- | ENot e ->
+
+ | ENot e ->
not (eval' e)
-
+
| EAnd (e1, e2) ->
(eval' e1) && (eval' e2)
-
- | EOr (e1, e2) ->
+
+ | EOr (e1, e2) ->
(eval' e1) || (eval' e2)
-
+
| EFlag nm ->
let v =
var_get nm
in
assert(v = "true" || v = "false");
(v = "true")
-
+
| ETest (nm, vl) ->
let v =
var_get nm
@@ -74,21 +74,21 @@ module OASISExpr = struct
(v = vl)
in
eval' t
-
+
let choose ?printer ?name var_get lst =
- let rec choose_aux =
+ let rec choose_aux =
function
| (cond, vl) :: tl ->
- if eval var_get cond then
- vl
+ if eval var_get cond then
+ vl
else
choose_aux tl
| [] ->
- let str_lst =
+ let str_lst =
if lst = [] then
s_ "<empty>"
else
- String.concat
+ String.concat
(s_ ", ")
(List.map
(fun (cond, vl) ->
@@ -97,10 +97,10 @@ module OASISExpr = struct
| None -> s_ "<no printer>")
lst)
in
- match name with
+ match name with
| Some nm ->
failwith
- (Printf.sprintf
+ (Printf.sprintf
(f_ "No result for the choice list '%s': %s")
nm str_lst)
| None ->
@@ -110,23 +110,22 @@ module OASISExpr = struct
str_lst)
in
choose_aux (List.rev lst)
-
+
end
-# 117 "myocamlbuild.ml"
module BaseEnvLight = struct
-(* # 21 "/home/chambart/.opam/base/build/oasis.0.3.0/src/base/BaseEnvLight.ml" *)
-
+# 21 "/tmp/buildd/oasis-0.2.0/src/base/BaseEnvLight.ml"
+
module MapString = Map.Make(String)
-
+
type t = string MapString.t
-
+
let default_filename =
- Filename.concat
+ Filename.concat
(Sys.getcwd ())
"setup.data"
-
+
let load ?(allow_empty=false) ?(filename=default_filename) () =
if Sys.file_exists filename then
begin
@@ -139,23 +138,23 @@ module BaseEnvLight = struct
let line =
ref 1
in
- let st_line =
+ let st_line =
Stream.from
(fun _ ->
try
- match Stream.next st with
+ match Stream.next st with
| '\n' -> incr line; Some '\n'
| c -> Some c
with Stream.Failure -> None)
in
- let lexer =
+ let lexer =
Genlex.make_lexer ["="] st_line
in
let rec read_file mp =
- match Stream.npeek 3 lexer with
+ match Stream.npeek 3 lexer with
| [Genlex.Ident nm; Genlex.Kwd "="; Genlex.String value] ->
- Stream.junk lexer;
- Stream.junk lexer;
+ Stream.junk lexer;
+ Stream.junk lexer;
Stream.junk lexer;
read_file (MapString.add nm value mp)
| [] ->
@@ -178,44 +177,43 @@ module BaseEnvLight = struct
end
else
begin
- failwith
- (Printf.sprintf
+ failwith
+ (Printf.sprintf
"Unable to load environment, the file '%s' doesn't exist."
filename)
end
-
+
let var_get name env =
let rec var_expand str =
let buff =
Buffer.create ((String.length str) * 2)
in
- Buffer.add_substitute
+ Buffer.add_substitute
buff
- (fun var ->
- try
+ (fun var ->
+ try
var_expand (MapString.find var env)
with Not_found ->
- failwith
- (Printf.sprintf
+ failwith
+ (Printf.sprintf
"No variable %s defined when trying to expand %S."
- var
+ var
str))
str;
Buffer.contents buff
in
var_expand (MapString.find name env)
-
- let var_choose lst env =
+
+ let var_choose lst env =
OASISExpr.choose
(fun nm -> var_get nm env)
lst
end
-# 215 "myocamlbuild.ml"
module MyOCamlbuildFindlib = struct
-(* # 21 "/home/chambart/.opam/base/build/oasis.0.3.0/src/plugins/ocamlbuild/MyOCamlbuildFindlib.ml" *)
-
+# 21 "/tmp/buildd/oasis-0.2.0/src/plugins/ocamlbuild/MyOCamlbuildFindlib.ml"
+
(** OCamlbuild extension, copied from
* http://brion.inria.fr/gallium/index.php/Using_ocamlfind_with_ocamlbuild
* by N. Pouillard and others
@@ -225,14 +223,14 @@ module MyOCamlbuildFindlib = struct
* Modified by Sylvain Le Gall
*)
open Ocamlbuild_plugin
-
+
(* these functions are not really officially exported *)
let run_and_read =
Ocamlbuild_pack.My_unix.run_and_read
-
+
let blank_sep_strings =
Ocamlbuild_pack.Lexers.blank_sep_strings
-
+
let split s ch =
let x =
ref []
@@ -247,24 +245,24 @@ module MyOCamlbuildFindlib = struct
try
go s
with Not_found -> !x
-
+
let split_nl s = split s '\n'
-
+
let before_space s =
try
String.before s (String.index s ' ')
with Not_found -> s
-
+
(* this lists all supported packages *)
let find_packages () =
List.map before_space (split_nl & run_and_read "ocamlfind list")
-
+
(* this is supposed to list available syntaxes, but I don't know how to do it. *)
let find_syntaxes () = ["camlp4o"; "camlp4r"]
-
+
(* ocamlfind command *)
let ocamlfind x = S[A"ocamlfind"; x]
-
+
let dispatch =
function
| Before_options ->
@@ -294,7 +292,7 @@ module MyOCamlbuildFindlib = struct
flag ["ocaml"; "infer_interface"; "pkg_"^pkg] & S[A"-package"; A pkg];
end
(find_packages ());
-
+
(* Like -package but for extensions syntax. Morover -syntax is useless
* when linking. *)
List.iter begin fun syntax ->
@@ -303,7 +301,7 @@ module MyOCamlbuildFindlib = struct
flag ["ocaml"; "doc"; "syntax_"^syntax] & S[A"-syntax"; A syntax];
flag ["ocaml"; "infer_interface"; "syntax_"^syntax] & S[A"-syntax"; A syntax];
end (find_syntaxes ());
-
+
(* The default "thread" tag is not compatible with ocamlfind.
* Indeed, the default rules add the "threads.cma" or "threads.cmxa"
* options when using this tag. When using the "-linkpkg" option with
@@ -313,61 +311,49 @@ module MyOCamlbuildFindlib = struct
* the "threads" package using the previous plugin.
*)
flag ["ocaml"; "pkg_threads"; "compile"] (S[A "-thread"]);
- flag ["ocaml"; "pkg_threads"; "doc"] (S[A "-I"; A "+threads"]);
flag ["ocaml"; "pkg_threads"; "link"] (S[A "-thread"]);
flag ["ocaml"; "pkg_threads"; "infer_interface"] (S[A "-thread"])
-
+
| _ ->
()
-
+
end
module MyOCamlbuildBase = struct
-(* # 21 "/home/chambart/.opam/base/build/oasis.0.3.0/src/plugins/ocamlbuild/MyOCamlbuildBase.ml" *)
-
+# 21 "/tmp/buildd/oasis-0.2.0/src/plugins/ocamlbuild/MyOCamlbuildBase.ml"
+
(** Base functions for writing myocamlbuild.ml
@author Sylvain Le Gall
*)
-
-
-
+
+
+
open Ocamlbuild_plugin
- module OC = Ocamlbuild_pack.Ocaml_compiler
-
+
type dir = string
type file = string
type name = string
type tag = string
-
-(* # 56 "/home/chambart/.opam/base/build/oasis.0.3.0/src/plugins/ocamlbuild/MyOCamlbuildBase.ml" *)
-
+
+# 55 "/tmp/buildd/oasis-0.2.0/src/plugins/ocamlbuild/MyOCamlbuildBase.ml"
+
type t =
{
lib_ocaml: (name * dir list) list;
lib_c: (name * dir * file list) list;
flags: (tag list * (spec OASISExpr.choices)) list;
- (* Replace the 'dir: include' from _tags by a precise interdepends in
- * directory.
- *)
- includes: (dir * dir list) list;
}
-
+
let env_filename =
Pathname.basename
BaseEnvLight.default_filename
-
+
let dispatch_combine lst =
fun e ->
List.iter
(fun dispatch -> dispatch e)
lst
-
- let tag_libstubs nm =
- "use_lib"^nm^"_stubs"
-
- let nm_libstubs nm =
- nm^"_stubs"
-
+
let dispatch t e =
let env =
BaseEnvLight.load
@@ -394,64 +380,53 @@ module MyOCamlbuildBase = struct
Options.ext_lib, "ext_lib";
Options.ext_dll, "ext_dll";
]
-
+
| After_rules ->
(* Declare OCaml libraries *)
List.iter
(function
- | nm, [] ->
- ocaml_lib nm
- | nm, dir :: tl ->
- ocaml_lib ~dir:dir (dir^"/"^nm);
+ | lib, [] ->
+ ocaml_lib lib;
+ | lib, dir :: tl ->
+ ocaml_lib ~dir:dir lib;
List.iter
(fun dir ->
- List.iter
- (fun str ->
- flag ["ocaml"; "use_"^nm; str] (S[A"-I"; P dir]))
- ["compile"; "infer_interface"; "doc"])
+ flag
+ ["ocaml"; "use_"^lib; "compile"]
+ (S[A"-I"; P dir]))
tl)
t.lib_ocaml;
-
- (* Declare directories dependencies, replace "include" in _tags. *)
- List.iter
- (fun (dir, include_dirs) ->
- Pathname.define_context dir include_dirs)
- t.includes;
-
+
(* Declare C libraries *)
List.iter
(fun (lib, dir, headers) ->
(* Handle C part of library *)
- flag ["link"; "library"; "ocaml"; "byte"; tag_libstubs lib]
- (S[A"-dllib"; A("-l"^(nm_libstubs lib)); A"-cclib";
- A("-l"^(nm_libstubs lib))]);
-
- flag ["link"; "library"; "ocaml"; "native"; tag_libstubs lib]
- (S[A"-cclib"; A("-l"^(nm_libstubs lib))]);
+ flag ["link"; "library"; "ocaml"; "byte"; "use_lib"^lib]
+ (S[A"-dllib"; A("-l"^lib); A"-cclib"; A("-l"^lib)]);
+
+ flag ["link"; "library"; "ocaml"; "native"; "use_lib"^lib]
+ (S[A"-cclib"; A("-l"^lib)]);
- flag ["link"; "program"; "ocaml"; "byte"; tag_libstubs lib]
- (S[A"-dllib"; A("dll"^(nm_libstubs lib))]);
-
+ flag ["link"; "program"; "ocaml"; "byte"; "use_lib"^lib]
+ (S[A"-dllib"; A("dll"^lib)]);
+
(* When ocaml link something that use the C library, then one
need that file to be up to date.
*)
- dep ["link"; "ocaml"; "program"; tag_libstubs lib]
- [dir/"lib"^(nm_libstubs lib)^"."^(!Options.ext_lib)];
-
- dep ["compile"; "ocaml"; "program"; tag_libstubs lib]
- [dir/"lib"^(nm_libstubs lib)^"."^(!Options.ext_lib)];
-
+ dep ["link"; "ocaml"; "use_lib"^lib]
+ [dir/"lib"^lib^"."^(!Options.ext_lib)];
+
(* TODO: be more specific about what depends on headers *)
(* Depends on .h files *)
dep ["compile"; "c"]
headers;
-
+
(* Setup search path for lib *)
flag ["link"; "ocaml"; "use_"^lib]
(S[A"-I"; P(dir)]);
)
t.lib_c;
-
+
(* Add flags *)
List.iter
(fun (tags, cond_specs) ->
@@ -462,38 +437,29 @@ module MyOCamlbuildBase = struct
t.flags
| _ ->
()
-
+
let dispatch_default t =
dispatch_combine
[
dispatch t;
MyOCamlbuildFindlib.dispatch;
]
-
+
end
-# 476 "myocamlbuild.ml"
open Ocamlbuild_plugin;;
let package_default =
{
MyOCamlbuildBase.lib_ocaml =
- [("krobot", ["src/lib"]); ("krobot-can", ["src/can"])];
+ [("src/lib/krobot", ["src/lib"]); ("src/can/krobot-can", ["src/can"])];
lib_c = [("krobot", "src/lib", []); ("krobot-can", "src/can", [])];
flags = [];
- includes =
- [
- ("src/tools", ["src/can"; "src/lib"]);
- ("src/driver", ["src/can"; "src/lib"]);
- ("src/can", ["src/lib"]);
- ("examples", ["src/can"])
- ];
}
;;
let dispatch_default = MyOCamlbuildBase.dispatch_default package_default;;
-# 497 "myocamlbuild.ml"
(* OASIS_STOP *)
open Ocamlbuild_plugin
diff --git a/info/control2011/setup.ml b/info/control2011/setup.ml
index 0e2f232..18934ac 100644
--- a/info/control2011/setup.ml
+++ b/info/control2011/setup.ml
@@ -8,363 +8,288 @@
*)
(* OASIS_START *)
-(* DO NOT EDIT (digest: 93e97b5dfbc11c54162cfd7f571db7a3) *)
+(* DO NOT EDIT (digest: 868e08593bf53c96a1f79923d193b872) *)
(*
- Regenerated by OASIS v0.3.0
+ Regenerated by OASIS v0.2.0
Visit http://oasis.forge.ocamlcore.org for more information and
documentation about functions used in this file.
*)
module OASISGettext = struct
-(* # 21 "/home/chambart/.opam/base/build/oasis.0.3.0/src/oasis/OASISGettext.ml" *)
-
- let ns_ str =
+# 21 "/tmp/buildd/oasis-0.2.0/src/oasis/OASISGettext.ml"
+
+ let ns_ str =
str
-
- let s_ str =
+
+ let s_ str =
str
-
+
let f_ (str : ('a, 'b, 'c, 'd) format4) =
str
-
+
let fn_ fmt1 fmt2 n =
if n = 1 then
fmt1^^""
else
fmt2^^""
-
- let init =
+
+ let init =
[]
-
+
end
module OASISContext = struct
-(* # 21 "/home/chambart/.opam/base/build/oasis.0.3.0/src/oasis/OASISContext.ml" *)
-
- open OASISGettext
-
+# 21 "/tmp/buildd/oasis-0.2.0/src/oasis/OASISContext.ml"
+
+ open OASISGettext
+
type level =
[ `Debug
- | `Info
+ | `Info
| `Warning
| `Error]
-
+
type t =
{
- quiet: bool;
- info: bool;
- debug: bool;
- ignore_plugins: bool;
- ignore_unknown_fields: bool;
- printf: level -> string -> unit;
+ verbose: bool;
+ debug: bool;
+ ignore_plugins: bool;
+ printf: level -> string -> unit;
}
-
- let printf lvl str =
- let beg =
- match lvl with
+
+ let printf lvl str =
+ let beg =
+ match lvl with
| `Error -> s_ "E: "
| `Warning -> s_ "W: "
| `Info -> s_ "I: "
| `Debug -> s_ "D: "
in
- prerr_endline (beg^str)
-
+ match lvl with
+ | `Error ->
+ prerr_endline (beg^str)
+ | _ ->
+ print_endline (beg^str)
+
let default =
- ref
+ ref
{
- quiet = false;
- info = false;
- debug = false;
- ignore_plugins = false;
- ignore_unknown_fields = false;
- printf = printf;
+ verbose = true;
+ debug = false;
+ ignore_plugins = false;
+ printf = printf;
}
-
- let quiet =
- {!default with quiet = true}
-
-
+
+ let quiet =
+ {!default with
+ verbose = false;
+ debug = false;
+ }
+
+
let args () =
["-quiet",
- Arg.Unit (fun () -> default := {!default with quiet = true}),
+ Arg.Unit (fun () -> default := {!default with verbose = false}),
(s_ " Run quietly");
-
- "-info",
- Arg.Unit (fun () -> default := {!default with info = true}),
- (s_ " Display information message");
-
-
+
"-debug",
Arg.Unit (fun () -> default := {!default with debug = true}),
(s_ " Output debug message")]
end
-module OASISString = struct
-(* # 1 "/home/chambart/.opam/base/build/oasis.0.3.0/src/oasis/OASISString.ml" *)
-
-
-
- (** Various string utilities.
-
- Mostly inspired by extlib and batteries ExtString and BatString libraries.
-
- @author Sylvain Le Gall
- *)
-
- let nsplitf str f =
- if str = "" then
- []
- else
- let buf = Buffer.create 13 in
- let lst = ref [] in
- let push () =
- lst := Buffer.contents buf :: !lst;
- Buffer.clear buf
- in
- let str_len = String.length str in
- for i = 0 to str_len - 1 do
- if f str.[i] then
- push ()
- else
- Buffer.add_char buf str.[i]
- done;
- push ();
- List.rev !lst
-
- (** [nsplit c s] Split the string [s] at char [c]. It doesn't include the
- separator.
- *)
- let nsplit str c =
- nsplitf str ((=) c)
-
- let find ~what ?(offset=0) str =
- let what_idx = ref 0 in
- let str_idx = ref offset in
- while !str_idx < String.length str &&
- !what_idx < String.length what do
- if str.[!str_idx] = what.[!what_idx] then
- incr what_idx
- else
- what_idx := 0;
- incr str_idx
- done;
- if !what_idx <> String.length what then
- raise Not_found
- else
- !str_idx - !what_idx
-
- let sub_start str len =
- let str_len = String.length str in
- if len >= str_len then
- ""
- else
- String.sub str len (str_len - len)
-
- let sub_end ?(offset=0) str len =
- let str_len = String.length str in
- if len >= str_len then
- ""
- else
- String.sub str 0 (str_len - len)
-
- let starts_with ~what ?(offset=0) str =
- let what_idx = ref 0 in
- let str_idx = ref offset in
- let ok = ref true in
- while !ok &&
- !str_idx < String.length str &&
- !what_idx < String.length what do
- if str.[!str_idx] = what.[!what_idx] then
- incr what_idx
- else
- ok := false;
- incr str_idx
- done;
- if !what_idx = String.length what then
- true
- else
- false
-
- let strip_starts_with ~what str =
- if starts_with ~what str then
- sub_start str (String.length what)
- else
- raise Not_found
-
- let ends_with ~what ?(offset=0) str =
- let what_idx = ref ((String.length what) - 1) in
- let str_idx = ref ((String.length str) - 1) in
- let ok = ref true in
- while !ok &&
- offset <= !str_idx &&
- 0 <= !what_idx do
- if str.[!str_idx] = what.[!what_idx] then
- decr what_idx
- else
- ok := false;
- decr str_idx
- done;
- if !what_idx = -1 then
- true
- else
- false
-
- let strip_ends_with ~what str =
- if ends_with ~what str then
- sub_end str (String.length what)
- else
- raise Not_found
-
- let replace_chars f s =
- let buf = String.make (String.length s) 'X' in
- for i = 0 to String.length s - 1 do
- buf.[i] <- f s.[i]
- done;
- buf
-
-end
-
module OASISUtils = struct
-(* # 21 "/home/chambart/.opam/base/build/oasis.0.3.0/src/oasis/OASISUtils.ml" *)
-
- open OASISGettext
-
+# 21 "/tmp/buildd/oasis-0.2.0/src/oasis/OASISUtils.ml"
+
module MapString = Map.Make(String)
-
+
let map_string_of_assoc assoc =
List.fold_left
(fun acc (k, v) -> MapString.add k v acc)
MapString.empty
assoc
-
+
module SetString = Set.Make(String)
-
+
let set_string_add_list st lst =
- List.fold_left
+ List.fold_left
(fun acc e -> SetString.add e acc)
st
lst
-
+
let set_string_of_list =
set_string_add_list
SetString.empty
-
-
- let compare_csl s1 s2 =
+
+
+ let compare_csl s1 s2 =
String.compare (String.lowercase s1) (String.lowercase s2)
-
- module HashStringCsl =
+
+ module HashStringCsl =
Hashtbl.Make
(struct
type t = string
-
- let equal s1 s2 =
+
+ let equal s1 s2 =
(String.lowercase s1) = (String.lowercase s2)
-
+
let hash s =
Hashtbl.hash (String.lowercase s)
end)
-
- let varname_of_string ?(hyphen='_') s =
+
+ let split sep str =
+ let str_len =
+ String.length str
+ in
+ let rec split_aux acc pos =
+ if pos < str_len then
+ (
+ let pos_sep =
+ try
+ String.index_from str pos sep
+ with Not_found ->
+ str_len
+ in
+ let part =
+ String.sub str pos (pos_sep - pos)
+ in
+ let acc =
+ part :: acc
+ in
+ if pos_sep >= str_len then
+ (
+ (* Nothing more in the string *)
+ List.rev acc
+ )
+ else if pos_sep = (str_len - 1) then
+ (
+ (* String end with a separator *)
+ List.rev ("" :: acc)
+ )
+ else
+ (
+ split_aux acc (pos_sep + 1)
+ )
+ )
+ else
+ (
+ List.rev acc
+ )
+ in
+ split_aux [] 0
+
+
+ let varname_of_string ?(hyphen='_') s =
if String.length s = 0 then
begin
- invalid_arg "varname_of_string"
+ invalid_arg "varname_of_string"
end
else
begin
- let buf =
- OASISString.replace_chars
+ let buff =
+ Buffer.create (String.length s)
+ in
+ (* Start with a _ if digit *)
+ if '0' <= s.[0] && s.[0] <= '9' then
+ Buffer.add_char buff hyphen;
+
+ String.iter
(fun c ->
- if ('a' <= c && c <= 'z')
- ||
- ('A' <= c && c <= 'Z')
- ||
+ if ('a' <= c && c <= 'z')
+ ||
+ ('A' <= c && c <= 'Z')
+ ||
('0' <= c && c <= '9') then
- c
+ Buffer.add_char buff c
else
- hyphen)
+ Buffer.add_char buff hyphen)
s;
- in
- let buf =
- (* Start with a _ if digit *)
- if '0' <= s.[0] && s.[0] <= '9' then
- "_"^buf
- else
- buf
- in
- String.lowercase buf
+
+ String.lowercase (Buffer.contents buff)
end
-
- let varname_concat ?(hyphen='_') p s =
- let what = String.make 1 hyphen in
- let p =
- try
- OASISString.strip_ends_with ~what p
- with Not_found ->
- p
+
+ let varname_concat ?(hyphen='_') p s =
+ let p =
+ let p_len =
+ String.length p
+ in
+ if p_len > 0 && p.[p_len - 1] = hyphen then
+ String.sub p 0 (p_len - 1)
+ else
+ p
in
- let s =
- try
- OASISString.strip_starts_with ~what s
- with Not_found ->
- s
+ let s =
+ let s_len =
+ String.length s
+ in
+ if s_len > 0 && s.[0] = hyphen then
+ String.sub s 1 (s_len - 1)
+ else
+ s
in
- p^what^s
-
-
- let is_varname str =
+ Printf.sprintf "%s%c%s" p hyphen s
+
+
+ let is_varname str =
str = varname_of_string str
-
- let failwithf fmt = Printf.ksprintf failwith fmt
-
+
+ let failwithf1 fmt a =
+ failwith (Printf.sprintf fmt a)
+
+ let failwithf2 fmt a b =
+ failwith (Printf.sprintf fmt a b)
+
+ let failwithf3 fmt a b c =
+ failwith (Printf.sprintf fmt a b c)
+
+ let failwithf4 fmt a b c d =
+ failwith (Printf.sprintf fmt a b c d)
+
+ let failwithf5 fmt a b c d e =
+ failwith (Printf.sprintf fmt a b c d e)
+
end
module PropList = struct
-(* # 21 "/home/chambart/.opam/base/build/oasis.0.3.0/src/oasis/PropList.ml" *)
-
+# 21 "/tmp/buildd/oasis-0.2.0/src/oasis/PropList.ml"
+
open OASISGettext
-
+
type name = string
-
- exception Not_set of name * string option
+
+ exception Not_set of name * string option
exception No_printer of name
exception Unknown_field of name * name
-
- let () =
- Printexc.register_printer
- (function
- | Not_set (nm, Some rsn) ->
- Some
- (Printf.sprintf (f_ "Field '%s' is not set: %s") nm rsn)
- | Not_set (nm, None) ->
- Some
- (Printf.sprintf (f_ "Field '%s' is not set") nm)
- | No_printer nm ->
- Some
- (Printf.sprintf (f_ "No default printer for value %s") nm)
- | Unknown_field (nm, schm) ->
- Some
- (Printf.sprintf (f_ "Field %s is not defined in schema %s") nm schm)
- | _ ->
- None)
-
+
+ let string_of_exception =
+ function
+ | Not_set (nm, Some rsn) ->
+ Printf.sprintf (f_ "Field '%s' is not set: %s") nm rsn
+ | Not_set (nm, None) ->
+ Printf.sprintf (f_ "Field '%s' is not set") nm
+ | No_printer nm ->
+ Printf.sprintf (f_ "No default printer for value %s") nm
+ | Unknown_field (nm, schm) ->
+ Printf.sprintf (f_ "Field %s is not defined in schema %s") nm schm
+ | e ->
+ raise e
+
module Data =
struct
-
- type t =
+
+ type t =
(name, unit -> unit) Hashtbl.t
-
+
let create () =
Hashtbl.create 13
-
+
let clear t =
Hashtbl.clear t
-
-(* # 71 "/home/chambart/.opam/base/build/oasis.0.3.0/src/oasis/PropList.ml" *)
+
+# 59 "/tmp/buildd/oasis-0.2.0/src/oasis/PropList.ml"
end
-
- module Schema =
+
+ module Schema =
struct
-
+
type ('ctxt, 'extra) value =
{
get: Data.t -> string;
@@ -372,7 +297,7 @@ module PropList = struct
help: (unit -> string) option;
extra: 'extra;
}
-
+
type ('ctxt, 'extra) t =
{
name: name;
@@ -380,81 +305,81 @@ module PropList = struct
order: name Queue.t;
name_norm: string -> string;
}
-
- let create ?(case_insensitive=false) nm =
+
+ let create ?(case_insensitive=false) nm =
{
name = nm;
fields = Hashtbl.create 13;
order = Queue.create ();
- name_norm =
- (if case_insensitive then
+ name_norm =
+ (if case_insensitive then
String.lowercase
else
fun s -> s);
}
-
- let add t nm set get extra help =
- let key =
+
+ let add t nm set get extra help =
+ let key =
t.name_norm nm
in
-
+
if Hashtbl.mem t.fields key then
failwith
- (Printf.sprintf
+ (Printf.sprintf
(f_ "Field '%s' is already defined in schema '%s'")
nm t.name);
- Hashtbl.add
- t.fields
- key
+ Hashtbl.add
+ t.fields
+ key
{
- set = set;
- get = get;
+ set = set;
+ get = get;
help = help;
extra = extra;
};
- Queue.add nm t.order
-
+ Queue.add nm t.order
+
let mem t nm =
- Hashtbl.mem t.fields nm
-
- let find t nm =
+ Hashtbl.mem t.fields nm
+
+ let find t nm =
try
Hashtbl.find t.fields (t.name_norm nm)
with Not_found ->
raise (Unknown_field (nm, t.name))
-
+
let get t data nm =
(find t nm).get data
-
+
let set t data nm ?context x =
- (find t nm).set
- data
- ?context
+ (find t nm).set
+ data
+ ?context
x
-
+
let fold f acc t =
- Queue.fold
+ Queue.fold
(fun acc k ->
let v =
find t k
in
f acc k v.extra v.help)
- acc
+ acc
t.order
-
+
let iter f t =
- fold
+ fold
(fun () -> f)
()
t
-
- let name t =
+
+ let name t =
t.name
end
-
+
module Field =
struct
-
+
type ('ctxt, 'value, 'extra) t =
{
set: Data.t -> ?context:'ctxt -> 'value -> unit;
@@ -464,52 +389,52 @@ module PropList = struct
help: (unit -> string) option;
extra: 'extra;
}
-
- let new_id =
+
+ let new_id =
let last_id =
ref 0
in
fun () -> incr last_id; !last_id
-
+
let create ?schema ?name ?parse ?print ?default ?update ?help extra =
(* Default value container *)
- let v =
- ref None
+ let v =
+ ref None
in
-
+
(* If name is not given, create unique one *)
- let nm =
- match name with
+ let nm =
+ match name with
| Some s -> s
| None -> Printf.sprintf "_anon_%d" (new_id ())
in
-
+
(* Last chance to get a value: the default *)
- let default () =
- match default with
+ let default () =
+ match default with
| Some d -> d
| None -> raise (Not_set (nm, Some (s_ "no default value")))
in
-
+
(* Get data *)
let get data =
(* Get value *)
- try
+ try
(Hashtbl.find data nm) ();
- match !v with
- | Some x -> x
+ match !v with
+ | Some x -> x
| None -> default ()
with Not_found ->
default ()
in
-
+
(* Set data *)
- let set data ?context x =
- let x =
- match update with
+ let set data ?context x =
+ let x =
+ match update with
| Some f ->
begin
- try
+ try
f ?context (get data) x
with Not_set _ ->
x
@@ -517,31 +442,31 @@ module PropList = struct
| None ->
x
in
- Hashtbl.replace
- data
- nm
- (fun () -> v := Some x)
+ Hashtbl.replace
+ data
+ nm
+ (fun () -> v := Some x)
in
-
+
(* Parse string value, if possible *)
let parse =
- match parse with
- | Some f ->
+ match parse with
+ | Some f ->
f
| None ->
fun ?context s ->
- failwith
- (Printf.sprintf
+ failwith
+ (Printf.sprintf
(f_ "Cannot parse field '%s' when setting value %S")
nm
s)
in
-
+
(* Set data, from string *)
let sets data ?context s =
set ?context data (parse ?context s)
in
-
+
(* Output value as string, if possible *)
let print =
match print with
@@ -550,20 +475,20 @@ module PropList = struct
| None ->
fun _ -> raise (No_printer nm)
in
-
+
(* Get data, as a string *)
let gets data =
print (get data)
in
-
- begin
- match schema with
+
+ begin
+ match schema with
| Some t ->
Schema.add t nm sets gets extra help
| None ->
()
end;
-
+
{
set = set;
get = get;
@@ -572,84 +497,94 @@ module PropList = struct
help = help;
extra = extra;
}
-
- let fset data t ?context x =
+
+ let fset data t ?context x =
t.set data ?context x
-
+
let fget data t =
t.get data
-
+
let fsets data t ?context s =
t.sets data ?context s
-
+
let fgets data t =
- t.gets data
-
+ t.gets data
+
end
-
+
module FieldRO =
struct
-
+
let create ?schema ?name ?parse ?print ?default ?update ?help extra =
- let fld =
+ let fld =
Field.create ?schema ?name ?parse ?print ?default ?update ?help extra
in
fun data -> Field.fget data fld
-
+
end
end
module OASISMessage = struct
-(* # 21 "/home/chambart/.opam/base/build/oasis.0.3.0/src/oasis/OASISMessage.ml" *)
-
-
+# 21 "/tmp/buildd/oasis-0.2.0/src/oasis/OASISMessage.ml"
+
+
open OASISGettext
open OASISContext
-
+
let generic_message ~ctxt lvl fmt =
- let cond =
- if ctxt.quiet then
- false
- else
- match lvl with
- | `Debug -> ctxt.debug
- | `Info -> ctxt.info
- | _ -> true
+ let cond =
+ match lvl with
+ | `Debug -> ctxt.debug
+ | _ -> ctxt.verbose
in
- Printf.ksprintf
- (fun str ->
+ Printf.ksprintf
+ (fun str ->
if cond then
begin
ctxt.printf lvl str
end)
fmt
-
+
let debug ~ctxt fmt =
ge...
[truncated message content] |
|
From: Pierre C. <Ba...@us...> - 2013-04-11 00:53:11
|
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 ce3ff750e287178ea2034d0a683371562d679e9e (commit)
via 33c5cf6e79111ec89d78e7e4b1659548665511e7 (commit)
from ea4f473d3e5947d777e4cecee84681d00961496c (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 ce3ff750e287178ea2034d0a683371562d679e9e
Author: Pierre Chambart <pie...@oc...>
Date: Thu Apr 11 02:52:55 2013 +0200
Change krobot_urg dump format
commit 33c5cf6e79111ec89d78e7e4b1659548665511e7
Author: Pierre Chambart <pie...@oc...>
Date: Thu Apr 11 02:48:21 2013 +0200
Change the transmition format for urg messages, add to the viewer
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/src/lib/krobot_bus.ml b/info/control2011/src/lib/krobot_bus.ml
index 34595f3..387f972 100644
--- a/info/control2011/src/lib/krobot_bus.ml
+++ b/info/control2011/src/lib/krobot_bus.ml
@@ -41,7 +41,8 @@ type message =
| Set_fake_beacons of vertice option * vertice option
| Collisions of Bezier.curve * (float * (vertice * float) option) list
| Coins of vertice list
- | Urg of int array
+ | Urg of vertice array
+ | Urg_lines of (vertice*vertice) array
| Beacon_raw of (int * int * int * int * int * int
* int * int * int * int * int)
@@ -151,6 +152,8 @@ let string_of_message = function
l))
| Urg distances ->
sprintf "Urg (many_points...)"
+ | Urg_lines lines ->
+ sprintf "Urg_lines (many_lines...)"
| Beacon_raw _ ->
sprintf "Raw beacon packet"
diff --git a/info/control2011/src/lib/krobot_bus.mli b/info/control2011/src/lib/krobot_bus.mli
index ad33c96..b04aac3 100644
--- a/info/control2011/src/lib/krobot_bus.mli
+++ b/info/control2011/src/lib/krobot_bus.mli
@@ -87,7 +87,8 @@ type message =
| Coins of vertice list
(** distances mesured by the URG (in millimeters) *)
- | Urg of int array
+ | Urg of vertice array
+ | Urg_lines of (vertice*vertice) array
| Beacon_raw of (int * int * int * int * int * int
* int * int * int * int * int)
diff --git a/info/control2011/src/lib/krobot_config.ml b/info/control2011/src/lib/krobot_config.ml
index 43189d7..127b63e 100644
--- a/info/control2011/src/lib/krobot_config.ml
+++ b/info/control2011/src/lib/krobot_config.ml
@@ -119,6 +119,7 @@ let initial_coins = []
(* 2.55, 0.3; *) ]
*)
+let urg_position = { x = 0.19; y = 0. }
let urg_angles =
[|-2.356194490192344836998472601408; -2.350058567040802071090865865699;
diff --git a/info/control2011/src/lib/krobot_config.mli b/info/control2011/src/lib/krobot_config.mli
index 6770e8b..159f59f 100644
--- a/info/control2011/src/lib/krobot_config.mli
+++ b/info/control2011/src/lib/krobot_config.mli
@@ -53,5 +53,7 @@ val fixed_obstacles : Krobot_geom.obj list
val initial_coins : Krobot_geom.vertice list
+val urg_position : Krobot_geom.vertice
+
val urg_angles : float array
(* angles (in radiant) for each index of urg messages *)
diff --git a/info/control2011/src/tools/krobot_urg.ml b/info/control2011/src/tools/krobot_urg.ml
index 6b1acaf..aecb465 100644
--- a/info/control2011/src/tools/krobot_urg.ml
+++ b/info/control2011/src/tools/krobot_urg.ml
@@ -14,6 +14,7 @@ open Lwt_react
open Lwt_preemptive
open Krobot_bus
open Krobot_message
+open Krobot_geom
let section = Lwt_log.Section.make "krobot(urg)"
@@ -21,21 +22,32 @@ let section = Lwt_log.Section.make "krobot(urg)"
| read/send loop |
+-----------------------------------------------------------------+ *)
-let to_array (b:Urg.point_data) =
+let scale = 0.001
+
+let convert_pos dist angle =
+ let x = float dist *. cos angle *. scale +. Krobot_config.urg_position.x in
+ let y = float dist *. sin angle *. scale +. Krobot_config.urg_position.y in
+ (x), (-. y) (* the urg is top down -> y is reversed *)
+
+let convert (b:Urg.point_data) =
let dim = Bigarray.Array1.dim b in
- let a = Array.create dim 0 in
+ let l = ref [] in
for i = 0 to dim - 1 do
- a.(i) <- Nativeint.to_int b.{i};
+ let data = Nativeint.to_int b.{i} in
+ if data > 0
+ then
+ let angle = Krobot_config.urg_angles.(i) in
+ let (x,y) = convert_pos data angle in
+ l := {x;y} :: !l
done;
- a
+ Array.of_list !l
let loop bus urg =
let rec aux () =
lwt _ = Lwt_preemptive.detach Urg_simple.get urg in
let time = Unix.gettimeofday () in
- let msg = Urg (to_array urg.Urg_simple.data) in
+ let msg = Urg (convert urg.Urg_simple.data) in
lwt () = Krobot_bus.send bus (time, msg) in
- lwt () = Lwt_log.info_f "send things" in
lwt () = Lwt_unix.sleep 0.01 in
aux () in
aux ()
@@ -57,31 +69,21 @@ let handle_message (timestamp, message) =
| _ ->
()
-let min_val = 10
-
-let less a b =
- if a < min_val
- then false
- else if b < min_val
- then true
- else a < b
-
-let extrema a cmp =
- let m = ref a.(0) in
- let pos = ref 0 in
- for i = 0 to Array.length a - 1 do
- let v = a.(i) in
- if cmp v !m
- then (m := v; pos := i)
- done;
- !m, !pos
+(* let print_pos l = *)
+(* Format.printf "[@[<1>@ "; *)
+(* Array.iter (fun {x;y} -> Format.printf "(%f,%f);@ " x y) l; *)
+(* Format.printf "@]]@." *)
+
+let print_pos ts l =
+ Printf.printf "%f " ts;
+ Array.iter (fun {x;y} -> Format.printf "%f %f " x y) l;
+ Printf.printf "\n%!"
let handle_listener (timestamp, message) =
match message with
| Urg data ->
- let min, pos_min = extrema data less in
- let rad_min = Krobot_config.urg_angles.(pos_min) in
- Lwt_io.printf "%i %f\n%!" min rad_min
+ print_pos timestamp data;
+ return ()
| _ -> Lwt.return ()
(* +-----------------------------------------------------------------+
diff --git a/info/control2011/src/tools/krobot_viewer.ml b/info/control2011/src/tools/krobot_viewer.ml
index 8ccb7b0..564a27c 100644
--- a/info/control2011/src/tools/krobot_viewer.ml
+++ b/info/control2011/src/tools/krobot_viewer.ml
@@ -61,6 +61,9 @@ type viewer = {
mutable collisions : (Bezier.curve * (float * (vertice * float) option) list) option;
(* A curve and a list of: [(curve_parameter, (center, radius))] *)
+
+ mutable urg : vertice array;
+ mutable urg_lines : (vertice*vertice) array;
}
(* +-----------------------------------------------------------------+
@@ -353,6 +356,25 @@ let draw viewer =
draw_beacon b1;
draw_beacon b2;
+ let draw_urg a =
+ Cairo.set_source_rgba ctx 0.5 0.5 0.5 0.5;
+ let aux {x;y} =
+ Cairo.arc ctx x y 0.01 0. (2. *. pi);
+ Cairo.fill ctx
+ in
+ Array.iter aux a in
+ draw_urg viewer.urg;
+
+ let draw_urg_lines a =
+ Cairo.set_source_rgba ctx 1. 0.5 1. 0.5;
+ let aux ({x=x1;y=y1},{x=x2;y=y2}) =
+ Cairo.move_to ctx x1 y1;
+ Cairo.line_to ctx x2 y2;
+ Cairo.stroke ctx
+ in
+ Array.iter aux a in
+ draw_urg_lines viewer.urg_lines;
+
(* Draw the path of the VM if any or the path of the planner if the
VM is not following a trajectory. *)
let path =
@@ -474,6 +496,37 @@ let set_beacons viewer x y =
Krobot_bus.Set_fake_beacons (b1, b2)))
(* +-----------------------------------------------------------------+
+ | Urg handling |
+ +-----------------------------------------------------------------+ *)
+
+let convert_pos dist angle =
+ let x = float dist *. cos angle *. 0.001 in
+ let y = float dist *. sin angle *. 0.001 in
+ { x ; y }
+
+let project_urg viewer pos =
+ (* TODO: put the real urg position rather than the robot position *)
+ let theta = viewer.state.theta in
+ let rot = rot_mat theta in
+ let f { x;y } =
+ let urg_pos = [| x; y; 1. |] in
+ let urg_pos = mult rot urg_pos in
+ let state_pos = Krobot_geom.translate viewer.state.pos
+ { vx = urg_pos.(0); vy = urg_pos.(1) } in
+ state_pos
+ in
+ Array.map f pos
+
+let project_urg_lines viewer lines =
+ let rot = rot_mat viewer.state.theta in
+ let f v =
+ let v = [|v.x;v.y;1.|] in
+ let v = mult rot v in
+ Krobot_geom.translate viewer.state.pos { vx = v.(0); vy = v.(1) }
+ in
+ Array.map (fun (v1,v2) -> f v1,f v2) lines
+
+(* +-----------------------------------------------------------------+
| Message handling |
+-----------------------------------------------------------------+ *)
@@ -605,6 +658,12 @@ let handle_message viewer (timestamp, message) =
viewer.collisions <- Some (curve, l);
queue_draw viewer
+ | Urg dist ->
+ viewer.urg <- project_urg viewer dist
+
+ | Urg_lines lines ->
+ viewer.urg_lines <- project_urg_lines viewer lines
+
| _ ->
()
@@ -657,6 +716,8 @@ lwt () =
motor_status = (false, false, false, false);
coins = Krobot_config.initial_coins;
collisions = None;
+ urg = [||];
+ urg_lines = [||];
} in
(* Handle messages. *)
hooks/post-receive
--
krobot
|
|
From: Pierre C. <Ba...@us...> - 2013-04-06 22:57:44
|
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 ea4f473d3e5947d777e4cecee84681d00961496c (commit)
via 08d02d817b9535f9634e2fd2c9bd64f0a1cc5b24 (commit)
from 53fc37f4d093087aa81f0d50668689421da71958 (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 ea4f473d3e5947d777e4cecee84681d00961496c
Author: Pierre Chambart <pie...@oc...>
Date: Sun Apr 7 00:56:10 2013 +0200
[camlcv] houghlines using type matrix
commit 08d02d817b9535f9634e2fd2c9bd64f0a1cc5b24
Author: Pierre Chambart <pie...@oc...>
Date: Sun Apr 7 00:54:54 2013 +0200
New beacon reader and transmitter
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/_oasis b/info/control2011/_oasis
index 91019c0..f90e326 100644
--- a/info/control2011/_oasis
+++ b/info/control2011/_oasis
@@ -312,6 +312,13 @@ Executable "krobot-urg"
MainIs: krobot_urg.ml
BuildDepends: krobot, urg, lwt.syntax, lwt.preemptive, threads
+Executable "krobot-beacon-reader"
+ Path: src/tools
+ Install: true
+ CompiledObject: best
+ MainIs: krobot_beacon_reader.ml
+ BuildDepends: krobot, lwt.syntax, lwt.unix
+
# +-------------------------------------------------------------------+
# | Examples |
# +-------------------------------------------------------------------+
diff --git a/info/control2011/src/lib/krobot_bus.ml b/info/control2011/src/lib/krobot_bus.ml
index b39ed69..34595f3 100644
--- a/info/control2011/src/lib/krobot_bus.ml
+++ b/info/control2011/src/lib/krobot_bus.ml
@@ -42,6 +42,8 @@ type message =
| Collisions of Bezier.curve * (float * (vertice * float) option) list
| Coins of vertice list
| Urg of int array
+ | Beacon_raw of (int * int * int * int * int * int
+ * int * int * int * int * int)
type t = {
oc : Lwt_io.output_channel;
@@ -149,6 +151,8 @@ let string_of_message = function
l))
| Urg distances ->
sprintf "Urg (many_points...)"
+ | Beacon_raw _ ->
+ sprintf "Raw beacon packet"
(* +-----------------------------------------------------------------+
| Sending/receiving messages |
diff --git a/info/control2011/src/lib/krobot_bus.mli b/info/control2011/src/lib/krobot_bus.mli
index ba3122e..ad33c96 100644
--- a/info/control2011/src/lib/krobot_bus.mli
+++ b/info/control2011/src/lib/krobot_bus.mli
@@ -89,6 +89,9 @@ type message =
(** distances mesured by the URG (in millimeters) *)
| Urg of int array
+ | Beacon_raw of (int * int * int * int * int * int
+ * int * int * int * int * int)
+
val string_of_message : message -> string
(** Returns a string representation of the given message. *)
diff --git a/info/control2011/src/tools/krobot_beacon_reader.ml b/info/control2011/src/tools/krobot_beacon_reader.ml
new file mode 100644
index 0000000..eb6b934
--- /dev/null
+++ b/info/control2011/src/tools/krobot_beacon_reader.ml
@@ -0,0 +1,125 @@
+(*
+ * krobot_beacon_reader.ml
+ * ----------------
+ * Copyright : (c) 2013, Pierre Chambart
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+(* Read beacon messages on serial port and broadcast it on the bus. *)
+
+open Lwt
+open Lwt_react
+open Lwt_preemptive
+open Krobot_bus
+open Krobot_message
+
+let section = Lwt_log.Section.make "krobot(beacon_reader)"
+
+let open_serial path rate =
+ lwt fd = Lwt_unix.openfile path [Unix.O_RDWR; Unix.O_NONBLOCK] 0o660 in
+ let tio = {
+ (* taken from minicom *)
+ Unix.c_ignbrk = true; Unix.c_brkint = false; Unix.c_ignpar = false;
+ Unix.c_parmrk = false; Unix.c_inpck = false; Unix.c_istrip = false;
+ Unix.c_inlcr = false; Unix.c_igncr = false; Unix.c_icrnl = false;
+ Unix.c_ixon = false; Unix.c_ixoff = false; Unix.c_opost = false;
+ Unix.c_obaud = rate; Unix.c_ibaud = rate; Unix.c_csize = 8;
+ Unix.c_cstopb = 1; Unix.c_cread = true; Unix.c_parenb = false;
+ Unix.c_parodd = false; Unix.c_hupcl = false; Unix.c_clocal = true;
+ Unix.c_isig = false; Unix.c_icanon = false; Unix.c_noflsh = false;
+ Unix.c_echo = false; Unix.c_echoe = false; Unix.c_echok = false;
+ Unix.c_echonl = false; Unix.c_vintr = '\000'; Unix.c_vquit = '\000';
+ Unix.c_verase = '\000'; Unix.c_vkill = '\000'; Unix.c_veof = '\000';
+ Unix.c_veol = '\000'; Unix.c_vmin = 1; Unix.c_vtime = 5;
+ Unix.c_vstart = '\000'; Unix.c_vstop = '\000'
+ } in
+ lwt () = Lwt_unix.tcsetattr fd Unix.TCSAFLUSH tio in
+ return fd
+
+let rec get_packet ic =
+ lwt line = Lwt_io.read_line ic in
+ try
+ let s = Scanf.sscanf line "%i %i %i %i %i %i %i %i %i %i %i "
+ (fun x1 x2 x3 x4 x5 x6 x7 x8 x9 x10 x11 ->
+ x1, x2, x3, x4, x5, x6, x7, x8, x9, x10, x11) in
+ (* let x1, x2, x3, x4, x5, x6, x7, x8, x9, x10, x11 = s in *)
+ (* lwt () = Lwt_log.info_f ~section "%i %i %i %i %i %i %i %i %i %i %i " *)
+ (* x1 x2 x3 x4 x5 x6 x7 x8 x9 x10 x11 in *)
+ return s
+ with _ -> get_packet ic
+
+(* +-----------------------------------------------------------------+
+ | read/send loop |
+ +-----------------------------------------------------------------+ *)
+
+let loop bus ic =
+ let rec aux () =
+ lwt packet = get_packet ic in
+ let time = Unix.gettimeofday () in
+ let msg = Beacon_raw packet in
+ lwt () = Krobot_bus.send bus (time, msg) in
+ aux () in
+ aux ()
+
+(* +-----------------------------------------------------------------+
+ | Message handling |
+ +-----------------------------------------------------------------+ *)
+
+let handle_message (timestamp, message) =
+ match message with
+ | Kill "beacon_reader" ->
+ exit 0
+ | _ ->
+ ()
+
+(* +-----------------------------------------------------------------+
+ | Command-line arguments |
+ +-----------------------------------------------------------------+ *)
+
+let fork = ref true
+let tty = ref "/dev/ttyUSB0"
+let baudrate = ref 57600
+
+let options = Arg.align [
+ "-no-fork", Arg.Clear fork, " Run in foreground";
+ "-tty", Arg.Set_string tty, " set tty file";
+ "-baudrate", Arg.Set_int baudrate, " set tty baudrate file";
+]
+
+let usage = "\
+Usage: krobot-beacon-reader [options]
+options are:"
+
+(* +-----------------------------------------------------------------+
+ | Entry point |
+ +-----------------------------------------------------------------+ *)
+
+lwt () =
+ Arg.parse options ignore usage;
+
+ (* Display all informative messages. *)
+ Lwt_log.append_rule "*" Lwt_log.Info;
+
+ (* Open the krobot bus. *)
+ lwt bus = Krobot_bus.get () in
+
+ lwt fd = open_serial !tty !baudrate in
+ let ic = Lwt_io.of_fd ~mode:Lwt_io.input fd in
+
+ lwt _ = get_packet ic in
+
+ lwt () = Lwt_log.info ~section "got first packet" in
+
+ (* Fork if not prevented. *)
+ if !fork then Krobot_daemon.daemonize bus;
+
+ (* Handle krobot message. *)
+ E.keep (E.map handle_message (Krobot_bus.recv bus));
+
+ (* Kill any running urg_handler. *)
+ lwt () = Krobot_bus.send bus (Unix.gettimeofday (), Krobot_bus.Kill "beacon_reader") in
+
+ (* Loop forever. *)
+ loop bus ic
diff --git a/info/control2011/src/tools/krobot_urg.ml b/info/control2011/src/tools/krobot_urg.ml
index 00761e1..6b1acaf 100644
--- a/info/control2011/src/tools/krobot_urg.ml
+++ b/info/control2011/src/tools/krobot_urg.ml
@@ -31,8 +31,8 @@ let to_array (b:Urg.point_data) =
let loop bus urg =
let rec aux () =
- let time = Unix.gettimeofday () in
lwt _ = Lwt_preemptive.detach Urg_simple.get urg in
+ let time = Unix.gettimeofday () in
let msg = Urg (to_array urg.Urg_simple.data) in
lwt () = Krobot_bus.send bus (time, msg) in
lwt () = Lwt_log.info_f "send things" in
diff --git a/info/vision/camlcv/_oasis b/info/vision/camlcv/_oasis
index 198848e..69e0ed7 100644
--- a/info/vision/camlcv/_oasis
+++ b/info/vision/camlcv/_oasis
@@ -100,6 +100,12 @@ Executable "test_houghLines"
Install: false
BuildDepends: cv
+Executable "test_houghLines_draw"
+ Path: test/
+ MainIs: test_houghLines_draw.ml
+ Install: false
+ BuildDepends: cv
+
Executable "test_contour_canny"
Path: test/
MainIs: test_contour_canny.ml
diff --git a/info/vision/camlcv/src/cvCore.ml b/info/vision/camlcv/src/cvCore.ml
index c77548b..cabf39e 100644
--- a/info/vision/camlcv/src/cvCore.ml
+++ b/info/vision/camlcv/src/cvCore.ml
@@ -50,6 +50,19 @@ let depth_f32 = IPL_DEPTH_32F
type 'a image_depth = 'a depth
+type ('channel) cvMat_float32 =
+ (float, Bigarray.float32_elt, Bigarray.c_layout) Bigarray.Array3.t
+
+type ('channel) cvMat_float64 =
+ (float, Bigarray.float64_elt, Bigarray.c_layout) Bigarray.Array3.t
+
+type ('channel) cvMat_int =
+ (int32, Bigarray.int32_elt, Bigarray.c_layout) Bigarray.Array3.t
+(* there is no 64 bit type in CvMat... so stick to int32 *)
+
+type int_mat =
+ (int, Bigarray.int8_unsigned_elt, Bigarray.c_layout) Bigarray.Array2.t
+
type color_conversion =
| CV_BGR2BGRA
| CV_RGB2RGBA
@@ -615,6 +628,17 @@ let houghLinesP img ?(minLineLength=0.) ?(maxLineGap=0.) rho theta threshold =
houghLinesP' img vec rho theta threshold minLineLength maxLineGap;
Array.init (vec4i_vect_size vec) (fun i -> vec4i_vect_get vec i)
+external houghLinesP_mat' :
+ int_mat ->
+ vec4i_vect ->
+ float -> float -> int -> float -> float -> unit
+ = "ocaml_HoughLinesP_mat_bytecode" "ocaml_HoughLinesP_mat"
+
+let houghLinesP_mat mat ?(minLineLength=0.) ?(maxLineGap=0.) rho theta threshold =
+ let vec = create_vec4i_vect () in
+ houghLinesP_mat' mat vec rho theta threshold minLineLength maxLineGap;
+ Array.init (vec4i_vect_size vec) (fun i -> vec4i_vect_get vec i)
+
type calib_cb =
| CV_CALIB_CB_ADAPTIVE_THRESH
| CV_CALIB_CB_NORMALIZE_IMAGE
@@ -672,16 +696,6 @@ let find_corner_subpix ?(criteria=default_criteria) ?(winSize=11,11) ?(zeroZone=
(** camera calibration *)
-type ('channel) cvMat_float32 =
- (float, Bigarray.float32_elt, Bigarray.c_layout) Bigarray.Array3.t
-
-type ('channel) cvMat_float64 =
- (float, Bigarray.float64_elt, Bigarray.c_layout) Bigarray.Array3.t
-
-type ('channel) cvMat_int =
- (int32, Bigarray.int32_elt, Bigarray.c_layout) Bigarray.Array3.t
-(* there is no 64 bit type in CvMat... so stick to int32 *)
-
external cvCalibrateCamera2 : [`Channel_3] cvMat_float32 -> [`Channel_2] cvMat_float32 ->
[`Channel_1] cvMat_int -> cvSize -> [`Channel_1] cvMat_float32 ->
[`Channel_1] cvMat_float32 -> float
diff --git a/info/vision/camlcv/src/cvCore.mli b/info/vision/camlcv/src/cvCore.mli
index 9c559f3..ef1a5fc 100644
--- a/info/vision/camlcv/src/cvCore.mli
+++ b/info/vision/camlcv/src/cvCore.mli
@@ -292,6 +292,15 @@ val good_features_to_track :
?maxLineGap:float -> float -> float -> int -> vec4i array
(** [houghLinesP img rho theta threshold] *)
+type int_mat =
+ (int, Bigarray.int8_unsigned_elt, Bigarray.c_layout) Bigarray.Array2.t
+
+ val houghLinesP_mat :
+ int_mat ->
+ ?minLineLength:float ->
+ ?maxLineGap:float -> float -> float -> int -> vec4i array
+(** [houghLinesP img rho theta threshold] *)
+
type calib_cb =
| CV_CALIB_CB_ADAPTIVE_THRESH
| CV_CALIB_CB_NORMALIZE_IMAGE
diff --git a/info/vision/camlcv/src/cv_caml.c b/info/vision/camlcv/src/cv_caml.c
index f7a0a55..21998a4 100644
--- a/info/vision/camlcv/src/cv_caml.c
+++ b/info/vision/camlcv/src/cv_caml.c
@@ -312,12 +312,12 @@ static CvMat CvMat_val(value v) {
void* data = Data_bigarray_val(v);
int rows = Bigarray_val(v)->dim[0];
int cols = Bigarray_val(v)->dim[1];
- int channels = Bigarray_val(v)->dim[2];
+ int channels = (Bigarray_val(v)->num_dims == 2) ? 1 : Bigarray_val(v)->dim[2];
int kind = Bigarray_val(v)->flags & BIGARRAY_KIND_MASK;
- int type = 0;
+ int type = -1;
switch (kind) {
- case BIGARRAY_FLOAT32:
+ case CAML_BA_FLOAT32:
switch (channels) {
case 1:
type = CV_32FC1;
@@ -333,7 +333,7 @@ static CvMat CvMat_val(value v) {
}
break;
- case BIGARRAY_FLOAT64:
+ case CAML_BA_FLOAT64:
switch (channels) {
case 1:
type = CV_64FC1;
@@ -349,7 +349,7 @@ static CvMat CvMat_val(value v) {
}
break;
- case BIGARRAY_INT32:
+ case CAML_BA_INT32:
switch (channels) {
case 1:
type = CV_32SC1;
@@ -365,10 +365,47 @@ static CvMat CvMat_val(value v) {
}
break;
+ case CAML_BA_UINT8:
+ switch (channels) {
+ case 1:
+ type = CV_8UC1;
+ break;
+ case 2:
+ type = CV_8UC2;
+ break;
+ case 3:
+ type = CV_8UC3;
+ break;
+ default:
+ break;
+ }
+ break;
+
+ case CAML_BA_SINT8:
+ switch (channels) {
+ case 1:
+ type = CV_8SC1;
+ break;
+ case 2:
+ type = CV_8SC2;
+ break;
+ case 3:
+ type = CV_8SC3;
+ break;
+ default:
+ break;
+ }
+ break;
+
default:
break;
}
- if(type == 0) caml_failwith("CvMat_val case not handled");
+
+ if(type == -1)
+ {
+ fflush(stdout);
+ caml_failwith("CvMat_val case not handled");
+ }
return (cvMat(rows, cols, type, data));
}
@@ -1346,6 +1383,42 @@ extern "C" CAMLprim value ocaml_HoughLinesP_bytecode( value * argv, int argn )
argv[4], argv[5], argv[6] );
}
+extern "C" CAMLprim value ocaml_HoughLinesP_mat(value vmat,
+ value vlines,
+ value rho,
+ value theta,
+ value threshold,
+ value minLineLength,
+ value maxLineGap)
+{
+ CAMLparam5( vmat, vlines, rho, theta, threshold );
+ CAMLxparam2( minLineLength, maxLineGap );
+
+ CvMat mat = CvMat_val(vmat);
+ Mat m = Mat(&mat);
+
+ /* Size s = m.size(); */
+ /* printf("mat: depth %i channels %i eltsize %i w %i h %i\n", m.channels(), m.depth(), m.elemSize(), s.width, s.height); */
+ /* fflush(stdout); */
+
+ ERRWRAP(
+ HoughLinesP(Mat(&mat),
+ *Vector_val<Vec4i>(vlines),
+ Double_val(rho),
+ Double_val(theta),
+ Int_val(threshold),
+ Double_val(minLineLength),
+ Double_val(maxLineGap)));
+
+ CAMLreturn(Val_unit);
+}
+
+extern "C" CAMLprim value ocaml_HoughLinesP_mat_bytecode( value * argv, int argn )
+{
+ return ocaml_HoughLinesP_mat( argv[0], argv[1], argv[2], argv[3],
+ argv[4], argv[5], argv[6] );
+}
+
extern "C" CAMLprim value ocaml_CvSeq_info(value vseq)
{
CAMLparam1(vseq);
diff --git a/info/vision/camlcv/test/test_houghLines_draw.ml b/info/vision/camlcv/test/test_houghLines_draw.ml
new file mode 100644
index 0000000..a0c6586
--- /dev/null
+++ b/info/vision/camlcv/test/test_houghLines_draw.ml
@@ -0,0 +1,63 @@
+open CvCore
+open CvHighGui
+open Test_base
+
+let pi = 4. *. (atan 1.)
+
+let x = 500
+let y = 500
+
+let img = Bigarray.Array2.create Bigarray.int8_unsigned Bigarray.c_layout 500 500
+let out = create_image ~x ~y depth_u8 channel_3
+
+let points = [| 12.3, 3.5;
+ 2.4, 5.2;
+ 7.6, 26.8;
+ 10., 10.;
+ 11., 11.;
+ 12., 12.;
+ 13., 13.; |]
+
+let rec show () =
+ (* let x,y = image_size img in *)
+ (* let points' = Array.map (fun (x,y) -> int_of_float x, int_of_float y) points in *)
+
+ (* draw_points *)
+ (* ~color:white *)
+ (* ~size:1 *)
+ (* img *)
+ (* points; *)
+
+ (* Array.iter (fun (x,y) -> *)
+ (* let i, j = int_of_float x, int_of_float y in *)
+ (* img.{i,j} <- 255) points; *)
+
+ for i = 100 to 300 do
+ img.{i,i} <- 255;
+ done;
+
+ for i = 100 to 300 do
+ img.{i+30,400-i} <- 255;
+ done;
+
+ let t1 = Unix.gettimeofday () in
+ let lines =
+ houghLinesP_mat
+ ~minLineLength:2.
+ ~maxLineGap:20.
+ img 0.1 (pi /. 180.) 50 in
+ let t2 = Unix.gettimeofday () in
+
+ Printf.printf "lines: %i time: %f\n%!"
+ (Array.length lines) (t2 -. t1);
+
+ Array.iter (fun (x1,y1,x2,y2) ->
+ line ~color:red out (x1,y1) (x2,y2)) lines;
+
+ show_image "out" out;
+
+ match wait_key 10 with
+ | Some 'q' -> ()
+ | _ -> show ()
+
+let _ = show ()
hooks/post-receive
--
krobot
|
