From 5aa71d13feccbb9857b9c398def36e2106226a2e Mon Sep 17 00:00:00 2001
From: gac-S_Changer <gac-S_Changer@PC12288.psi.ch>
Date: Wed, 1 Mar 2017 15:30:04 +0100
Subject: [PATCH] Closedown

---
 config/devices.properties                     | 12 +++----
 .../usr/usrapp/ForceDetection/comModbus.pgx   |  8 ++++-
 script/RobotSC.py                             | 34 +++++++++++++++++++
 script/RobotTCP.py                            | 12 +++++--
 4 files changed, 56 insertions(+), 10 deletions(-)

diff --git a/config/devices.properties b/config/devices.properties
index 9f86e8d..c0a7b50 100644
--- a/config/devices.properties
+++ b/config/devices.properties
@@ -1,12 +1,12 @@
 #robot=RobotTcp|127.0.0.1:1000|||
 puck_detection=ch.psi.mxsc.PuckDetection|raspberrypi:5556|||
 robot_modbus=ch.psi.pshell.modbus.ModbusTCP|129.129.126.100:502|||
-jf1=ch.psi.pshell.modbus.AnalogInput|robot_modbus 0||100|
-jf2=ch.psi.pshell.modbus.AnalogInput|robot_modbus 1||100|
-jf3=ch.psi.pshell.modbus.AnalogInput|robot_modbus 2||100|
-jf4=ch.psi.pshell.modbus.AnalogInput|robot_modbus 3||100|
-jf5=ch.psi.pshell.modbus.AnalogInput|robot_modbus 4||100|
-jf6=ch.psi.pshell.modbus.AnalogInput|robot_modbus 5||100|
+#jf1=ch.psi.pshell.modbus.AnalogInput|robot_modbus 0||100|
+#jf2=ch.psi.pshell.modbus.AnalogInput|robot_modbus 1||100|
+#jf3=ch.psi.pshell.modbus.AnalogInput|robot_modbus 2||100|
+#jf4=ch.psi.pshell.modbus.AnalogInput|robot_modbus 3||100|
+#jf5=ch.psi.pshell.modbus.AnalogInput|robot_modbus 4||100|
+#jf6=ch.psi.pshell.modbus.AnalogInput|robot_modbus 5||100|
 robot_sts=ch.psi.pshell.modbus.AnalogInputArray|robot_modbus 6 6||100|
 robot_cmd=ch.psi.pshell.modbus.AnalogOutput|robot_modbus 12|||
 robot_args=ch.psi.pshell.modbus.AnalogOutputArray|robot_modbus 47 12|||
diff --git a/robot/MXLAB/Controller1/usr/usrapp/ForceDetection/comModbus.pgx b/robot/MXLAB/Controller1/usr/usrapp/ForceDetection/comModbus.pgx
index 146cce8..5c36ae4 100644
--- a/robot/MXLAB/Controller1/usr/usrapp/ForceDetection/comModbus.pgx
+++ b/robot/MXLAB/Controller1/usr/usrapp/ForceDetection/comModbus.pgx
@@ -9,6 +9,7 @@
       <Local name="ret" type="num" xsi:type="array" size="32" />
       <Local name="args" type="num" xsi:type="array" size="32" />
       <Local name="error" type="num" xsi:type="array" size="1" />
+      <Local name="aux" type="num" xsi:type="array" size="1" />
     </Locals>
     <Code><![CDATA[begin
   req = false
@@ -18,7 +19,12 @@
     //Record the joint force (comment out next line for use with expansion add-on)
     getJointForce(l_nJointForce)
     for i=0 to 5
-      aioSet(JF[i], l_nJointForce[i]*100)  
+      aux = l_nJointForce[i]*100
+      if aux<0 
+        //aux = 65536 + aux
+        aux = - aux
+      endIf
+      aioSet(JF[i], aux)  
     endFor
     
     
diff --git a/script/RobotSC.py b/script/RobotSC.py
index 4673a98..bf7b203 100644
--- a/script/RobotSC.py
+++ b/script/RobotSC.py
@@ -7,8 +7,42 @@ class RobotSC(RobotTCP):
     def on_event(self,ev):
         #print "EVT: " + ev
         pass
+    def doUpdate(self):
+        RobotTCP.doUpdate(self)
+        if self.state != State.Offline:
+            self.get_joint_forces()
+
+
+    
 
 
 add_device(RobotSC("robot", "129.129.126.100:1000"), force = True)   
 robot.setPolling(100)
 #robot.set_monitor_speed(20)
+
+
+class JF1(ReadonlyRegisterBase):
+    def doRead(self):
+        return  None if robot.joint_forces == None else robot.joint_forces[0]
+class JF2(ReadonlyRegisterBase):
+    def doRead(self):
+        return  None if robot.joint_forces == None else robot.joint_forces[1]
+class JF3(ReadonlyRegisterBase):
+    def doRead(self):
+        return  None if robot.joint_forces == None else robot.joint_forces[2]
+class JF4(ReadonlyRegisterBase):
+    def doRead(self):
+        return  None if robot.joint_forces == None else robot.joint_forces[3]
+class JF5(ReadonlyRegisterBase):
+    def doRead(self):
+        return  None if robot.joint_forces == None else robot.joint_forces[4]
+class JF6(ReadonlyRegisterBase):
+    def doRead(self):
+        return  None if robot.joint_forces == None else robot.joint_forces[5]
+
+add_device(JF1(), force = True)   
+add_device(JF2(), force = True)   
+add_device(JF3(), force = True)   
+add_device(JF4(), force = True)   
+add_device(JF5(), force = True)   
+add_device(JF6(), force = True)   
diff --git a/script/RobotTCP.py b/script/RobotTCP.py
index 63d9e04..5e130fe 100644
--- a/script/RobotTCP.py
+++ b/script/RobotTCP.py
@@ -18,6 +18,7 @@ class RobotTCP(TcpDevice, Stoppable):
          self.working_mode = None
          self.status = None
          self.lock = threading.Lock()
+         self.joint_forces = None
          
     def _sendReceive(self, msg_id, msg = "", timeout =  None):  
         tx = self.header if (self.header != None) else  ""
@@ -244,9 +245,14 @@ class RobotTCP(TcpDevice, Stoppable):
     def set_move_id(self, id):
         return self.evaluate("setMoveId(" + str(id) + " )")
 
-    def get_joint_forces(self):
-        self.evaluate("getJointForce(arr)")        
-        return self.get_float_arr(6)
+    def get_joint_forces(self):        
+        try:
+            self.evaluate("getJointForce(arr)")        
+            self.joint_forces = self.get_float_arr(6)            
+            return self.joint_forces
+        excepr:
+            return self.joint_forces = None
+            raise
 
     def movej(self, joint_or_point, tool, desc):
         return self.eval_int("movej(" + joint_or_point + ", " + tool + ", " + desc +")")