class RobotModbus(DeviceBase):
    def __init__(self, name):
         DeviceBase.__init__(self, name)
         robot_req.write(0)

    def execute(self, command, *argv):
        if robot_req.read() != 0:
            raise Exception("Ongoing command")
        if robot_ack.read() != 0:
            raise Exception("Robot is not ready")
        robot_cmd.write(command)
        args = [0] * robot_args.size
        index = 0        
        for arg in argv:
            args[index] = arg
            index = index + 1
            if index == robot_args.size:
                raise Exception("Invalid number of arguments")
        robot_args.write(to_array(args, 'i'))
        try:           
            self.request()         
            err = robot_ack.take()            
            if err == 1:
                ret = robot_ret.read()            
                return ret
            if err == 2:
                raise Exception("Invalid command: " + str(command))                
            raise Exception("Unknown error: " + str(err)) 
        finally:
            self.cancel_request()
    
    def request(self):
        robot_req.write(1)
        while robot_ack.read() == 0:
            time.sleep(0.001)

    def cancel_request(self):
        robot_req.write(0)
        while robot_ack.read() != 0:
            time.sleep(0.001)
        
    def mount(self, puck, sample):
        return self.execute('1', '1', puck, sample)        

add_device(RobotModbus("robot_mb"), force = True)