summaryrefslogtreecommitdiff
path: root/digital
diff options
context:
space:
mode:
Diffstat (limited to 'digital')
-rw-r--r--digital/ai/tools/test_simu_control_guybrush.py30
-rw-r--r--digital/io-hub/src/guybrush/Makefile3
-rw-r--r--digital/io-hub/src/guybrush/bottom_clamp.c440
-rw-r--r--digital/io-hub/src/guybrush/main.c37
-rw-r--r--digital/io-hub/tools/io_hub/io_hub.py9
5 files changed, 513 insertions, 6 deletions
diff --git a/digital/ai/tools/test_simu_control_guybrush.py b/digital/ai/tools/test_simu_control_guybrush.py
index 407648dc..146ad04a 100644
--- a/digital/ai/tools/test_simu_control_guybrush.py
+++ b/digital/ai/tools/test_simu_control_guybrush.py
@@ -77,6 +77,22 @@ class TestSimuControl (TestSimu):
out_button ('Door open',
1 << defs.OUTPUT_DOOR_OPEN
| 1 << defs.OUTPUT_DOOR_CLOSE)
+ self.tree_detected_button = Button (self.control_frame,
+ text = 'Tree Detected', padx = 0, pady = 0,
+ command = self.tree_detected)
+ self.tree_detected_button.pack()
+ self.stop_tree_approach_button = Button (self.control_frame,
+ text = 'Stop tree approach', padx = 0, pady = 0,
+ command = self.stop_tree_approach)
+ self.stop_tree_approach_button.pack()
+ self.empty_tree_button = Button (self.control_frame,
+ text = 'Empty Tree', padx = 0, pady = 0,
+ command = self.empty_tree)
+ self.empty_tree_button.pack()
+ self.robot_is_back_button = Button (self.control_frame,
+ text = 'Robot is back', padx = 0, pady = 0,
+ command = self.robot_is_back)
+ self.robot_is_back_button.pack()
self.backward_var = IntVar ()
self.backward_button = Checkbutton (self.control_frame,
text = 'Backward', variable = self.backward_var)
@@ -106,7 +122,19 @@ class TestSimuControl (TestSimu):
self.asserv.goto_angle (a)
def lower_clamp_rotate_command (self):
- self.mimot.speed_pos ('a0', self.LOWER_CLAMP_ROTATION_STROKE / 2)
+ self.mimot.speed_pos ('a0', self.LOWER_CLAMP_ROTATION_STROKE / 2)
+
+ def tree_detected(self):
+ self.io.tree_detected()
+
+ def stop_tree_approach(self):
+ self.io.stop_tree_approach()
+
+ def empty_tree(self):
+ self.io.empty_tree()
+
+ def robot_is_back(self):
+ self.io.robot_is_back()
if __name__ == '__main__':
run ('guybrush', TestSimuControl)
diff --git a/digital/io-hub/src/guybrush/Makefile b/digital/io-hub/src/guybrush/Makefile
index 7e3db6fd..9099dc20 100644
--- a/digital/io-hub/src/guybrush/Makefile
+++ b/digital/io-hub/src/guybrush/Makefile
@@ -10,7 +10,8 @@ io_hub_SOURCES = main.c top.c \
contact.avr.c contact.host.c \
output.c output.host.c \
twi_master.c asserv.c mimot.c beacon.c \
- chrono.c timer.avr.c simu.host.c
+ chrono.c timer.avr.c simu.host.c \
+ bottom_clamp.c
# Modules needed for IO.
MODULES = proto uart twi utils \
adc devices/usdist \
diff --git a/digital/io-hub/src/guybrush/bottom_clamp.c b/digital/io-hub/src/guybrush/bottom_clamp.c
new file mode 100644
index 00000000..55f913a5
--- /dev/null
+++ b/digital/io-hub/src/guybrush/bottom_clamp.c
@@ -0,0 +1,440 @@
+/* bottom_clamp.c */
+/* guybrush - Eurobot 2012 AI. {{{
+ *
+ * Copyright (C) 2012 Julien Balleyguier
+ *
+ * APBTeam:
+ * Web: http://apbteam.org/
+ * Email: team AT apbteam DOT org
+ *
+ * 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 2 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, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * }}} */
+#include "common.h"
+#include "output.h"
+
+#include "mimot.h"
+#include "contact.h"
+#include "bot.h"
+
+
+#define FSM_NAME AI
+#include "fsm.h"
+#include "fsm_queue.h"
+
+/*
+ * There is two FSM in this file.
+ *
+ * The clamp FSM handles high level clamp behaviour, new elements, drop, and
+ * gives orders to the clamp move FSM.
+ *
+ * The clamp move FSM only handle moving the clamp without load or moving an
+ * element from a slot to another one.
+ */
+
+FSM_STATES (
+ /* Initial state. */
+ CLAMP_START,
+
+ /* Initialisation sequence: */
+ /* opening the 2 clamps */
+ CLAMP_INIT_OPEN,
+ /* Finding 0 position */
+ CLAMP_INIT_FIND_0,
+ /* Hide the clamp in order not to go over the robot. */
+ CLAMP_INIT_HIDE_CLAMP,
+ /* Clamp ready, waiting in rest position. */
+ CLAMP_INIT_READY,
+ /* Returning to idle position. */
+ CLAMP_GOING_IDLE,
+ /* Waiting external events, clamp at middle level. */
+ CLAMP_IDLE,
+ /* Taking a coin */
+ CLAMP_TAKE_COIN,
+ /* turning half way */
+ CLAMP_TURN_HALF_WAY,
+ /* Droping the cd. */
+ CLAMP_DROP_CD,
+ /* Clamp blocked. */
+ CLAMP_BLOCKED,
+ BOTTOM_CLAMP_HIDE_POS,
+ UNFOLD_UPPER_SET,
+ BOTTOM_CLAMP_READY,
+ READY_TO_EMPTY_TREE,
+ CLOSE_ALL_CLAMPS,
+ REARRANGE_CD,
+ BOTTOM_CLAMP_HIDE_POS2,
+ RELEASE_ASSERV,
+ FOLD_UPPER_SET,
+ OPEN_UPPER_CLAMPS,
+ REARRANGE_CD_CLAMP,
+ BOTOM_CLAMP_BACK
+ )
+
+FSM_EVENTS (
+ /* coin detected in the clamp */
+ coin_detected,
+ /* Lower clamp rotation motor success. */
+ lower_clamp_rotation_success,
+ /* Lower clamp rotation motor failure. */
+ lower_clamp_rotation_failure,
+ /*upper set is totally open*/
+ upper_set_up,
+ /*upper set is totally close*/
+ upper_set_down,
+ /* Tree detected, get ready to empty it. */
+ tree_detected,
+ /* stop the tree-emptying cycle*/
+ stop_tree_approach,
+ /* signal sent when the robot is ready to empty the tree. the approach has been finished*/
+ clamps_ready,
+ /* the robot is ready to empty the tree, and the top fsm is asking him to do so*/
+ empty_tree,
+ robot_is_back
+ )
+
+
+FSM_START_WITH (CLAMP_IDLE)
+
+/*-------------------------------------
+ TIMEOUT DEFINITION
+---------------------------------------*/
+#define TIMEOUT_OPEN_CLAMPS 40
+#define TIMEOUT_CLOSE_CLAMPS 40
+#define TIMEOUT_DROP_CD 100
+#define TIMEOUT_FREE_ASSERV 100
+#define TIMEOUT_RECENTRAGE 100
+
+/*-------------------------------------
+ ROTATION DEFINITION
+---------------------------------------*/
+#define HIDE_POS 3
+#define BACK_TO_READY 16-HIDE_POS
+#define HALF_TURN 8
+#define HIDE_POS_TREE 3
+#define HIDE_POS_TREE_2 1
+#define BACK_TO_READY_TREE 16-HIDE_POS_TREE
+#define SPEED_ROTATION 0x20
+/*-------------------------------------
+ Clamp context
+---------------------------------------*/
+struct clamp_t
+{
+ /** Current position. */
+ int pos_current;
+ /* Clamp_1 is low, which means, it is ready to take a coin*/
+ uint8_t clamp_1_down;
+ /** True if clamp 1 is open. */
+ uint8_t clamp_1_open;
+ /** True if clamp 2 is open. */
+ uint8_t clamp_2_open;
+};
+
+
+static void
+clamp_blocked (void)
+{
+}
+
+/*Global context. */
+struct clamp_t clamp_global;
+#define ctx clamp_global
+
+/*---------------------------------------------------------*/
+/* INIT part of the FSM */
+/*---------------------------------------------------------*/
+
+/* Init Bottom clamp. */
+
+FSM_TRANS (CLAMP_START, init_actuators, CLAMP_INIT_OPEN)
+{
+ /* Opening the 2 clamps. */
+ ctx.clamp_1_open = 1;
+ IO_CLR (OUTPUT_LOWER_CLAMP_1_CLOSE);
+ ctx.clamp_2_open = 1;
+ IO_CLR (OUTPUT_LOWER_CLAMP_2_CLOSE);
+ /* recentrage the middle clamp. */
+ IO_SET (OUTPUT_UPPER_CLAMP_OUT);
+ IO_CLR (OUTPUT_UPPER_CLAMP_IN);
+
+ return FSM_NEXT (CLAMP_START,init_actuators);
+
+}
+
+FSM_TRANS_TIMEOUT (CLAMP_INIT_OPEN, TIMEOUT_OPEN_CLAMPS, CLAMP_INIT_FIND_0)
+{
+ /*Findig the 0 position. */
+ mimot_motor0_find_zero (0x05, 1, 0);
+ ctx.pos_current = 0;
+ return FSM_NEXT_TIMEOUT (CLAMP_INIT_OPEN);
+}
+
+FSM_TRANS (CLAMP_INIT_FIND_0, lower_clamp_rotation_success, CLAMP_INIT_HIDE_CLAMP)
+{
+ /* Hidding the clamp inside the robot. */
+ int move_needed;
+ move_needed = HIDE_POS * 250;
+ ctx.pos_current += move_needed;
+ mimot_move_motor0_absolute (ctx.pos_current, SPEED_ROTATION);
+ return FSM_NEXT (CLAMP_INIT_FIND_0, lower_clamp_rotation_success);
+}
+
+FSM_TRANS (CLAMP_INIT_HIDE_CLAMP, lower_clamp_rotation_success, CLAMP_INIT_READY)
+{
+ /*Nothing to do,just waiting for the clamp to be hidden*/
+ return FSM_NEXT (CLAMP_INIT_HIDE_CLAMP, lower_clamp_rotation_success);
+}
+
+
+FSM_TRANS (CLAMP_INIT_HIDE_CLAMP, lower_clamp_rotation_failure, CLAMP_BLOCKED)
+{
+ /*The clamp is blocked somehow*/
+ clamp_blocked();
+ return FSM_NEXT (CLAMP_INIT_HIDE_CLAMP, lower_clamp_rotation_failure);
+}
+
+FSM_TRANS (CLAMP_INIT_READY,init_start_round, CLAMP_GOING_IDLE)
+{
+ int move_needed;
+ move_needed = BACK_TO_READY * 250;
+ ctx.pos_current += move_needed;
+ mimot_move_motor0_absolute (ctx.pos_current, SPEED_ROTATION);
+ ctx.clamp_1_down = 1;
+ return FSM_NEXT (CLAMP_INIT_READY, init_start_round);
+}
+
+FSM_TRANS (CLAMP_GOING_IDLE, lower_clamp_rotation_success, CLAMP_IDLE)
+{
+ /*Going back to the idle position, ready for showtime.*/
+ return FSM_NEXT (CLAMP_GOING_IDLE, lower_clamp_rotation_success);
+}
+
+FSM_TRANS (CLAMP_GOING_IDLE, lower_clamp_rotation_failure, CLAMP_BLOCKED)
+{
+ /* The clamp is blocked somehow. */
+ clamp_blocked();
+ return FSM_NEXT (CLAMP_GOING_IDLE, lower_clamp_rotation_failure);
+}
+
+
+/*---------------------------------------------------------*/
+/* parts of the FSM that Takes coin */
+/*---------------------------------------------------------*/
+FSM_TRANS (CLAMP_IDLE, coin_detected, CLAMP_TAKE_COIN)
+{
+ if (ctx.clamp_1_down)
+ {
+ IO_SET (OUTPUT_LOWER_CLAMP_1_CLOSE); //close it
+ ctx.clamp_1_open = 0;
+ }
+ else // if the clamp 2 is ready
+ {
+ IO_SET(OUTPUT_LOWER_CLAMP_2_CLOSE);
+ ctx.clamp_2_open = 0;
+ }
+ return FSM_NEXT (CLAMP_IDLE, coin_detected);
+}
+
+
+FSM_TRANS_TIMEOUT (CLAMP_TAKE_COIN, TIMEOUT_CLOSE_CLAMPS, CLAMP_TURN_HALF_WAY)
+{
+ int move_needed;
+ move_needed = HALF_TURN * 250;
+ ctx.pos_current += move_needed;
+ mimot_move_motor0_absolute (ctx.pos_current, SPEED_ROTATION);
+ return FSM_NEXT_TIMEOUT (CLAMP_TAKE_COIN);
+}
+
+FSM_TRANS_TIMEOUT (CLAMP_TURN_HALF_WAY, TIMEOUT_DROP_CD, CLAMP_DROP_CD)
+{
+ /*If the clamp 1 is closed.*/
+ if (!ctx.clamp_1_open)
+ {
+ /*Open it.*/
+ IO_CLR (OUTPUT_LOWER_CLAMP_1_CLOSE);
+ ctx.clamp_1_open = 1;
+ /*Clamp 1 is now up (clamp 2 is down).*/
+ ctx.clamp_1_down = 0;
+ }
+ /*If the clamp 2 is closed. */
+ else
+ {
+ IO_CLR (OUTPUT_LOWER_CLAMP_2_CLOSE);
+ ctx.clamp_2_open = 1;
+ /*Clamp 1 is now down (clamp 2 is up). */
+ ctx.clamp_1_down = 1;
+ }
+ return FSM_NEXT_TIMEOUT (CLAMP_TURN_HALF_WAY);
+}
+
+FSM_TRANS (CLAMP_DROP_CD,lower_clamp_rotation_success,CLAMP_IDLE)
+{
+ /*Nothing to do, just waiting to be in IDLE state again*/
+ return FSM_NEXT (CLAMP_DROP_CD,lower_clamp_rotation_success);
+}
+
+FSM_TRANS (CLAMP_DROP_CD, lower_clamp_rotation_failure, CLAMP_BLOCKED)
+{
+ /*The clamp is blocked somehow. */
+ clamp_blocked();
+ return FSM_NEXT (CLAMP_DROP_CD, lower_clamp_rotation_failure);
+}
+/*---------------------------------------------------------*/
+/* Parts of the FSM that is Approaching the tree (totem) */
+/*---------------------------------------------------------*/
+
+FSM_TRANS (CLAMP_IDLE, tree_detected, BOTTOM_CLAMP_HIDE_POS)
+{
+ /*Contrepression*/
+ IO_SET (OUTPUT_UPPER_CLAMP_UP);
+ /*Hidding the clamp inside the robot*/
+ int move_needed;
+ if (ctx.clamp_1_down)
+ {
+ move_needed = HIDE_POS_TREE * 250;
+ }
+ else
+ {
+ move_needed = (HALF_TURN + HIDE_POS_TREE) * 250;
+ ctx.clamp_1_down = 1;
+ }
+ ctx.pos_current += move_needed;
+ mimot_move_motor0_absolute (ctx.pos_current, SPEED_ROTATION);
+ return FSM_NEXT (CLAMP_IDLE, tree_detected);
+}
+
+
+FSM_TRANS (BOTTOM_CLAMP_HIDE_POS, lower_clamp_rotation_success, UNFOLD_UPPER_SET)
+{
+ IO_CLR (OUTPUT_UPPER_CLAMP_UP);
+ IO_SET (OUTPUT_UPPER_CLAMP_DOWN);
+ return FSM_NEXT (BOTTOM_CLAMP_HIDE_POS, lower_clamp_rotation_success);
+}
+
+FSM_TRANS (BOTTOM_CLAMP_HIDE_POS, lower_clamp_rotation_failure, CLAMP_BLOCKED)
+{
+ /*The clamp is blocked somehow*/
+ clamp_blocked();
+ return FSM_NEXT (BOTTOM_CLAMP_HIDE_POS, lower_clamp_rotation_failure);
+}
+
+
+FSM_TRANS (UNFOLD_UPPER_SET, upper_set_down, BOTTOM_CLAMP_READY)
+{
+ /*putting the bottom clamp back to ready*/
+ int move_needed;
+ move_needed = BACK_TO_READY_TREE * 250;
+ ctx.pos_current += move_needed;
+ mimot_move_motor0_absolute (ctx.pos_current, SPEED_ROTATION);
+
+ /*opening the top clamp*/
+ IO_SET (OUTPUT_UPPER_CLAMP_OPEN);
+
+ return FSM_NEXT (UNFOLD_UPPER_SET, upper_set_down);
+
+}
+
+FSM_TRANS (BOTTOM_CLAMP_READY, lower_clamp_rotation_success, READY_TO_EMPTY_TREE)
+{
+
+ fsm_queue_post_event (FSM_EVENT (AI, clamps_ready));
+ return FSM_NEXT (BOTTOM_CLAMP_READY, lower_clamp_rotation_success);
+
+}
+
+FSM_TRANS (BOTTOM_CLAMP_READY, lower_clamp_rotation_failure, CLAMP_BLOCKED)
+{
+ /*The clamp is blocked somehow*/
+ clamp_blocked();
+ return FSM_NEXT (BOTTOM_CLAMP_HIDE_POS, lower_clamp_rotation_failure);
+}
+
+FSM_TRANS (READY_TO_EMPTY_TREE, empty_tree, CLOSE_ALL_CLAMPS)
+{
+ /*Closgin bottom clamp*/
+ IO_SET (OUTPUT_LOWER_CLAMP_1_CLOSE);
+ ctx.clamp_1_open=0;
+ //
+ /*(We need to close both clamp to have an easier turn around) */
+ IO_SET (OUTPUT_LOWER_CLAMP_2_CLOSE);
+ ctx.clamp_2_open=0;
+
+
+ /*closing upper & middle clamps*/
+ IO_CLR (OUTPUT_UPPER_CLAMP_OPEN);
+
+
+ return FSM_NEXT (READY_TO_EMPTY_TREE, empty_tree);
+
+}
+
+/*---------------------------------------------------------*/
+/* Parts of the FSM that is emptying the tree (totem) */
+/*---------------------------------------------------------*/
+
+FSM_TRANS(CLOSE_ALL_CLAMPS, robot_is_back,REARRANGE_CD)
+{
+ IO_CLR (OUTPUT_UPPER_CLAMP_OUT);
+ IO_SET (OUTPUT_UPPER_CLAMP_IN);
+ return FSM_NEXT (CLOSE_ALL_CLAMPS, robot_is_back);
+
+}
+
+FSM_TRANS_TIMEOUT (REARRANGE_CD, TIMEOUT_RECENTRAGE, BOTTOM_CLAMP_HIDE_POS2)
+{
+ /*Hidding the clamp inside the robot*/
+ int move_needed;
+ move_needed = HIDE_POS_TREE_2 * 250;
+ ctx.pos_current += move_needed;
+ mimot_move_motor0_absolute (ctx.pos_current, SPEED_ROTATION);
+ return FSM_NEXT_TIMEOUT (REARRANGE_CD);
+}
+FSM_TRANS (BOTTOM_CLAMP_HIDE_POS2, lower_clamp_rotation_success, RELEASE_ASSERV)
+{
+ mimot_motor0_free();
+ return FSM_NEXT (BOTTOM_CLAMP_HIDE_POS2, lower_clamp_rotation_success);
+
+}
+
+FSM_TRANS_TIMEOUT (RELEASE_ASSERV, TIMEOUT_FREE_ASSERV, FOLD_UPPER_SET)
+{
+ IO_CLR (OUTPUT_UPPER_CLAMP_DOWN);
+ IO_SET (OUTPUT_UPPER_CLAMP_UP);
+ return FSM_NEXT_TIMEOUT (RELEASE_ASSERV);
+
+}
+FSM_TRANS (FOLD_UPPER_SET, upper_set_up, OPEN_UPPER_CLAMPS)
+{
+ IO_SET (OUTPUT_UPPER_CLAMP_OPEN);
+ IO_CLR (OUTPUT_UPPER_CLAMP_IN);
+ IO_SET (OUTPUT_UPPER_CLAMP_OUT);
+ return FSM_NEXT (FOLD_UPPER_SET, upper_set_up);
+
+}
+
+FSM_TRANS_TIMEOUT (OPEN_UPPER_CLAMPS, TIMEOUT_OPEN_CLAMPS, CLAMP_TURN_HALF_WAY)
+{
+ IO_SET (OUTPUT_UPPER_CLAMP_OPEN);
+ /*we reopen clamp 2*/
+ IO_CLR (OUTPUT_LOWER_CLAMP_2_CLOSE);
+ ctx.clamp_2_open = 1;
+ int move_needed;
+ move_needed = (HALF_TURN-HIDE_POS_TREE_2) * 250;
+ ctx.pos_current += move_needed;
+ mimot_move_motor0_absolute (ctx.pos_current, SPEED_ROTATION);
+ return FSM_NEXT_TIMEOUT (OPEN_UPPER_CLAMPS);
+
+}
+
diff --git a/digital/io-hub/src/guybrush/main.c b/digital/io-hub/src/guybrush/main.c
index 79940eac..f73ec254 100644
--- a/digital/io-hub/src/guybrush/main.c
+++ b/digital/io-hub/src/guybrush/main.c
@@ -158,11 +158,24 @@ main_event_to_fsm (void)
FSM_HANDLE_E (AI, robot_move_success);
else if (robot_move_status == failure)
FSM_HANDLE_E (AI, robot_move_failure);
+ if (mimot_motor0_status == success)
+ FSM_HANDLE_E (AI, lower_clamp_rotation_success);
+ else if (mimot_motor0_status == failure)
+ FSM_HANDLE_E (AI, lower_clamp_rotation_failure);
+ if (!IO_GET (CONTACT_LOWER_CLAMP_SENSOR_1)
+ || !IO_GET (CONTACT_LOWER_CLAMP_SENSOR_2)
+ || !IO_GET (CONTACT_LOWER_CLAMP_SENSOR_3)
+ || !IO_GET (CONTACT_LOWER_CLAMP_SENSOR_4))
+ FSM_HANDLE_E (AI, coin_detected);
/* Jack. */
if (!contact_get_jack ())
FSM_HANDLE_E (AI, jack_inserted);
else
FSM_HANDLE_E (AI, jack_removed);
+ if (!IO_GET(CONTACT_UPPER_CLAMP_DOWN))
+ FSM_HANDLE_E (AI,upper_set_down);
+ if (!IO_GET(CONTACT_UPPER_CLAMP_UP))
+ FSM_HANDLE_E (AI,upper_set_up);
/* Events from the event queue. */
if (fsm_queue_poll ())
{
@@ -269,6 +282,26 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
/* Enter FSM debug mode, then step once. */
main_fsm_debug_mode = MAIN_FSM_DEBUG_STEP;
break;
+ case c ('t',0):
+ /* Simulate tree detection. */
+ fsm_queue_post_event (FSM_EVENT (AI, tree_detected));
+ break;
+ case c ('s',0):
+ /* Simulate stop tree approach. */
+ fsm_queue_post_event (FSM_EVENT (AI, stop_tree_approach));
+ break;
+ case c ('e',0):
+ /* Simulate the empty tree command. */
+ fsm_queue_post_event (FSM_EVENT (AI, empty_tree));
+ break;
+ case c ('r',0):
+ /* Simulate the robot_is_back command. */
+ fsm_queue_post_event (FSM_EVENT (AI, robot_is_back));
+ break;
+ case c ('c',0):
+ /* Simulate the coin detected command. */
+ fsm_queue_post_event (FSM_EVENT (AI, coin_detected));
+ break;
case c ('m', 5):
/* Go to position.
* - 2w: x, y.
@@ -282,9 +315,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
break;
case c ('w', 0):
/* Disable all motor control. */
- mimot_motor0_free ();
- mimot_motor1_free ();
- asserv_free_motor ();
+ mimot_reset();
break;
case c ('o', 5):
/* Set/clear/toggle outputs.
diff --git a/digital/io-hub/tools/io_hub/io_hub.py b/digital/io-hub/tools/io_hub/io_hub.py
index 6623a346..4aa388b0 100644
--- a/digital/io-hub/tools/io_hub/io_hub.py
+++ b/digital/io-hub/tools/io_hub/io_hub.py
@@ -92,4 +92,11 @@ class ProtoRobospierre (Proto):
class ProtoGuybrush (Proto):
- pass
+ def tree_detected(self):
+ self.proto.send('t')
+ def stop_tree_approach(self):
+ self.proto.send('s')
+ def empty_tree(self):
+ self.proto.send('e')
+ def robot_is_back(self):
+ self.proto.send('r')