diff --git a/common/extctrl2014.lc b/common/extctrl2014.lc
new file mode 100644
index 0000000000000000000000000000000000000000..d22bf6c72036478815a0617d0ab1132049777c1a
--- /dev/null
+++ b/common/extctrl2014.lc
@@ -0,0 +1,125 @@
+/* Types and samples used in controllers */
+
+typedef struct {
+ /* 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 */
+
+ /* State references and feedforward signals (sum of torques are applied): */
+ 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. */
+} irb2ext_joint_abs;
+
+typedef struct {
+ /* Possibly updated control parameters: */
+ float parKp;
+ float parKv;
+ float parKi;
+
+ /* Modified references and feed forward: */
+ double posRef;
+ float velRef;
+ float trqFfw;
+} ext2irb_joint_abs;
+
+typedef struct {
+ int instruction;
+ float value1;
+ float value2;
+ float value3;
+ float value4;
+ float value5;
+ float value6;
+ byte string1[20];
+ byte string2[20];
+} extctrl_communication_data;
+
+typedef struct {
+ int kind;
+ irb2ext_joint_abs joint[_];
+ // mocgendata.instruction != 0 when RAPID has sent data
+ extctrl_communication_data mocgendata;
+} irb2ext_robot_abs;
+
+typedef struct {
+ boolean obtaining;
+ boolean manualMode;
+ boolean controlActive;
+ irb2ext_robot_abs robot[_];
+} irb2ext_type;
+
+typedef struct {
+ ext2irb_joint_abs joint[_];
+ // mocgendata.instruction != 0 when data is sent to RAPID
+ extctrl_communication_data mocgendata;
+} ext2irb_robot_abs;
+
+typedef struct {
+ ext2irb_robot_abs robot[_];
+} ext2irb_type;
+
+sample irb2ext_type irb2ext;
+sample ext2irb_type ext2irb;
+
+/* Types and samples used over the network */
+
+typedef struct {
+ /* Possibly updated control parameters: */
+ float parKp;
+ float parKv;
+ float parKi;
+
+ /* Modified references and feed forward: */
+ double posOffset;
+ float velOffset;
+ float trqFfwOffset;
+} ext2irb_joint_offset;
+
+typedef struct {
+ int kind;
+ irb2ext_joint_abs joint[_];
+ // Length of mocgendata is 1 when RAPID has sent data
+ extctrl_communication_data mocgendata[_];
+} irb2ext_robot_net;
+
+typedef struct {
+ short cookie;
+ boolean obtaining;
+ boolean manualMode;
+ boolean controlActive;
+ irb2ext_robot_net robot[_];
+} irb2ext_net_type;
+
+typedef struct {
+ ext2irb_joint_offset joint[_];
+ // Length of mocgendata is 1 when data is sent back to RAPID
+ extctrl_communication_data mocgendata[_];
+} ext2irb_robot_net;
+
+typedef struct {
+ short cookie;
+ ext2irb_robot_net robot[_];
+} ext2irb_net_type;
+
+typedef irb2ext_joint_abs irb2ext_joint_net;
+typedef ext2irb_joint_offset ext2irb_joint_net;
+typedef float force_torque_vector[6];
+
+sample irb2ext_net_type irb2ext_net;
+sample ext2irb_net_type ext2irb_net;
+sample force_torque_vector force_torque_net[_];
diff --git a/simulator/controller.py b/simulator/controller.py
index ffcc89bd991763608fcddabc0d892cfbb93bbfdb..fa21177fa7096b633841960778625177dca01a2b 100755
--- a/simulator/controller.py
+++ b/simulator/controller.py
@@ -39,7 +39,8 @@ class Arm(extctrl2014_lc.ext2irb_robot_net):
class Robot(extctrl2014_lc.ext2irb_net):
- def __init__(self, arms):
+ def __init__(self, cookie=0, arms=[]):
+ self.cookie = cookie
self.robot = arms
def __repr__(self):
@@ -107,9 +108,10 @@ if __name__ == "__main__":
delta = -delta
offset += delta
print "ROBOT", value
- feedback = Robot([ Arm(joint=[ Joint(posOffset=offset),
- Joint(posOffset=offset*2) ],
- mocgendata=[])])
+ feedback = Robot(cookie=value.cookie,
+ arms=[ Arm(joint=[ Joint(posOffset=offset),
+ Joint(posOffset=offset*2) ],
+ mocgendata=[])])
encoder.encode(feedback,
extctrl2014_lc.ext2irb_net.signature)
diff --git a/simulator/robot.py b/simulator/robot.py
index 4aec3597181d831ec88e39907dc922d85ef57cb1..3300ffc336cccc43250e597b95f67be83fe61312 100755
--- a/simulator/robot.py
+++ b/simulator/robot.py
@@ -64,7 +64,8 @@ class Arm(extctrl2014_lc.irb2ext_robot_net):
class Robot(extctrl2014_lc.irb2ext_net):
- def __init__(self, arms):
+ def __init__(self, cookie=0, arms=[]):
+ self.cookie = cookie
self.obtaining = False
self.manualMode = False
self.controlActive = False
@@ -124,9 +125,9 @@ if __name__ == '__main__':
encoder.add_decl(extctrl2014_lc.irb2ext_net.signature)
encoder.add_decl(extctrl2014_lc.force_torque_net.signature)
- robot = Robot([ Arm(kind=0,
- joint=[ Joint(), Joint() ],
- mocgendata=[])])
+ robot = Robot(arms=[ Arm(kind=0,
+ joint=[ Joint(), Joint() ],
+ mocgendata=[])])
def run_decoder():
while True:
value,decl = decoder.decode()
@@ -145,6 +146,7 @@ if __name__ == '__main__':
t.start()
for i in range(100):
time.sleep(0.1)
+ robot.cookie = i
encoder.encode(robot,
extctrl2014_lc.irb2ext_net.signature)
# Let connection timeout (whenever that gets implemented)