From 5dab87f11cb68d256ec7406219e054c8277d7355 Mon Sep 17 00:00:00 2001
From: Timothy Stack <stack@flux.utah.edu>
Date: Wed, 6 Apr 2005 06:25:04 +0000
Subject: [PATCH] Try to deal with rmcd going away.
---
robots/emc/emcd.c | 36 ++++++++++++++++++++++++++----------
robots/emc/emcd.h | 10 ++++++++++
2 files changed, 36 insertions(+), 10 deletions(-)
diff --git a/robots/emc/emcd.c b/robots/emc/emcd.c
index f05155ec8a..b2da127ab5 100755
--- a/robots/emc/emcd.c
+++ b/robots/emc/emcd.c
@@ -936,9 +936,13 @@ void ev_callback(event_handle_t handle,
MA_Speed, speed,
MA_TAG_DONE);
+ match->flags |= ERF_HAS_GOAL;
+ match->last_goal_pos = mp.data.mtp_payload_u.command_goto.position;
+
if (rmc_data.handle != NULL) {
mtp_send_packet(rmc_data.handle, &mp);
}
+#if 0
else {
mtp_print_packet(stdout, &mp);
@@ -953,6 +957,7 @@ void ev_callback(event_handle_t handle,
EA_TAG_DONE);
#endif
}
+#endif
mtp_free_packet(&mp);
}
@@ -1077,15 +1082,26 @@ int unknown_client_callback(elvin_io_handler_t handler,
struct emc_robot_config *erc = rli->data;
struct mtp_packet gmp;
- mtp_init_packet(&gmp,
- MA_Opcode, MTP_COMMAND_GOTO,
- MA_Role, MTP_ROLE_EMC,
- MA_CommandID, 1,
- MA_RobotID, erc->id,
- MA_X, (double)erc->init_x,
- MA_Y, (double)erc->init_y,
- MA_Theta, (double)erc->init_theta,
- MA_TAG_DONE);
+ if (erc->flags & ERF_HAS_GOAL) {
+ mtp_init_packet(&gmp,
+ MA_Opcode, MTP_COMMAND_GOTO,
+ MA_Role, MTP_ROLE_EMC,
+ MA_CommandID, 1,
+ MA_RobotID, erc->id,
+ MA_Position, &erc->last_goal_pos,
+ MA_TAG_DONE);
+ }
+ else {
+ mtp_init_packet(&gmp,
+ MA_Opcode, MTP_COMMAND_GOTO,
+ MA_Role, MTP_ROLE_EMC,
+ MA_CommandID, 1,
+ MA_RobotID, erc->id,
+ MA_X, (double)erc->init_x,
+ MA_Y, (double)erc->init_y,
+ MA_Theta, (double)erc->init_theta,
+ MA_TAG_DONE);
+ }
mtp_send_packet(mh, &gmp);
}
@@ -1377,7 +1393,7 @@ int emulab_callback(elvin_io_handler_t handler,
mp->data.mtp_payload_u.command_goto.position.x,
mp->data.mtp_payload_u.command_goto.position.y)
) {
-
+
/* forward the packet on to rmc... */
if (rmc_data.handle == NULL) {
error("no rmcd yet\n");
diff --git a/robots/emc/emcd.h b/robots/emc/emcd.h
index 9063f14d3e..9fd0c4f971 100644
--- a/robots/emc/emcd.h
+++ b/robots/emc/emcd.h
@@ -4,6 +4,14 @@
#include "mtp.h"
+enum {
+ ERB_HAS_GOAL,
+};
+
+enum {
+ ERF_HAS_GOAL = (1L << ERB_HAS_GOAL),
+};
+
struct emc_robot_config {
int id;
char *hostname;
@@ -14,6 +22,8 @@ struct emc_robot_config {
float init_y;
float init_theta;
struct robot_position last_update_pos;
+ struct robot_position last_goal_pos;
+ unsigned long flags;
};
struct rmc_client {
--
GitLab