run("RobotTCP")

class RobotSC(RobotTCP):    
    def mount(self, puck, sample):
        return self.execute('mount', puck, sample)   

    def firstmount(self, puck, sample):
        return self.execute('firstmount', puck, sample)

    def unmount(self, puck, sample):
        return self.execute('unmount', puck, sample)

    def moveHome():
        

    def on_event(self,ev):
        #print "EVT: " + ev
        pass
    
    def doUpdate(self):
        start = time.time()
        RobotTCP.doUpdate(self)
        if self.state != State.Offline:
            self.get_joint_forces()
            for dev in [jf1, jf2, jf3, jf4,jf5, jf6, jfc]: 
                dev.update()
        #print time.time() -start


    


add_device(RobotSC("robot", "129.129.126.100:1000"), force = True) 
add_device(RobotSC("robotemulator","129.129.126.174:1000"),force =True)  
robot.setPolling(20)
#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]
class jfc(ReadonlyRegisterBase):
    def doRead(self):
        if robot.joint_forces == None:
            return   float('NaN')  
        if robot.powered == False:
            return  float('NaN') 
        return (robot.joint_forces[1]+74)/4  + (robot.joint_forces[2]+30)/4 + (robot.joint_forces[4]-0.8)/0.2

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)   
add_device(jfc(), force = True)