diff --git a/mc/Makefile b/mc/Makefile
index d77a88f0ff7cea977af84501bb286e0119121177..e23c384ae006cf987e7773ad303bbb67c73dab91 100644
--- a/mc/Makefile
+++ b/mc/Makefile
@@ -5,7 +5,8 @@ TARGETS=lth_fixes.mc \
hijacknet_lth_net.mc \
fei82557End.mc \
hijacknet_test.mc \
- extctrl2014_lc.mc
+ extctrl2014_lc.mc \
+ hijacknet_extctrl2014.mc
AUTO_GEN=extctrl2014_lc.c \
extctrl2014_lc.h \
mc.globalize.c
@@ -19,6 +20,10 @@ EXTRA_CFLAGS_hijacknet_lth_net=-I/opt/robot/include
EXTRA_CFLAGS_fei82557End=-Wno-error -Wno-all -Wno-endif-labels
EXTRA_CFLAGS_extctrl2014_lc=-I/usr/include/labcomm \
-DLABCOMM_COMPAT="<labcomm_compat.h>"
+EXTRA_CFLAGS_hijacknet_extctrl2014=-I/usr/include/labcomm \
+ -DLABCOMM_COMPAT="<labcomm_compat.h>"
+EXTRA_LDFLAGS_hijacknet_extctrl2014=extctrl2014_lc.mc -Wl,-r,-llabcomm2014
+HIDE_hijacknet_extctrl2014=labcomm2014_
ROBOT=
ROBOTWARE=$(shell [ -n "$(ROBOT)" ] && \
../tools/abb_extctrl_setup.py --robot "$(ROBOT)" --robotware)
@@ -32,7 +37,7 @@ all: $(TARGETS)
mc_test_load: hijack_fei.mc.load spray_test.mc.load
-%.mc: %.mc.c
+%.mc: %.mc.c Makefile
WIND_BASE=/opt/robot/include/vxworks/5.5.1/ \
i586-wrs-vxworks-gcc \
-O3 -Wall -Wno-unused-but-set-variable -Werror \
@@ -44,9 +49,9 @@ mc_test_load: hijack_fei.mc.load spray_test.mc.load
-I. \
-I../common \
-o $@ \
- -c $<
+ $< $(EXTRA_LDFLAGS_$*)
-%.mc: %.c
+%.mc: %.c Makefile
WIND_BASE=/opt/robot/include/vxworks/5.5.1/ \
i586-wrs-vxworks-gcc \
-O3 -Wall -Wno-unused-but-set-variable -Werror \
@@ -56,7 +61,14 @@ mc_test_load: hijack_fei.mc.load spray_test.mc.load
-I. \
-I../common \
-o $@ \
- -c $<
+ $< $(EXTRA_LDFLAGS_$*)
+ for h in $(HIDE_$*) ; do \
+ nm $@ | sed -e "s/.*[DTB] \($$h.*\)/\1/gp;d" > $@.hide || exit 1; \
+ echo "Hiding: $$h ($$(cat $@.hide | wc -l) symbols)" ; \
+ i586-wrs-vxworks-objcopy --localize-symbols=$@.hide $@ $@.tmp && \
+ mv $@.tmp $@ || echo "Hiding failed for $$h" || exit 1 ; \
+ rm $@.hide ; \
+ done
%_lc.c %_lc.h: %.lc
labcomm2014 --c=$*_lc.c --h=$*_lc.h $<
@@ -92,7 +104,10 @@ host_commands:
@echo "ncftpput -uu -pp $(ROBOT) /hd0a/$(DESTDIR) hijacknet_hpnet.mc"
@echo "ncftpput -uu -pp $(ROBOT) /hd0a/$(DESTDIR) hijacknet_fei.mc"
@echo "ncftpput -uu -pp $(ROBOT) /hd0a/$(DESTDIR) hijacknet.mc"
+ @echo
@echo "ncftpput -uu -pp $(ROBOT) /hd0a/$(DESTDIR) hijacknet_test.mc"
+ @echo
+ @echo "ncftpput -uu -pp $(ROBOT) /hd0a/$(DESTDIR) hijacknet_extctrl2014.mc"
.PHONY: robot_commands
robot_commands:
@@ -101,8 +116,11 @@ robot_commands:
@echo "ld < /hd0a/$(DESTDIR)/hijacknet_hpnet.mc"
@echo "ld < /hd0a/$(DESTDIR)/hijacknet_fei.mc"
@echo "ld < /hd0a/$(DESTDIR)/hijacknet.mc"
+ @echo
@echo "ld < /hd0a/$(DESTDIR)/hijacknet_test.mc"
-
+ @echo
+ @echo "ld < /hd0a/$(DESTDIR)/hijacknet_extctrl2014.mc"
+ @echo "hijacknet_extctrl2014_connect \"fei0\",\"38:60:77:b5:0e:cf\""
.PHONY: clean
clean:
@@ -114,3 +132,4 @@ distclean: clean
hijacknet_fei.mc: fei82557.mc.h hijacknet_private.h
hijacknet.mc: hijacknet.h
+hijacknet_extctrl2014.mc: extctrl2014_lc.mc
diff --git a/mc/extctrl_pub.irc5_main_110310.h b/mc/extctrl_pub.irc5_main_110310.h
new file mode 100644
index 0000000000000000000000000000000000000000..e73a82e75084aae1b981cec06d971234afb6658b
--- /dev/null
+++ b/mc/extctrl_pub.irc5_main_110310.h
@@ -0,0 +1,98 @@
+#ifndef _EXTCTRL_PUB_H_
+#define _EXTCTRL_PUB_H_
+
+typedef struct {
+ int value[4];
+} axis_configuration_t; // Up to 128 axes (32 * 4)
+
+typedef struct {
+ int manualMode; // 1 = teach mode
+ int controlActive; // 1 = at least torque mode active
+} controller_state;
+
+
+typedef struct extctrl_irb2ext_joint {
+ /* Control parameters: */
+ float parKp; /* Position control proportional gain. */
+ float parKv; /* Velocity control proportional gain. */
+ float parKi; /* Velocity control integration gain. */
+ float parTrqMin; /* Minimum torque (control output) allowed. */
+ float parTrqMax; /* Dito maximum. */
+
+ /* Exposed (measured or computed) states: */
+ double posRawAbs; /* Measured unfiltered motor angle. [abs] */
+ double posRawFb; /* Measured unfiltered motor angle. [feedback] */
+ double posFlt; /* Filtered motor angle. */
+ float velRaw; /* Unfiltered angular velocity. */
+ float velFlt; /* Dito filtered, also for motor shaft. */
+ float velOut; /* Dito according to actual velocity control ref. */
+ float trqRaw; /* Measured (or internal) unfiltered torque. */
+ float trqRefFlt; /* filtered Torque ref */
+
+ double posRef; /* Nominal position reference. */
+ float velRef; /* Dito for the velocity reference/feedforward. */
+ float trqFfw; /* Model-based torque feedforward. */
+ float trqFfwGrav; /* total dynamic model based torque. */
+} EXTCTRL_IRB2EXT_JOINT;
+
+typedef struct extctrl_ext2irb_joint {
+ /* Possibly updated control parameters: */
+ float parKp;
+ float parKv;
+ float parKi;
+
+ /* Modified references and feed forward: */
+ double posRef;
+ float velRef;
+ float trqFfw;
+} EXTCTRL_EXT2IRB_JOINT;
+
+typedef struct extctrl_communication_data {
+ int instruction;
+ float value1;
+ float value2;
+ float value3;
+ float value4;
+ float value5;
+ float value6;
+ char str1[20];
+ char str2[20];
+} EXTCTRL_COMMUNICATION_DATA;
+
+/* To use extctrl_communication from RAPID
+ * RAPID SYNTAX:
+ *
+ * VAR mocgendata mydata=[1,2,3,4,5,6,'str1','str2'];
+ * MocGenInstr 88999, mydata
+ * mydata is written to extctrl_communication and NEW data is returned
+ */
+
+
+/*
+ * To be called by programs invoked from VxWorks prompt,
+ * i.e. concurrently, hence we need mutex protection. Mutex
+ * is allocated by extctrl_initialize.
+ */
+
+typedef int (*extctrl_active_cb)(axis_configuration_t axis_configuration);
+
+typedef int (*extctrl_submit_cb)(controller_state ctrl_state ,
+ axis_configuration_t axis_configuration,
+ EXTCTRL_IRB2EXT_JOINT *joint);
+
+typedef int (*extctrl_obtain_cb)(axis_configuration_t axis_configuration,
+ EXTCTRL_EXT2IRB_JOINT *joint);
+
+typedef int (*extctrl_communication_cb)(axis_configuration_t ac,
+ EXTCTRL_COMMUNICATION_DATA *data);
+
+#define extctrl_install_callbacks extctrl_install_callbacks_irc5_110310
+
+int extctrl_install_callbacks(extctrl_active_cb submit_active,
+ extctrl_submit_cb submit,
+ extctrl_active_cb obtain_active,
+ extctrl_obtain_cb obtain,
+ extctrl_active_cb communication_active,
+ extctrl_communication_cb communication);
+
+#endif
diff --git a/mc/hijacknet_extctrl2014.c b/mc/hijacknet_extctrl2014.c
new file mode 100644
index 0000000000000000000000000000000000000000..d07d701936ea4e36d8acde5ad511618b2ce2b376
--- /dev/null
+++ b/mc/hijacknet_extctrl2014.c
@@ -0,0 +1,1318 @@
+#include <stdio.h>
+#include <semLib.h>
+#include <sysLib.h>
+#include <string.h>
+#include <taskLib.h>
+#include <stdlib.h>
+#include <extctrl_pub.irc5_main_110310.h>
+#include "extctrl2014_lc.h"
+#include <labcomm2014.h>
+#include <labcomm2014_default_error_handler.h>
+#include <labcomm2014_default_memory.h>
+#include <labcomm2014_default_scheduler.h>
+#include <labcomm2014_private.h>
+#include <hijacknet.h>
+#if 0
+#include <labcomm_private.h>
+#include <lth/lth_net/lth_net.h>
+#include <lth/lth_net/lth_net_defs.h>
+#include "extctrl_irc5_main_20120112.h"
+#endif
+
+#define EXTCTRL2015_ETH_ID (('E' << 8) + ('X'))
+#define flag_MASK 0x07
+#define flag_LAST_FRAG 0x01
+#define flag_NEED_ACK 0x02
+#define flag_IS_ACK 0x04
+#define kind_MASK 0x30
+#define kind_INIT 0x10
+#define kind_DATA 0x20
+
+
+/*
+we need an /hd0a/orca/robotdata
+
+ac0.value[0] ac0.value[1] ac0.value[2] ac0.value[3] id0
+...
+acN.value[0] acN.value[1] acN.value[2] acN.value[3] idN
+*/
+
+#define MILLISECONDS_TO_TICK(ms) ((ms) * 1000 / sysClkRateGet())
+#define TIMING_BINS 400
+
+#define MAX_JOINT 128
+#define MAX_ROBOT 4
+#define MAX_FORCE_SENSORS 1
+#define BUFFER_SIZE 1500
+
+extern unsigned long int cpuFrequency;
+
+extern void moc_io_get_force_and_torque_feedback(int index,
+ float values[6]);
+
+typedef enum {
+ s_disconnected,
+ s_send_syn,
+ s_get_syn,
+ s_setup,
+ s_send_setup_syn,
+ s_connected,
+ s_submitting,
+ s_obtaining,
+ s_error
+} state_t;
+
+static char *state_name[] = {
+ "disconnected",
+ "send_syn",
+ "get_syn",
+ "setup",
+ "send_setup_syn",
+ "connected",
+ "submitting",
+ "obtaining",
+ "error"
+};
+
+static struct state {
+ state_t state;
+ SEM_ID mutex;
+ SEM_ID sem;
+ SEM_ID sem_communicate;
+ HIJACKNET_DRIVER *driver;
+ unsigned char src[6];
+ unsigned char dst[6];
+ int channel;
+ int cookie;
+ struct encoder_writer {
+ struct labcomm2014_writer writer;
+ struct labcomm2014_writer_action_context action_context;
+ int need_ack;
+ int frag_start;
+ unsigned char buffer[BUFFER_SIZE];
+ } writer;
+ struct {
+ struct labcomm2014_encoder *encoder;
+ int sequence;
+ } encoder;
+ struct {
+ struct labcomm2014_decoder *decoder;
+ int sequence;
+ int fragment;
+ int pos;
+ int valid;
+ unsigned char buffer[1500];
+ } decoder;
+ unsigned int t_submit_active[MAX_ROBOT];
+ unsigned int t_submit[MAX_ROBOT];
+ unsigned int t_obtain_active[MAX_ROBOT];
+ unsigned int t_obtain[MAX_ROBOT];
+ unsigned int t_communicate_active[MAX_ROBOT];
+ unsigned int t_communicate_submit[MAX_ROBOT];
+ unsigned int t_communicate_obtain[MAX_ROBOT];
+ int communicate_count[MAX_ROBOT];
+ int loops[MAX_ROBOT];
+ int ext2irb_updated;
+ int submitted;
+ int missed;
+ int total_missed;
+ unsigned int cpu_MHz;
+ unsigned int timing[TIMING_BINS][MAX_ROBOT];
+
+ int maxrobot;
+ axis_configuration_t ac[MAX_ROBOT];
+ int kind[MAX_ROBOT];
+ int axis_count[MAX_ROBOT];
+ extctrl2014_irb2ext_joint_net irb2ext_joint[MAX_JOINT];
+ extctrl2014_ext2irb_joint_net ext2irb_joint[MAX_JOINT];
+ extctrl2014_irb2ext_robot_net irb2ext_robot[MAX_ROBOT];
+ extctrl2014_ext2irb_robot_net ext2irb_robot[MAX_ROBOT];
+ extctrl2014_extctrl_communication_data ext2rapid[MAX_ROBOT];
+ extctrl2014_extctrl_communication_data rapid2ext[MAX_ROBOT];
+ extctrl2014_irb2ext_net irb2ext;
+ extctrl2014_ext2irb_net ext2irb;
+ int force_sensors;
+ extctrl2014_force_torque_vector force[MAX_FORCE_SENSORS];
+ extctrl2014_force_torque_net force_net;
+} state;
+
+#if 0
+
+#define PROTOCOL_ERROR \
+ printf("PROTOCOL ERROR %s:%d\n", __FUNCTION__, __LINE__); \
+ state->state = s_error;
+
+static inline unsigned int rdtsc()
+{
+ // Since we have at least one system with broken rdtsc (high and low word
+ // might be updated during read), use just low word
+
+ unsigned int result;
+ __asm__ __volatile__("rdtsc" : "=a" (result) : /* no inputs */ : "%edx");
+ return result;
+}
+
+static int get_robot_index(axis_configuration_t ac)
+{
+ int i;
+
+ for (i = 0 ; i < state.maxrobot ; i++) {
+ if (state.ac[i].value[0] != ac.value[0]) { continue; };
+ if (state.ac[i].value[1] != ac.value[1]) { continue; };
+ if (state.ac[i].value[2] != ac.value[2]) { continue; };
+ if (state.ac[i].value[3] != ac.value[3]) { continue; };
+ return i;
+ }
+ return -1;
+}
+
+static int writer(labcomm_writer_t *w, labcomm_writer_action_t action)
+{
+ int result = 0;
+ struct state *state;
+
+ //printf("%s: %d\n", __FUNCTION__, action);
+ state = w->context;
+ switch ((int)action) { /* Cast needed to avoid compile warning
+ at LABCOMM_USER_ACTION below */
+ case labcomm_writer_alloc: {
+ w->data = state->encoder.buffer;
+ w->data_size = sizeof(state->encoder.buffer);
+ w->count = sizeof(state->encoder.buffer);
+ w->pos = 22;
+ result = sizeof(state->encoder.buffer);
+ } break;
+ case labcomm_writer_free: {
+ // Ignored
+ } break;
+ case labcomm_writer_start: {
+ if (state->state == s_setup) {
+ // Reserve space for header only during setup, when in data
+ // exchange mode, position reset is done after each flush
+ w->pos = 22;
+ }
+ } break;
+ case labcomm_writer_continue: {
+ PROTOCOL_ERROR;
+ } break;
+ case labcomm_writer_end: {
+ if (state->state == s_setup) {
+ int sequence = state->encoder.sequence;
+
+ while (sequence == state->encoder.sequence) {
+ int pos;
+
+ // Write header into packet
+ memcpy(&w->data[0], state->dst, 6);
+ memcpy(&w->data[6], state->src, 6);
+ pos = w->pos;
+ w->pos = 12;
+ labcomm_write_short(w, LTH_NET_ID);
+ labcomm_write_int(w, sequence);
+ labcomm_write_short(w, 0);
+ labcomm_write_short(w, LTH_NET_SETUP_PACKET | (pos - 22));
+ w->pos = pos;
+ lth_net_send(state->unit, w->data, w->pos);
+ semTake(state->sem, MILLISECONDS_TO_TICK(1000));
+ }
+ w->pos = 22;
+ }
+ } break;
+ case labcomm_writer_available: {
+ } break;
+ case LABCOMM_USER_ACTION(1): {
+ int pos;
+
+ // Write header into packet
+ memcpy(&w->data[0], state->dst, 6);
+ memcpy(&w->data[6], state->src, 6);
+ pos = w->pos;
+ w->pos = 12;
+ labcomm_write_short(w, LTH_NET_ID);
+ labcomm_write_int(w, state->encoder.sequence);
+ labcomm_write_short(w, 0);
+ labcomm_write_short(w, pos - 22);
+ w->pos = pos;
+ lth_net_send(state->unit, w->data, w->pos);
+ state->encoder.sequence++;
+ w->pos = 22;
+ } break;
+ default: {
+ printf("Unhandled action: %d\n", action);
+ } break;
+ }
+
+ return result;
+}
+
+static int reader(labcomm_reader_t *r, labcomm_reader_action_t action)
+{
+ int result = 0;
+ struct state *state;
+ //printf("%s: %d (%d)\n", __FUNCTION__, action, r->pos);
+ state = r->context;
+ switch (action) {
+ case labcomm_reader_alloc: {
+ r->data = state->decoder.buffer;
+ r->data_size = sizeof(state->decoder.buffer);
+ r->count = 0;
+ result = sizeof(state->decoder.buffer);
+ } break;
+ case labcomm_reader_free: {
+ // Ignored
+ } break;
+ case labcomm_reader_start: {
+ r->pos = 0;
+ if (state->decoder.valid) {
+ r->count = state->decoder.pos;
+ } else {
+ r->count = 0;
+ }
+ result = r->count;
+ } break;
+ case labcomm_reader_continue: {
+ PROTOCOL_ERROR;
+ } break;
+ case labcomm_reader_end: {
+ state->decoder.valid = 0;
+ } break;
+ }
+ return result;
+}
+
+static void send_flags(int unit, int sequence, int flags)
+{
+ unsigned char buf[22];
+ struct labcomm_writer w;
+
+ //printf("send_flags sequence=%x flags=%x\n", sequence, flags);
+ labcomm_buffer_writer_setup(&w, buf, sizeof(buf));
+ memcpy(&w.data[0], state.dst, 6);
+ memcpy(&w.data[6], state.src, 6);
+ w.pos += 12;
+ labcomm_write_short(&w, LTH_NET_ID);
+ labcomm_write_int(&w, sequence);
+ labcomm_write_short(&w, 0);
+ labcomm_write_short(&w, flags);
+ lth_net_send(0, w.data, w.pos);
+}
+
+void ext2irb_net_handler(extctrl2014_ext2irb_net *v,
+ void *context)
+{
+ struct state *state = context;
+ int i, j;
+
+ // Mutex should already be taken here
+ state->ext2irb_updated = 1;
+
+ if (state->ext2irb.robot.n_0 != v->robot.n_0) {
+ // Robot count mismatch
+ goto out;
+ }
+ for (i = 0 ; i < v->robot.n_0 ; i++) {
+ if (state->ext2irb_robot[i].joint.n_0 != v->robot.a[i].joint.n_0) {
+ // Joint count mismatch
+ goto out;
+ }
+ for (j = 0 ; j < v->robot.a[i].joint.n_0 ; j++) {
+ state->ext2irb.robot.a[i].joint.a[j] = v->robot.a[i].joint.a[j];
+ }
+ if (v->robot.a[i].mocgendata.n_0) {
+ // No more resend of rapid data needed
+ state->irb2ext_robot[i].mocgendata.n_0 = 0;
+ // Indicate RAPID data received
+ state->ext2irb_robot[i].mocgendata.n_0 = 1;
+ state->ext2irb_robot[i].mocgendata.a[0] = v->robot.a[i].mocgendata.a[0];
+// semGive(state->sem_communicate);
+ }
+ }
+
+ semGive(state->sem);
+out:
+ ;
+}
+
+static void send_to_ext() {
+ semTake(state.sem, 0);
+ if (state.force_torque_send) {
+ int i;
+
+ for (i = 0 ; i < state.force_torque_send ; i++) {
+ moc_io_get_force_and_torque_feedback (i, state.force[i].a);
+ }
+ labcomm_encode_extctrl2014_force_torque_net(
+ state.encoder.encoder, &state.force_net);
+ }
+ labcomm_encode_extctrl2014_irb2ext_net(state.encoder.encoder,
+ &state.irb2ext);
+ labcomm_internal_encoder_user_action(state.encoder.encoder,
+ LABCOMM_USER_ACTION(1));
+}
+
+static int submit_active(axis_configuration_t ac)
+{
+ int result = 0;
+ int current_robot;
+
+ current_robot = get_robot_index(ac);
+ if (current_robot >= 0) {
+ state.loops[current_robot]++;
+ state.t_submit_active[current_robot] = rdtsc();
+ if (state.state == s_submitting || state.state == s_obtaining) {
+ result = 1;
+ }
+ }
+ return result;
+}
+
+static int submit(controller_state ctrl_state,
+ axis_configuration_t ac,
+ EXTCTRL_IRB2EXT_JOINT *src)
+{
+ int result = -1;
+ int current_robot;
+
+ current_robot = get_robot_index(ac);
+ if (current_robot >= 0) {
+ int i, axis_count;
+ extctrl2014_irb2ext_joint_abs *dst;
+
+ state.t_submit[current_robot] = rdtsc();
+ state.submitted = 1;
+ semTake(state.mutex, WAIT_FOREVER);
+
+ axis_count = state.axis_count[current_robot];
+ dst = state.irb2ext.robot.a[current_robot].joint.a;
+ state.irb2ext.robot.a[current_robot].kind = state.kind[current_robot];
+ state.irb2ext.obtaining = state.state == s_obtaining;
+ state.irb2ext.manualMode = ctrl_state.manualMode;
+ state.irb2ext.controlActive = ctrl_state.controlActive;
+ for (i = 0 ; i < axis_count ; i++) {
+ // Control parameters
+ dst[i].parKp = src[i].parKp;
+ dst[i].parKv = src[i].parKv;
+ dst[i].parKi = src[i].parKi;
+ dst[i].parTrqMin = src[i].parTrqMin;
+ dst[i].parTrqMax = src[i].parTrqMax;
+
+ // Exposed (measured or computed) states
+ dst[i].posRawAbs = src[i].posRawAbs;
+ dst[i].posRawFb = src[i].posRawFb;
+ dst[i].posFlt = src[i].posFlt;
+ dst[i].velRaw = src[i].velRaw;
+ dst[i].velFlt = src[i].velFlt;
+ dst[i].velOut = src[i].velOut;
+ dst[i].trqRaw = src[i].trqRaw;
+ dst[i].trqRefFlt = src[i].trqRefFlt;
+
+ // State references and feedforward signals (sum of torques are applied)
+ dst[i].posRef = src[i].posRef;
+ dst[i].velRef = src[i].velRef;
+ dst[i].trqFfw = src[i].trqFfw;
+ dst[i].trqFfwGrav = src[i].trqFfwGrav;
+ }
+
+ semGive(state.mutex);
+ result = 0;
+ }
+ return result;
+}
+
+static int obtain_active(axis_configuration_t ac)
+{
+ int result = 0;
+ int current_robot;
+
+ if (state.submitted) {
+ state.submitted = 0;
+ send_to_ext();
+ }
+ current_robot = get_robot_index(ac);
+ if (current_robot >= 0) {
+ state.t_obtain_active[current_robot] = rdtsc();
+ if (state.state == s_obtaining) {
+ result = 1;
+ }
+ }
+ return result;
+}
+
+static int obtain(axis_configuration_t ac,
+ EXTCTRL_EXT2IRB_JOINT *dst)
+{
+ int result = -1;
+ int current_robot;
+
+ current_robot = get_robot_index(ac);
+ if (current_robot == 0) {
+ if (semTake(state.sem, 2) == 0 && state.ext2irb_updated) {
+ state.missed = 0;
+ } else {
+ state.total_missed++;
+ state.missed++;
+ }
+ state.ext2irb_updated = 0;
+ }
+
+ if (current_robot >= 0) {
+ int i, axis_count, bin;
+
+ semTake(state.mutex, WAIT_FOREVER);
+ state.t_obtain[current_robot] = rdtsc();
+ // 10 us bins
+ bin = ((state.t_obtain[current_robot] -
+ state.t_submit_active[current_robot]) /
+ (state.cpu_MHz * (10 + 1)));
+ if (bin > TIMING_BINS) {
+ bin = TIMING_BINS - 1;
+ }
+ state.timing[bin][current_robot]++;
+
+ if (state.missed <= 2) {
+ extctrl2014_ext2irb_joint_offset *offset;
+ extctrl2014_irb2ext_joint_abs *ref;
+
+ axis_count = state.axis_count[current_robot];
+ ref = state.irb2ext.robot.a[current_robot].joint.a;
+ offset = state.ext2irb.robot.a[current_robot].joint.a;
+ for (i = 0 ; i < axis_count ; i++) {
+ // Possibly updated control parameters
+ dst[i].parKp = offset[i].parKp;
+ dst[i].parKv = offset[i].parKv;
+ dst[i].parKi = offset[i].parKi;
+
+ // Modified references and feed forward
+ // YES, we are applying offsets from last seen sample to this sample
+ // even if we have missed a few samples...
+ dst[i].posRef = ref[i].posRef + offset[i].posOffset;
+ dst[i].velRef = ref[i].velRef + offset[i].velOffset;
+ dst[i].trqFfw = ref[i].trqFfw + offset[i].trqFfwOffset;
+ }
+ result = 0;
+ }
+ }
+ semGive(state.mutex);
+
+ return result;
+}
+
+static int communicate_active(axis_configuration_t ac)
+{
+ int result = 0;
+ int current_robot;
+
+ // Never called? Ignored??
+ current_robot = get_robot_index(ac);
+ if (current_robot >= 0) {
+ state.t_communicate_active[current_robot] = rdtsc();
+ if (state.state == s_submitting || state.state == s_obtaining) {
+ result = 1;
+ }
+ }
+ return result;
+}
+
+static int communicate(axis_configuration_t ac,
+ EXTCTRL_COMMUNICATION_DATA *data)
+{
+ int result = -1;
+ int current_robot;
+
+ current_robot = get_robot_index(ac);
+ if (current_robot >= 0) {
+ // Temporary workaround while ABB locks networking while calling
+ // extctrl_communicate:
+ // 1. Send data where data.instruction is even
+ // 2. Receiva data when data.instruction is odd,
+ // only return result if received
+ // packet.instruction == data.instruction - 1
+ if ((data->instruction & 1) == 0) {
+ // Send packet
+ if (state.state == s_submitting || state.state == s_obtaining) {
+ extctrl2014_extctrl_communication_data *dst;
+
+ state.communicate_count[current_robot]++;
+ state.t_communicate_submit[current_robot] = rdtsc();
+ dst = state.irb2ext.robot.a[current_robot].mocgendata.a;
+ state.irb2ext.robot.a[current_robot].mocgendata.n_0 = 1;
+// semTake(state.sem_communicate, 0);
+ semTake(state.mutex, WAIT_FOREVER);
+ dst->instruction = data->instruction;
+ dst->value1 = data->value1;
+ dst->value2 = data->value2;
+ dst->value3 = data->value3;
+ dst->value4 = data->value4;
+ dst->value5 = data->value5;
+ dst->value6 = data->value6;
+ memcpy(dst->string1.a, data->str1, sizeof(data->str1));
+ memcpy(dst->string2.a, data->str2, sizeof(data->str2));
+ semGive(state.mutex);
+ result = 0;
+ }
+ } else {
+ // Check if there is a received packet
+ if (state.state == s_obtaining) {
+ extctrl2014_extctrl_communication_data *src;
+
+ src = state.ext2irb.robot.a[current_robot].mocgendata.a;
+ // semTake(state.sem_communicate, 8);
+
+ state.t_communicate_obtain[current_robot] = rdtsc();
+ semTake(state.mutex, WAIT_FOREVER);
+ if (src->instruction == data->instruction - 1) {
+ // We got valid a response to the RAPID data
+ data->value1 = src->value1;
+ data->value2 = src->value2;
+ data->value3 = src->value3;
+ data->value4 = src->value4;
+ data->value5 = src->value5;
+ data->value6 = src->value6;
+ memcpy( data->str1, src->string1.a, sizeof(data->str1));
+ memcpy( data->str2, src->string2.a, sizeof(data->str2));
+ result = 0;
+ }
+ src->instruction = 0;
+ semGive(state.mutex);
+ }
+ }
+ }
+ return result;
+}
+
+static int hook(int ether_unit, void *data, int length, void *context)
+{
+ unsigned char *buf = data;
+ struct state *state = context;
+ lth_net_head_t head;
+ int dump = 1;
+
+ semTake(state->mutex, WAIT_FOREVER);
+ //printf("%s: %d\n", __FUNCTION__, length);
+ // TODO: validate ethernet addresses and kind
+ if (lth_net_head_valid(&buf[12], length - 12, &head)) {
+ dump = 0;
+ if (head.length == 0) {
+ // Flag only packet
+ if (head.flags == LTH_NET_SYN_PACKET) {
+ if (state->state == s_send_syn || state->state == s_get_syn) {
+ state->decoder.sequence = head.sequence;
+ state->state = s_setup;
+ semGive(state->sem);
+ }
+ if (state->state == s_setup &&
+ state->decoder.sequence == head.sequence) {
+ send_flags(state->unit, head.sequence,
+ LTH_NET_ACK_PACKET | LTH_NET_SYN_PACKET);
+ }
+ } else if (head.flags == (LTH_NET_ACK_PACKET | LTH_NET_SYN_PACKET) &&
+ head.sequence == state->encoder.sequence) {
+ if (state->state == s_send_syn) {
+ state->state = s_get_syn;
+ semGive(state->sem);
+ }
+ } else if (head.flags == (LTH_NET_SETUP_PACKET | LTH_NET_ACK_PACKET) &&
+ head.sequence == state->encoder.sequence) {
+ state->encoder.sequence++;
+ semGive(state->sem);
+ } else if (head.flags == (LTH_NET_SETUP_PACKET | LTH_NET_SYN_PACKET)) {
+ send_flags(state->unit, head.sequence,
+ LTH_NET_SETUP_PACKET | LTH_NET_ACK_PACKET |
+ LTH_NET_SYN_PACKET);
+ } else if (head.flags == (LTH_NET_SETUP_PACKET | LTH_NET_ACK_PACKET |
+ LTH_NET_SYN_PACKET)) {
+ if (state->state == s_send_setup_syn &&
+ head.sequence == state->encoder.sequence) {
+ state->state = s_connected;
+ state->encoder.sequence++;
+ semGive(state->sem);
+ }
+ } else {
+ printf("UNHANDLED\n");
+ dump = 1;
+ }
+ } else {
+ // Data packet
+ int send_ack = 0;
+
+ if ((head.flags & LTH_NET_MORE_FRAGMENTS) == 0 &&
+ head.sequence == state->decoder.sequence - 1) {
+ send_ack = 1;
+ } else if (((state->state == s_setup &&
+ head.sequence == state->decoder.sequence) ||
+ (state->state == s_connected ||
+ state->state == s_submitting ||
+ state->state == s_obtaining)) &&
+ state->decoder.valid == 0) {
+ if (head.fragment == 0) {
+ state->decoder.pos = 0;
+ state->decoder.fragment = head.fragment;
+ }
+ if (state->decoder.fragment == head.fragment &&
+ state->decoder.pos + length - 22 < sizeof(state->decoder.buffer)) {
+ memcpy(&state->decoder.buffer[state->decoder.pos], &buf[22],
+ length - 22);
+ state->decoder.pos += length - 22;
+ state->decoder.fragment++;
+ if ((head.flags & LTH_NET_MORE_FRAGMENTS) == 0) {
+ send_ack = 1;
+ state->decoder.sequence++;
+ state->decoder.valid = 1;
+ // TODO: while loop?
+ labcomm_decoder_decode_one(state->decoder.decoder);
+ }
+ }
+ } else {
+ printf("Unhandled data %d %d %d %d\n",
+ state->state == s_setup,
+ head.sequence == state->decoder.sequence,
+ state->state == s_connected,
+ state->decoder.valid == 0);
+// dump = 1;
+ }
+ if (send_ack && (head.flags & LTH_NET_SETUP_PACKET)) {
+ send_flags(state->unit, head.sequence,
+ LTH_NET_SETUP_PACKET | LTH_NET_ACK_PACKET);
+ }
+ }
+ }
+ semGive(state->mutex);
+ if (dump) {
+ int i;
+
+ for (i = 0 ; i < length ; i++) {
+ if (i > 0 && i % 16 == 0) { printf("\n"); }
+ printf("%02x ", buf[i]);
+ }
+ printf("\n");
+ }
+ return 0;
+}
+
+static int hijacknet_extctrl2014_connect()
+{
+ printf("Connnect\n");
+
+ state.encoder.sequence = rdtsc();
+ labcomm_decoder_register_extctrl2014_ext2irb_net(
+ state.decoder.decoder, ext2irb_net_handler, &state);
+ while (1) {
+ state_t current_state;
+
+ semTake(state.sem, MILLISECONDS_TO_TICK(1000));
+ semTake(state.mutex, WAIT_FOREVER);
+ current_state = state.state;
+ semGive(state.mutex);
+
+ printf("state: %d %s\n", current_state, state_name[current_state]);
+ switch (current_state) {
+ case s_uninstalled: {
+ goto out;
+ } break;
+ case s_send_syn: {
+ semTake(state.mutex, WAIT_FOREVER);
+ send_flags(state.unit, state.encoder.sequence, LTH_NET_SYN_PACKET);
+ semGive(state.mutex);
+ } break;
+ case s_setup: {
+ labcomm_encoder_register_extctrl2014_irb2ext_net(
+ state.encoder.encoder);
+ if (state.force_torque_send) {
+ state.force_net.n_0 = state.force_torque_send;
+ state.force_net.a = state.force;
+ labcomm_encoder_register_extctrl2014_force_torque_net(
+ state.encoder.encoder);
+ }
+ semTake(state.mutex, WAIT_FOREVER);
+ state.state = s_send_setup_syn;
+ semGive(state.mutex);
+ } break;
+ case s_send_setup_syn: {
+ semTake(state.mutex, WAIT_FOREVER);
+ send_flags(state.unit, state.encoder.sequence,
+ LTH_NET_SETUP_PACKET |LTH_NET_SYN_PACKET);
+ semGive(state.mutex);
+ } break;
+ case s_connected: {
+ printf("A\n");
+ extctrl_install_callbacks(submit_active, submit,
+ obtain_active, obtain,
+ communicate_active, communicate);
+ printf("B\n");
+ goto out;
+ } break;
+ default: {
+ semTake(state.mutex, WAIT_FOREVER);
+ printf("Unhandled State: %d\n", state.state);
+ semGive(state.mutex);
+ } break;
+
+ }
+ }
+out:
+ printf("Connected\n");
+ return 0;
+}
+
+
+
+
+int hijacknet_extctrl2014_debug()
+{
+#define PRINT_FIELD(label, field) { \
+ int i; \
+ printf(" %-17s", label ":"); \
+ for (i = 0 ; i < state.maxrobot ; i++) { \
+ printf("%10d ", state.field[i]); \
+ } \
+ printf("\n"); \
+ }
+ PRINT_FIELD("kind", kind);
+#undef PRINT_FIELD
+ printf(" obtaining: %d\n", state.irb2ext.obtaining);
+ printf(" manualMode: %d\n", state.irb2ext.manualMode);
+ printf(" controlActive: %d\n", state.irb2ext.controlActive);
+ {
+ int i, j, n;
+
+ for (i = 0; i < MAX_JOINT ; i++) {
+ n = 0;
+ for (j = 0 ; j < state.maxrobot ; j++) {
+ if (state.irb2ext.robot.a[j].joint.n_0 > i) {
+ n++;
+ }
+ }
+ if (n) {
+ printf(" *** joint %d ***\n", i);
+#define PRINT_FIELD(label, field1, field2) { \
+ int j; \
+ printf(" %-17s", label ":"); \
+ for (j = 0 ; j < state.maxrobot ; j++) { \
+ if (state.irb2ext.robot.a[j].joint.n_0 > i) { \
+ printf("%10g (%10g) ", \
+ state.ext2irb.robot.a[j].joint.a[i].field1, \
+ state.irb2ext.robot.a[j].joint.a[i].field2); \
+ } else { \
+ printf("%10s %12s", "", ""); \
+ } \
+ } \
+ printf("\n"); \
+ }
+ PRINT_FIELD("parKp", parKp, parKp);
+ PRINT_FIELD("parKv", parKv, parKv);
+ PRINT_FIELD("parKi", parKi, parKi);
+ PRINT_FIELD("posOffset", posOffset, posRef);
+ PRINT_FIELD("velOffset", velOffset, velRef);
+ PRINT_FIELD("trqFfwOffset", trqFfwOffset, trqFfw);
+#undef PRINT_FIELD
+ }
+ }
+ }
+ {
+ int i;
+
+ for (i = 0 ; i < MAX_FORCE_SENSORS ; i++) {
+ float v[6];
+
+ moc_io_get_force_and_torque_feedback (i, v);
+ printf("F/T#%d %8g %8g %8g %8g %8g %8g %s\n",
+ i, v[0], v[1], v[2], v[3], v[4], v[5],
+ (i < state.force_torque_send)? "active" : "inactive");
+ }
+ }
+ return 0;
+}
+
+int hijacknet_extctrl2014_zero_counters()
+{
+ int i,j ;
+
+ state.total_missed = 0;
+ state.missed = 0;
+ for (i = 0 ; i < state.maxrobot ; i++) {
+ state.loops[i] = 0;
+ for (j = 0 ; j < TIMING_BINS ; j++) {
+ state.timing[j][i] = 0;
+ }
+ }
+ return 0;
+}
+
+int extctrl_stop_submit()
+{
+ int result = -1;
+
+ if (state.state != s_submitting && state.state != s_obtaining) {
+ printf("%s: not in states { submitting, obtaining }\n", __FUNCTION__);
+ } else {
+ state.state = s_connected;
+ result = 0;
+ }
+ return result;
+}
+
+int extctrl_start_submit()
+{
+ int result = -1;
+
+ if (state.state != s_connected && state.state != s_obtaining) {
+ printf("%s: not in states { installed, obtaining }\n", __FUNCTION__);
+ } else {
+ state.state = s_submitting;
+ result = 0;
+ }
+ return result;
+}
+
+
+#endif
+
+int extctrl_stop_obtain()
+{
+ int result = -1;
+
+// void *x = labcomm2014_encoder_new;
+// printf("%p\n", x);
+
+ if (state.state != s_obtaining) {
+ printf("%s: not in states { obtaining }\n", __FUNCTION__);
+ } else {
+ state.state = s_submitting;
+ result = 0;
+ }
+ return result;
+}
+
+int extctrl_start_obtain()
+{
+ int result = -1;
+
+ if (state.state != s_submitting) {
+ printf("%s: not in states { submitting }\n", __FUNCTION__);
+ } else {
+ state.state = s_obtaining;
+ result = 0;
+ }
+ return result;
+}
+
+int hijacknet_extctrl2014_status()
+{
+ printf("%s: \n", __FUNCTION__);
+ printf(" Built: %s %s\n", __DATE__, __TIME__);
+ printf(" State: %s (%d)\n", state_name[state.state], state.state);
+ printf(" Destination: %02x:%02x:%02x:%02x:%02x:%02x\n",
+ state.dst[0], state.dst[1], state.dst[2],
+ state.dst[3], state.dst[4], state.dst[5]);
+ printf(" Source: %02x:%02x:%02x:%02x:%02x:%02x\n",
+ state.src[0], state.src[1], state.src[2],
+ state.src[3], state.src[4], state.src[5]);
+#define PRINT_FIELD(label, field) { \
+ int i; \
+ printf(" %-17s", label ":"); \
+ for (i = 0 ; i < state.maxrobot ; i++) { \
+ printf("%10d ", state.field[i]); \
+ } \
+ printf("\n"); \
+ }
+
+ printf(" missed: %10d %10d\n", state.total_missed, state.missed);
+ PRINT_FIELD("loops", loops);
+ PRINT_FIELD("RAPID comm", communicate_count);
+ PRINT_FIELD("t_submit_active", t_submit_active);
+ PRINT_FIELD("t_submit", t_submit);
+ PRINT_FIELD("t_obtain_active", t_obtain_active);
+ PRINT_FIELD("t_obtain", t_obtain);
+ PRINT_FIELD("t_comm_active", t_communicate_active);
+ PRINT_FIELD("t_comm_submit", t_communicate_submit);
+ PRINT_FIELD("t_comm_obtain", t_communicate_obtain);
+#undef PRINT_FIELD
+ {
+ int i, j;
+
+ for (i = 0 ; i < TIMING_BINS ; i++) {
+ int n = 0;
+ for (j = 0 ; j < state.maxrobot ; j++) {
+ n += state.timing[i][j];
+ }
+ if (n) {
+ printf(" %4d - %4d: ", i*10, (i+1) * 10);
+ for (j = 0 ; j < state.maxrobot ; j++) {
+ printf("%d ", state.timing[i][j]);
+ }
+ printf("\n");
+ }
+ }
+ }
+ return state.state;
+}
+
+static int writer_alloc(
+ struct labcomm2014_writer *w,
+ struct labcomm2014_writer_action_context *action_context)
+{
+ printf("%s %s %s\n", __FUNCTION__, __DATE__, __TIME__);
+ w->data = state.writer.buffer;
+ w->data_size = BUFFER_SIZE;
+ w->count = BUFFER_SIZE;
+ w->pos = 0;
+
+ return w->error;
+}
+
+static int writer_free(
+ struct labcomm2014_writer *w,
+ struct labcomm2014_writer_action_context *action_context)
+{
+ printf("%s %s %s\n", __FUNCTION__, __DATE__, __TIME__);
+ w->data = NULL;
+ w->data_size = 0;
+ w->count = 0;
+ w->pos = 0;
+
+ return w->error;
+}
+
+static int writer_start(
+ struct labcomm2014_writer *w,
+ struct labcomm2014_writer_action_context *action_context,
+ int index,
+ const struct labcomm2014_signature *signature,
+ void *value)
+{
+ int cookie = state.cookie;
+ unsigned char flags;
+
+ printf("%s %p %p\n", __FUNCTION__, signature, value);
+ w->pos = 0;
+ if (signature != NULL && value != NULL) {
+ /* DATA packet */
+ state.writer.need_ack = 0;
+ flags = kind_DATA | flag_LAST_FRAG;
+ } else {
+ state.writer.need_ack = 1;
+ if (signature == NULL && value == NULL) {
+ /* INIT packet, labcomm not involved */
+ flags = kind_INIT | flag_LAST_FRAG | flag_NEED_ACK;
+ cookie = 0;
+ } else if (signature == NULL) {
+ /* ID packet */
+ flags = kind_DATA | flag_LAST_FRAG | flag_NEED_ACK;
+ } else {
+ /* signature packet */
+ flags = kind_DATA | flag_LAST_FRAG | flag_NEED_ACK;
+ }
+ }
+
+ // Write header into packet
+ memcpy(&w->data[0], state.dst, 6);
+ memcpy(&w->data[6], state.src, 6);
+ w->pos = 12;
+ labcomm2014_write_short(w, EXTCTRL2015_ETH_ID);
+ labcomm2014_write_byte(w, flags);
+ labcomm2014_write_packed32(w, state.channel);
+ labcomm2014_write_int(w, cookie);
+ labcomm2014_write_int(w, index);
+ /* reserve maximum space for frag_num and frag_length */
+ state.writer.frag_start = w->pos;
+ w->pos += 10;
+
+ return w->error;
+}
+
+static int writer_end(
+ struct labcomm2014_writer *w,
+ struct labcomm2014_writer_action_context *action_context)
+{
+ int first, last, length, offset;
+
+ first = state.writer.frag_start + 10;
+ last = w->pos;
+ length = last - first;
+ w->pos = state.writer.frag_start;
+ labcomm2014_write_packed32(w, 0); /* frag_num */
+ labcomm2014_write_packed32(w, length); /* frag_length */
+ offset = first - w->pos;
+ w->data[last] = 'X';
+ memmove(&w->data[w->pos], &w->data[first], length);
+ last = last - offset;
+ w->data[last] = 'Y';
+ printf("%s %d %d %d %d\n", __FUNCTION__, first, last, length, offset);
+ hijacknet_send(state.driver, w->data, last + offset);
+ w->pos = 0;
+
+ return w->error;
+}
+
+static int writer_flush(
+ struct labcomm2014_writer *w,
+ struct labcomm2014_writer_action_context *action_context)
+{
+ printf("%s %s %s\n", __FUNCTION__, __DATE__, __TIME__);
+ w->pos = 0;
+
+ return w->error;
+}
+
+static const struct labcomm2014_writer_action writer_action = {
+ .alloc = writer_alloc,
+ .free = writer_free,
+ .start = writer_start,
+ .end = writer_end,
+ .flush = writer_flush,
+ .ioctl = NULL
+};
+
+static void init_writer()
+{
+ state.writer.action_context.next = NULL;
+ state.writer.action_context.action = &writer_action;
+ state.writer.action_context.context = &state.writer;
+ state.writer.writer.action_context = &state.writer.action_context;
+ state.writer.writer.memory = NULL;
+ state.writer.writer.data = state.writer.buffer;
+ state.writer.writer.data_size = BUFFER_SIZE;
+ state.writer.writer.count = BUFFER_SIZE;
+ state.writer.writer.pos = 0;
+}
+
+static int read_robot_data(char *path)
+{
+ int result = 0;
+ FILE *f;
+ int maxjoint;
+
+ // Read arm configuration
+ f = fopen(path, "r");
+ if (f == NULL) {
+ result = -1;
+ goto out;
+ }
+ state.maxrobot = 0;
+ maxjoint = 0;
+ state.irb2ext.robot.a = state.irb2ext_robot;
+ state.ext2irb.robot.a = state.ext2irb_robot;
+ while (1) {
+ int n, joints;
+ unsigned int ac[4], kind;
+
+ n = fscanf(f, "%x %x %x %x %x", &ac[0], &ac[1], &ac[2], &ac[3], &kind);
+ if (n == EOF) { break; }
+ if (n == 5) {
+ if (state.maxrobot > MAX_ROBOT) {
+ printf("Too many robots in %s\n", path);
+ result = -1;
+ goto close;
+ }
+ state.ac[state.maxrobot].value[0] = ac[0];
+ state.ac[state.maxrobot].value[1] = ac[1];
+ state.ac[state.maxrobot].value[2] = ac[2];
+ state.ac[state.maxrobot].value[3] = ac[3];
+ state.kind[state.maxrobot] = kind;
+
+ {
+ int i, j;
+
+ joints = 0;
+ for (i = 0 ; i < 4 ; i++) {
+ for (j = 0 ; j < 32 ; j++) {
+ if (ac[i] & (1 << j)) { joints++; }
+ }
+ }
+ }
+ state.axis_count[state.maxrobot] = joints;
+
+ state.irb2ext.robot.a[state.maxrobot].joint.n_0 = joints;
+ state.irb2ext.robot.a[state.maxrobot].joint.a =
+ &state.irb2ext_joint[maxjoint];
+ state.irb2ext.robot.a[state.maxrobot].mocgendata.n_0 = 0;
+ state.irb2ext.robot.a[state.maxrobot].mocgendata.a =
+ &state.rapid2ext[state.maxrobot];
+
+ state.ext2irb.robot.a[state.maxrobot].joint.n_0 = joints;
+ state.ext2irb.robot.a[state.maxrobot].joint.a =
+ &state.ext2irb_joint[maxjoint];
+ state.ext2irb.robot.a[state.maxrobot].mocgendata.n_0 = 0;
+ state.ext2irb.robot.a[state.maxrobot].mocgendata.a =
+ &state.ext2rapid[state.maxrobot];
+
+ maxjoint += joints;
+ state.maxrobot++;
+ }
+ }
+ state.irb2ext.robot.n_0 = state.maxrobot;
+ state.ext2irb.robot.n_0 = state.maxrobot;
+close:
+ fclose(f);
+out:
+ return result;
+}
+
+static int atoe(unsigned char mac[6], char *str)
+{
+ unsigned int buf[6];
+ int err;
+
+ err = sscanf(str, "%x:%x:%x:%x:%x:%x",
+ &buf[0], &buf[1], &buf[2], &buf[3], &buf[4], &buf[5]);
+ if (err == 6) {
+ mac[0] = buf[0];
+ mac[1] = buf[1];
+ mac[2] = buf[2];
+ mac[3] = buf[3];
+ mac[4] = buf[4];
+ mac[5] = buf[5];
+ }
+ return err != 6;
+}
+
+static char* etoa(unsigned char mac[6])
+{
+ static char result[18];
+
+ sprintf(result, "%02x:%02x:%02x:%02x:%02x:%02x",
+ mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
+ return result;
+}
+
+int hijacknet_extctrl2014_connect(char *interface,
+ char *destination,
+ unsigned int channel,
+ unsigned int force_sensors)
+{
+ printf("%s %s %s\n", __FUNCTION__, __DATE__, __TIME__);
+
+ /* Sanity check and save parameters */
+ if (!interface) {
+ printf("No ethernet interface defined\n");
+ state.state = s_error;
+ return -1;
+ }
+ if (!destination) {
+ printf("No ethernet destination address defined\n");
+ state.state = s_error;
+ return -2;
+ }
+ if (atoe(state.dst, destination)) {
+ printf("Invalid ethernet destination address '%s'\n", destination);
+ state.state = s_error;
+ return -3;
+ }
+ state.channel = channel;
+ if (force_sensors > MAX_FORCE_SENSORS) {
+ printf("Too many force torque sensors requested [%d > %d]\n",
+ force_sensors, MAX_FORCE_SENSORS);
+ return -4;
+ } else {
+ state.force_sensors = force_sensors;
+ }
+
+
+ /* Sanity check data sizes */
+ if (sizeof(((EXTCTRL_COMMUNICATION_DATA*)(NULL))->str1) !=
+ sizeof(state.rapid2ext[0].string1.a)) {
+ printf("Size of RAPID string1 differ %d %d\n",
+ sizeof(((EXTCTRL_COMMUNICATION_DATA*)(NULL))->str1),
+ sizeof(state.rapid2ext[0].string1.a));
+ state.state = s_error;
+ return -5;
+ }
+ if (sizeof(((EXTCTRL_COMMUNICATION_DATA*)(NULL))->str2) !=
+ sizeof(state.rapid2ext[0].string2.a)) {
+ printf("Size of RAPID string2 differ %d %d\n",
+ sizeof(((EXTCTRL_COMMUNICATION_DATA*)(NULL))->str2),
+ sizeof(state.rapid2ext[0].string2.a));
+ state.state = s_error;
+ return -6;
+ }
+ if (sizeof(((EXTCTRL_COMMUNICATION_DATA*)(NULL))->str1) !=
+ sizeof(state.ext2rapid[0].string1.a)) {
+ printf("Size of RAPID string1 differ %d %d\n",
+ sizeof(((EXTCTRL_COMMUNICATION_DATA*)(NULL))->str1),
+ sizeof(state.ext2rapid[0].string1.a));
+ state.state = s_error;
+ return -7;
+ }
+ if (sizeof(((EXTCTRL_COMMUNICATION_DATA*)(NULL))->str2) !=
+ sizeof(state.ext2rapid[0].string2.a)) {
+ printf("Size of RAPID string2 differ %d %d\n",
+ sizeof(((EXTCTRL_COMMUNICATION_DATA*)(NULL))->str2),
+ sizeof(state.ext2rapid[0].string2.a));
+ state.state = s_error;
+ return -8;
+ }
+
+ /* Get robot configuration */
+ if (read_robot_data("/hd0a/orca/robotdata") < 0) {
+ printf("Failed to read '/hd0a/orca/robotdata', format (in hex):\n");
+ printf(" ac0 ac1 ac2 ac3 id\n");
+ return -9;
+ }
+
+ init_extctrl2014__signatures();
+ state.driver = hijacknet_open(interface);
+ hijacknet_get_mac_address(state.driver, state.src);
+
+ printf("%s", etoa(state.dst));
+ printf("%s", etoa(state.src));
+
+ hijacknet_extctrl2014_status();
+ init_writer();
+ /* Send INIT */
+ labcomm2014_writer_start(&state.writer.writer,
+ state.writer.writer.action_context,
+ 0, NULL, NULL);
+ labcomm2014_writer_end(&state.writer.writer,
+ state.writer.writer.action_context);
+
+ /* register data types */
+ state.encoder.encoder = labcomm2014_encoder_new(
+ &state.writer.writer,
+ labcomm2014_default_error_handler,
+ labcomm2014_default_memory,
+ labcomm2014_default_scheduler);
+ labcomm2014_encoder_register_extctrl2014_irb2ext_net(
+ state.encoder.encoder);
+ labcomm2014_encoder_register_extctrl2014_force_torque_net(
+ state.encoder.encoder);
+ labcomm2014_encoder_free(state.encoder.encoder);
+ hijacknet_close(state.driver);
+#if 0
+#endif
+// state.decoder.decoder = labcomm2014_decoder_new(reader, &state);
+
+// state.unit = unit;
+ state.cpu_MHz = cpuFrequency / 1000000;
+ state.mutex = semMCreate(SEM_Q_PRIORITY | SEM_INVERSION_SAFE);
+ state.sem = semBCreate(0, 0);
+ state.sem_communicate = semBCreate(0, 0);
+
+#if 0
+ if (lth_net_add_hook(state.unit, hook, &state) != 0) {
+ goto out;
+ }
+ state.state = s_send_syn;
+ state.decoder.valid = 0;
+ taskSpawn("t_connect", 254, 0, 20000, hijacknet_extctrl2014_connect,
+ 0,0,0,0,0,0,0,0,0,0);
+out:
+#endif
+ return 0;
+}
+
+int hijacknet_extctrl2014_disconnect()
+{
+ if (state.state != s_disconnected) {
+ extctrl_install_callbacks(NULL, NULL,
+ NULL, NULL,
+ NULL, NULL);
+// lth_net_del_hook(0, hook);
+ state.state = s_disconnected;
+ semDelete(state.mutex);
+ semDelete(state.sem);
+ semDelete(state.sem_communicate);
+ }
+ return 0;
+}
+
+