47 lines
1.5 KiB
Python
47 lines
1.5 KiB
Python
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)
|
|
|