class Dispatcher(object):
def __init__(self, context):
self.context = context
self.flag = True
self.needToRetract = False
self.draw_back_guidewire_curcuit_flag = True
self.guidewireProgressMotor = OrientalMotor(20, 21, True)
self.guidewireRotateMotor = MaxonMotor(2, "EPOS2", "MAXON SERIAL V2",
"USB", "USB0", 1000000)
self.catheterMotor = OrientalMotor(14, 15, True)
self.angioMotor = OrientalMotor(23, 24, False)
self.gripperFront = Gripper(7)
self.gripperBack = Gripper(8)
self.infraredReflectiveSensor = InfraredReflectiveSensor()
self.dispatchTask = threading.Thread(None, self.listening)
#self.dispatchTask.start()
self.cptt = 0
self.global_state = 0
def set_global_state(self, state):
self.global_state = state
def listening(self):
while self.flag:
if not self.context.get_system_status():
self.guidewireRotateMotor.close_device()
self.guidewireProgressMotor.close_device()
self.catheterMotor.close_device()
self.angioMotor.close_device()
sys.exit()
self.flag = False
print "system terminated"
else:
self.decode()
time.sleep(0.05)
def decode(self):
if self.context.get_catheter_move_instruction_sequence_length() > 0:
msg = self.context.fetch_latest_catheter_move_msg()
if self.draw_back_guidewire_curcuit_flag == False:
return
if msg.get_motor_orientation() == 0:
self.catheterMotor.set_speed(msg.get_motor_speed())
return
elif msg.get_motor_orientation() == 1:
self.catheterMotor.set_speed(-msg.get_motor_speed())
return
if not self.needToRetract:
if self.context.get_guidewire_progress_instruction_sequence_length(
) > 0:
self.set_global_state(
self.infraredReflectiveSensor.read_current_state())
if self.global_state == 0:
msg = self.context.fetch_latest_guidewire_progress_move_msg(
)
if self.draw_back_guidewire_curcuit_flag == False:
return
if msg.get_motor_orientation() == 0 and abs(
msg.get_motor_speed()) < 40 * 2 * 60:
#print -msg.get_motor_speed()
self.guidewireProgressMotor.set_speed(
-msg.get_motor_speed())
self.cptt = 0
elif msg.get_motor_orientation() == 1 and abs(
msg.get_motor_speed()) < 40 * 2 * 60:
#print msg.get_motor_speed()
self.guidewireProgressMotor.set_speed(
msg.get_motor_speed())
else:
self.guidewireProgressMotor.set_speed(0)
elif self.global_state == 2:
#print "retract"
self.guidewireProgressMotor.set_speed(0)
self.needToRetract = True
retractTask = threading.Thread(
None, self.draw_back_guidewire_curcuit)
retractTask.start()
elif self.global_state == 1:
#print "hehe", self.global_guidewire_distance
self.guidewireProgressMotor.set_speed(100)
elif self.global_state == 3:
self.guidewireProgressMotor.set_speed(0)
if self.context.get_guidewire_rotate_instruction_sequence_length(
) > 0:
msg = self.context.fetch_latest_guidewire_rotate_move_msg()
speed = msg.get_motor_speed()
position = (msg.get_motor_position() * 4000) / 360
if self.draw_back_guidewire_curcuit_flag == False:
return
if msg.get_motor_orientation() == 0:
self.guidewireRotateMotor.rm_move(speed)
pass
elif msg.get_motor_orientation() == 1:
self.guidewireRotateMotor.rm_move(-speed)
pass
if self.context.get_contrast_media_push_instruction_sequence_length(
) > 0:
msg = self.context.fetch_latest_contrast_media_push_move_msg()
ret = msg.get_motor_speed()
if self.draw_back_guidewire_curcuit_flag == False:
return
if msg.get_motor_orientation() == 0:
self.angioMotor.set_speed(-ret)
elif msg.get_motor_orientation() == 1:
self.angioMotor.set_speed(ret)
if self.context.get_retract_instruction_sequence_length() > 0:
if self.draw_back_guidewire_curcuit_flag == False:
return
self.draw_back_guidewire_curcuit()
if self.context.get_injection_command_sequence_length() > 0:
msg = self.context.fetch_latest_injection_msg_msg()
#print "injection command", msg.get_speed(),msg.get_volume()
if msg.get_volume() < 18:
self.angioMotor.set_pos_speed(msg.get_speed())
self.angioMotor.set_position(msg.get_volume())
self.angioMotor.push_contrast_media()
elif msg.get_volume() == 500.0:
self.angioMotor.pull_back()
def push_guidewire_back(self):
#self.context.clear()
self.draw_back_guidewire_curcuit_flag == False
#self.gripperFront.gripper_chuck_loosen()
#self.gripperBack.gripper_chuck_loosen()
#time.sleep(1)
# fasten front gripper
self.gripperFront.gripper_chuck_fasten()
# self-tightening chunck
self.gripperBack.gripper_chuck_fasten()
time.sleep(1)
self.guidewireRotateMotor.rm_move_to_position(90, 4000) # +/loosen
time.sleep(3)
#self.gripperFront.gripper_chuck_loosen()
#time.sleep(1)
self.guidewireProgressMotor.set_speed(-800)
self.global_state = self.infraredReflectiveSensor.read_current_state()
while self.global_state != 1:
time.sleep(0.5)
self.global_state = self.infraredReflectiveSensor.read_current_state(
)
print "retracting", self.global_state
print "back limitation arrived"
self.guidewireProgressMotor.set_speed(0)
self.guidewireRotateMotor.rm_move_to_position(90, -4000)
time.sleep(3)
self.gripperFront.gripper_chuck_loosen()
self.gripperBack.gripper_chuck_loosen()
self.draw_back_guidewire_curcuit_flag == True
self.needToRetract = False
def push_guidewire(self):
# self.context.clear()
self.draw_back_guidewire_curcuit_flag == False
for i in range(0, 2):
self.gripperFront.gripper_chuck_loosen()
self.gripperBack.gripper_chuck_loosen()
time.sleep(1)
self.gripperFront.gripper_chuck_fasten()
self.gripperBack.gripper_chuck_fasten()
time.sleep(1)
self.guidewireRotateMotor.rm_move_to_position(40, 5000) # +/loosen
time.sleep(8)
self.gripperFront.gripper_chuck_loosen()
self.global_guidewire_distance = self.ultraSoundModule.read_current_distance(
)
self.guidewireProgressMotor.set_speed(600)
while self.global_guidewire_distance > (self.frontLimitDistance +
2):
time.sleep(0.5)
self.global_guidewire_distance = self.ultraSoundModule.read_current_distance(
)
print "pushing"
print "pushing limitation arrived"
#time.sleep(15)
self.guidewireProgressMotor.set_speed(0)
time.sleep(1)
self.gripperFront.gripper_chuck_loosen()
self.gripperBack.gripper_chuck_loosen()
time.sleep(1)
self.gripperFront.gripper_chuck_fasten()
self.gripperBack.gripper_chuck_fasten()
time.sleep(1)
self.guidewireRotateMotor.rm_move_to_position(40, -8000)
time.sleep(8)
self.gripperFront.gripper_chuck_loosen()
#time.sleep(1)
self.guidewireProgressMotor.set_speed(-600)
while self.global_guidewire_distance < (self.behindLimitDistance -
2):
time.sleep(0.5)
self.global_guidewire_distance = self.ultraSoundModule.read_current_distance(
)
print "fetching"
print "fetch limitation arrived"
#self.guidewireProgressMotor.set_speed(-400)
#time.sleep(15)
self.guidewireProgressMotor.set_speed(0)
time.sleep(1)
self.draw_back_guidewire_curcuit_flag == True
def push_guidewire_advance(self):
self.guidewireProgressMotor.set_speed(800)
self.global_state = self.infraredReflectiveSensor.read_current_state()
while self.global_state != 2:
time.sleep(0.5)
self.global_state = self.infraredReflectiveSensor.read_current_state(
)
#print "pushing", self.global_state
#print "front limitation arrived"
self.guidewireProgressMotor.set_speed(0)
#self.guidewireRotateMotor.rm_move_to_position(90, -8000)
#time.sleep(4)
def multitime_push_guidewire(self):
for i in range(0, 8):
dispatcher.push_guidewire_advance()
dispatcher.push_guidewire_back()
print(i)
def draw_guidewire_back(self):
#self.draw_back_guidewire_curcuit_flag == False
#self.gripperFront.gripper_chuck_loosen()
#self.gripperBack.gripper_chuck_fasten()
#time.sleep(1)
#self.guidewireRotateMotor.rm_move_to_position(85,4000)
#time.sleep(3)
self.guidewireProgressMotor.set_speed(-800)
self.global_state = self.infraredReflectiveSensor.read_current_state()
while self.global_state != 1:
time.sleep(0.5)
self.global_state = self.infraredReflectiveSensor.read_current_state(
)
#print "retracting", self.global_state
#print "back limitation arrived"
self.guidewireProgressMotor.set_speed(0)
#self.guidewireRotateMotor.rm_move_to_position(85, -4000)
#time.sleep(4)
#self.draw_back_guidewire_curcuit_flag == True
#self.needToRetract = False
def draw_guidewire_advance(self):
self.gripperFront.gripper_chuck_loosen()
self.gripperBack.gripper_chuck_loosen()
time.sleep(1)
self.gripperFront.gripper_chuck_fasten()
self.gripperBack.gripper_chuck_fasten()
time.sleep(1)
self.guidewireRotateMotor.rm_move_to_position(80, 4000)
time.sleep(3)
self.guidewireProgressMotor.set_speed(800)
self.global_state = self.infraredReflectiveSensor.read_current_state()
while self.global_state != 2:
time.sleep(0.5)
self.global_state = self.infraredReflectiveSensor.read_current_state(
)
#print "advancing", self.global_state
#print "front limitation arrived"
self.guidewireProgressMotor.set_speed(0)
#self.gripperFront.gripper_chuck_fasten()
#self.gripperBack.gripper_chuck_fasten()
time.sleep(1)
self.guidewireRotateMotor.rm_move_to_position(80, -4000)
time.sleep(3)
# self.gripperFront.gripper_chuck_fasten()
# time.sleep(1)
#self.gripperFront.gripper_chuck_loosen()
self.gripperBack.gripper_chuck_loosen()
time.sleep(1)
def multitime_draw_back_guidewire(self):
for i in range(0, 8):
dispatcher.draw_guidewire_advance()
dispatcher.draw_guidewire_back()
print(i)
def automatic_procedure(self):
self.angioMotor.set_pos_speed(2)
self.angioMotor.set_position(5)
self.angioMotor.push_contrast_media()
print "angiographing finish"
time.sleep(5)
self.multitime_push_guidewire()
self.multitime_draw_back_guidewire()
def push_and_pull(self):
self.multitime_push_guidewire()
self.multitime_draw_back_guidewire()
def loosen(self):
self.gripperBack.gripper_chuck_fasten()
time.sleep(1)
self.gripperBack.gripper_chuck_loosen()
#self.gripperBack.gripper_chuck_loosen()
time.sleep(1)
class Dispatcher(object):
"""
description:this class plays an role in th command and control of the interventional robot which includes:
-- the control of GPIOs of the raspberryPi which connet motors, sensors and grippers
-- the distribution of tasks in different threads
-- the command and control of the actions of interventional robot in surgery
author:Cheng WANG
"""
def __init__(self, context, local_mode=0):
self.context = context
# ---------------------------------------------------------------------------------------------
# initialisation
# ---------------------------------------------------------------------------------------------
self.flag = True
self.cptt = 0
self.global_state = 0
self.needToRetract = False
self.draw_back_guidewire_curcuit_flag = True
self.number_of_cycles = 0
# ---------------------------------------------------------------------------------------------
# execution units of the interventional robot
# ---------------------------------------------------------------------------------------------
self.guidewireProgressMotor = OrientalMotor(20, 21, True)
self.guidewireRotateMotor = OrientalMotor(19, 26, True)
self.catheterMotor = OrientalMotor(17, 27, True)
self.angioMotor = OrientalMotor(23, 24, False)
self.gripperFront = Gripper(7)
self.gripperBack = Gripper(8)
# ---------------------------------------------------------------------------------------------
# sensors
# ---------------------------------------------------------------------------------------------
self.infraredReflectiveSensor = InfraredReflectiveSensor()
# ---------------------------
# feedback
# ---------------------------
self.forcefeedback = Feedback("/dev/ttyUSB1", 9600, 8, 'N', 1)
self.torquefeedback = Feedback("/dev/ttyUSB0", 9600, 8, 'N', 1)
# ---------------------------------------------------------------------------------------------
# EmergencySwitch
# ---------------------------------------------------------------------------------------------
self.switch = EmergencySwitch()
self.emSwitch = 1
self.lastSwitch = 0
self.em_count = 0
# ---------------------------------------------------------------------------------------------
# speed parameters
# ---------------------------------------------------------------------------------------------
self.speedProgress = 1000
self.speedRotate = 60
self.speedCatheter = 10
self.rotateTime = 180 / self.speedRotate
self.pos_speed = 5
self.position_cgf = 2
self.position_cgb = -100
# ---------------------------------------------------------------------------------------------
# real time task to parse commandes in context
# ---------------------------------------------------------------------------------------------
self.dispatchTask = threading.Thread(
None, self.do_parse_commandes_in_context)
self.dispatchTask.start()
self.aquirefeedbackTask = threading.Thread(None,
self.aquirefeedback_context)
self.aquirefeedbackTask.start()
def set_global_state(self, state):
self.global_state = state
def do_parse_commandes_in_context(self):
"""
determine system's status and start to decode or to close devices
"""
while self.flag:
if not self.context.get_system_status():
self.guidewireRotateMotor.close_device()
self.guidewireProgressMotor.close_device()
self.catheterMotor.close_device()
self.angioMotor.close_device()
sys.exit()
self.flag = False
print "system terminated"
else:
self.emSwitch = self.switch.read_current_state()
if self.emSwitch == 1:
time.sleep(0.02)
self.guidewireRotateMotor.standby()
self.guidewireProgressMotor.standby()
self.catheterMotor.standby()
self.angioMotor.standby()
self.lastSwitch = 1
elif self.emSwitch == 0 and self.lastSwitch == 1:
self.guidewireRotateMotor.enable()
self.guidewireProgressMotor.enable()
self.catheterMotor.enable()
self.angioMotor.enable()
self.lastSwitch = 0
self.decode()
elif self.emSwitch == 0 and self.lastSwitch == 0:
self.decode()
time.sleep(0.05)
def decode(self):
"""
decode messages in the sequence and performe operations
"""
# ------------------------------------------------------------------------------
# advance according the set distance
# -----------------------------------------------------------------------------
if self.context.get_catheter_guidewire_push_sequence_lenght() > 0:
msg = self.context.fetch_latest_catheter_guidewire_push_msg()
if self.draw_back_guidewire_curcuit_flag == False:
return
speed = msg.get_motor_speed()
relative_position = msg.get_motor_relative_distance()
# ---------------------------------------------------------------------------------------------
# catheter execution case
# ---------------------------------------------------------------------------------------------
if self.context.get_catheter_move_instruction_sequence_length() > 0:
msg = self.context.fetch_latest_catheter_move_msg()
if self.draw_back_guidewire_curcuit_flag == False:
return
if msg.get_motor_orientation() == 0:
self.catheterMotor.set_speed(msg.get_motor_speed() / 10.0)
return
elif msg.get_motor_orientation() == 1:
self.catheterMotor.set_speed(-msg.get_motor_speed() / 10.0)
return
# ---------------------------------------------------------------------------------------------
# guidewire progress execution case
# ---------------------------------------------------------------------------------------------
if not self.needToRetract: # or not self.need ....
if self.context.get_guidewire_progress_instruction_sequence_length(
) > 0:
self.set_global_state(
self.infraredReflectiveSensor.read_current_state())
if self.global_state == 0:
msg = self.context.fetch_latest_guidewire_progress_move_msg(
)
if self.draw_back_guidewire_curcuit_flag == False:
return
if msg.get_motor_orientation() == 0 and abs(
msg.get_motor_speed()) < 40 * 2 * 60:
#print -msg.get_motor_speed()
self.guidewireProgressMotor.set_speed(
-msg.get_motor_speed())
self.cptt = 0
elif msg.get_motor_orientation() == 1 and abs(
msg.get_motor_speed()) < 40 * 2 * 60:
#print msg.get_motor_speed()
self.guidewireProgressMotor.set_speed(
msg.get_motor_speed())
else:
self.guidewireProgressMotor.set_speed(0)
elif self.global_state == 2:
#print "retract"
self.guidewireProgressMotor.set_speed(0)
self.needToRetract = True
retractTask = threading.Thread(None,
self.push_guidewire_back)
retractTask.start()
elif self.global_state == 1:
#print "hehe", self.global_guidewire_distance
self.guidewireProgressMotor.set_speed(self.speedProgress)
elif self.global_state == 3:
self.guidewireProgressMotor.set_speed(0)
# ---------------------------------------------------------------------------------------------
# guidewire rotate execution case
# ---------------------------------------------------------------------------------------------
if self.context.get_guidewire_rotate_instruction_sequence_length(
) > 0:
msg = self.context.fetch_latest_guidewire_rotate_move_msg()
speed = msg.get_motor_speed()
position = (msg.get_motor_position() * 4000) / 360
if self.draw_back_guidewire_curcuit_flag == False:
return
if msg.get_motor_orientation() == 0:
self.guidewireRotateMotor.set_speed(speed)
pass
elif msg.get_motor_orientation() == 1:
self.guidewireRotateMotor.set_speed(-speed)
pass
# ---------------------------------------------------------------------------------------------
# contrast media push execution case
# ---------------------------------------------------------------------------------------------
if self.context.get_contrast_media_push_instruction_sequence_length(
) > 0:
msg = self.context.fetch_latest_contrast_media_push_move_msg()
ret = msg.get_motor_speed()
if self.draw_back_guidewire_curcuit_flag == False:
return
if msg.get_motor_orientation() == 0:
self.angioMotor.set_speed(-ret)
elif msg.get_motor_orientation() == 1:
self.angioMotor.set_speed(ret)
if self.context.get_retract_instruction_sequence_length() > 0:
if self.draw_back_guidewire_curcuit_flag == False:
return
self.draw_back_guidewire_curcuit()
if self.context.get_injection_command_sequence_length() > 0:
msg = self.context.fetch_latest_injection_msg_msg()
#print "injection command", msg.get_speed(),msg.get_volume()
if msg.get_volume() < 0:
self.angioMotor.set_pos_speed(msg.get_speed())
self.angioMotor.set_position(msg.get_volume() / 4.5)
self.angioMotor.pull_contrast_media()
elif msg.get_volume() <= 30:
self.angioMotor.set_pos_speed(msg.get_speed())
self.angioMotor.set_position(msg.get_volume() / 4.5)
self.angioMotor.push_contrast_media()
def push_contrast_agent(self):
"""
Contrast agent push
"""
self.angioMotor.set_pos_speed(self.pos_speed)
self.angioMotor.set_position(self.position_cgf / 4.5)
self.angioMotor.push_contrast_media()
def pull_contrast_agent(self):
self.angioMotor.set_pos_speed(self.pos_speed)
self.angioMotor.set_position(self.position_cgb / 4.5)
self.angioMotor.pull_contrast_media()
def push_guidewire_back(self):
"""
the shifboard get back when guidewire progress
"""
self.context.clear_guidewire_message()
self.draw_back_guidewire_curcuit_flag == False
#self.gripperFront.gripper_chuck_loosen()
#self.gripperBack.gripper_chuck_loosen()
#time.sleep(1)
# fasten front gripper
self.gripperFront.gripper_chuck_fasten()
# self-tightening chunck
self.gripperBack.gripper_chuck_fasten()
time.sleep(1)
self.guidewireRotateMotor.set_speed(-self.speedRotate) # +/loosen
time.sleep(self.rotateTime)
self.guidewireRotateMotor.set_speed(0)
#self.gripperFront.gripper_chuck_loosen()
#time.sleep(1)
self.guidewireProgressMotor.set_speed(-self.speedProgress)
#time.sleep(3)
self.global_state = self.infraredReflectiveSensor.read_current_state()
while self.global_state != 1:
#self.global_state = self.infraredReflectiveSensor.read_current_state()
#if self.global_state == 4:
#self.global_state = self.infraredReflectiveSensor.read_current_state()
#continue
time.sleep(0.5)
self.global_state = self.infraredReflectiveSensor.read_current_state(
)
print "retracting", self.global_state
print "back limitation arrived"
self.guidewireProgressMotor.set_speed(0)
self.guidewireRotateMotor.set_speed(self.speedRotate)
time.sleep(self.rotateTime)
self.guidewireRotateMotor.set_speed(0)
self.gripperFront.gripper_chuck_loosen()
self.gripperBack.gripper_chuck_loosen()
self.draw_back_guidewire_curcuit_flag == True
self.context.clear_guidewire_message()
self.needToRetract = False
def push_guidewire_advance(self):
"""
the shiftboard advance with pushing guidewire
"""
#self.context.clear_guidewire_message()
self.guidewireProgressMotor.set_speed(self.speedProgress)
self.global_state = self.infraredReflectiveSensor.read_current_state()
while self.global_state != 2:
time.sleep(0.5)
self.global_state = self.infraredReflectiveSensor.read_current_state(
)
#print "pushing", self.global_state
#print "front limitation arrived"
self.guidewireProgressMotor.set_speed(0)
#self.guidewireRotateMotor.rm_move_to_position(90, -8000)
#time.sleep(4)
def multitime_push_guidewire(self):
"""
the process of pushing guidewire for several times
"""
self.define_number_of_cycles()
for i in range(0, self.number_of_cycles):
self.push_guidewire_advance()
self.push_guidewire_back()
print(i)
def draw_guidewire_back(self):
"""
the shiftboard go back with drawing back guidewire
"""
self.guidewireRotateMotor.set_speed(self.speedRotate)
time.sleep(self.rotateTime)
self.guidewireRotateMotor.set_speed(0)
self.gripperBack.gripper_chuck_loosen()
time.sleep(1)
self.guidewireProgressMotor.set_speed(-self.speedProgress)
self.global_state = self.infraredReflectiveSensor.read_current_state()
while self.global_state != 1:
time.sleep(0.5)
self.global_state = self.infraredReflectiveSensor.read_current_state(
)
#print "retracting", self.global_state
#print "back limitation arrived"
self.guidewireProgressMotor.set_speed(0)
#self.guidewireRotateMotor.rm_move_to_position(85, -4000)
#time.sleep(4)
#self.draw_back_guidewire_curcuit_flag == True
#self.needToRetract = False
def chuck_loosen(self):
self.gripperBack.gripper_chuck_fasten()
time.sleep(1)
self.guidewireRotateMotor.set_speed(-self.speedRotate)
time.sleep(self.rotateTime)
self.guidewirRotateMotor.set_speed(0)
def chuck_fasten(self):
self.gripperBack.gripper_chuck_fasten()
time.sleep(1)
self.guidewireRotateMotor.set_speed(self.speedRotate)
time.sleep(self.rotateTime)
self.guidewireRotateMotor.set_speed(0)
def draw_guidewire_advance(self):
"""
the shiftboard advance in the process of drawing back of guidewire
"""
self.gripperFront.gripper_chuck_loosen()
self.gripperBack.gripper_chuck_loosen()
time.sleep(1)
self.gripperFront.gripper_chuck_fasten()
self.gripperBack.gripper_chuck_fasten()
time.sleep(1)
self.guidewireRotateMotor.set_speed(-self.speedRotate)
time.sleep(self.rotateTime)
self.guidewireRotateMotor.set_speed(0)
self.guidewireProgressMotor.set_speed(self.speedProgress)
self.global_state = self.infraredReflectiveSensor.read_current_state()
while self.global_state != 2:
time.sleep(0.5)
self.global_state = self.infraredReflectiveSensor.read_current_state(
)
#print "advancing", self.global_state
#print "front limitation arrived"
self.guidewireProgressMotor.set_speed(0)
#self.gripperFront.gripper_chuck_fasten()
#self.gripperBack.gripper_chuck_fasten()
time.sleep(1)
#self.guidewireRotateMotor.rm_move_to_position(80, -4000)
#time.sleep(3)
# self.gripperFront.gripper_chuck_fasten()
# time.sleep(1)
#self.gripperFront.gripper_chuck_loosen()
#self.gripperBack.gripper_chuck_loosen()
#time.sleep(1)
def multitime_draw_back_guidewire(self):
"""
the process of drawing back guidewire for several times
"""
self.define_number_of_cycles()
for i in range(0, self.number_of_cycles):
self.draw_guidewire_advance()
self.draw_guidewire_back()
print(i)
def automatic_procedure(self):
self.angioMotor.set_pos_speed(4)
self.angioMotor.set_position(10)
self.angioMotor.push_contrast_media()
print "angiographing finish"
time.sleep(5)
self.multitime_push_guidewire()
def push_and_pull(self):
"""
the test of processing and drawing back guidewire for several times
"""
self.multitime_push_guidewire()
self.multitime_draw_back_guidewire()
def loosen(self):
"""
the test of gripper
"""
self.gripperBack.gripper_chuck_fasten()
time.sleep(1)
self.gripperBack.gripper_chuck_loosen()
#self.gripperBack.gripper_chuck_loosen()
time.sleep(1)
def catheter_advance(self):
"""
the process of guidewire and cathter advance by turns
"""
self.gripperFront.gripper_chuck_loosen()
self.gripperBack.gripper_chuck_loosen()
self.draw_back_guidewire_curcuit_flag == True
self.needToRetract = False
self.guidewireProgressMotor.set_speed(self.speedProgress)
self.catheterMotor.set_speed(self.speedCatheter)
self.global_state = self.infraredReflectiveSensor.read_current_state()
while self.global_state != 2:
time.sleep(0.5)
self.global_state = self.infraredReflectiveSensor.read_current_state(
)
#print "pushing", self.global_state
#print "front limitation arrived"
self.guidewireProgressMotor.set_speed(0)
self.catheterMotor.set_speed(0)
#self.guidewireRotateMotor.rm_move_to_position(90, -8000)
#time.sleep(4)
#self.context.clear()
self.draw_back_guidewire_curcuit_flag == False
#self.gripperFront.gripper_chuck_loosen()
#self.gripperBack.gripper_chuck_loosen()
#time.sleep(1)
# fasten front gripper
self.gripperFront.gripper_chuck_fasten()
# self-tightening chunck
self.gripperBack.gripper_chuck_fasten()
time.sleep(1)
self.guidewireRotateMotor.set_speed(-self.speedRotate) # +/loosen
time.sleep(self.rotateTime)
self.guidewireRotateMotor.set_speed(0)
#self.gripperFront.gripper_chuck_loosen()
#time.sleep(1)
self.guidewireProgressMotor.set_speed(-self.speedProgress)
self.global_state = self.infraredReflectiveSensor.read_current_state()
while self.global_state != 1:
time.sleep(0.5)
self.global_state = self.infraredReflectiveSensor.read_current_state(
)
print "retracting", self.global_state
print "back limitation arrived"
self.guidewireProgressMotor.set_speed(0)
self.guidewireRotateMotor.set_speed(self.speedRotate)
time.sleep(self.rotateTime)
self.guidewireRotateMotor.set_speed(0)
def multitime_catheter_advance(self):
"""
the process of guidewire and cathter advance by turns for several times
"""
self.define_number_of_cycles()
for i in range(0, self.number_of_cycles):
self.catheter_advance()
#print(i)
def test(self):
self.gripperBack.gripper_chuck_fasten()
def catheter_back(self):
"""
the process of guidewire and cathter by turns for several times
"""
self.define_number_of_cycles()
for i in range(0, self.number_of_cycles):
self.draw_guidewire_back()
self.catheterMotor.set_speed(self.speedCatheter)
print(i)
def initialization(self):
"""
the initialization of the status of robot
"""
self.draw_back_guidewire_curcuit_flag == False
#self.gripperFront.gripper_chuck_loosen()
#self.gripperBack.gripper_chuck_loosen()
#time.sleep(3)
# fasten front gripper
# self.gripperFront.gripper_chuck_fasten()
# self-tightening chunck
# self.gripperBack.gripper_chuck_fasten()
# time.sleep(1)
# self.guidewireRotateMotor.rm_move_to_position(90, 4000) # +/loosen
# time.sleep(3)
#self.gripperFront.gripper_chuck_loosen()
#time.sleep(1)
self.guidewireProgressMotor.set_speed(-self.speedProgress)
self.global_state = self.infraredReflectiveSensor.read_current_state()
while self.global_state != 1:
time.sleep(0.5)
self.global_state = self.infraredReflectiveSensor.read_current_state(
)
print "retracting", self.global_state
print "back limitation arrived"
self.guidewireProgressMotor.set_speed(0)
self.guidewireRotateMotor.set_speed(self.speedRotate)
time.sleep(self.rotateTime)
self.guidewireRotateMotor.set_speed(0)
self.gripperFront.gripper_chuck_loosen()
self.gripperBack.gripper_chuck_loosen()
self.draw_back_guidewire_curcuit_flag == True
self.needToRetract = False
def catheter(self):
self.catheterMotor.set_speed(self.speedCatheter)
def define_number_of_cycles(self):
"""
define the number of cycels of the robot operation
"""
self.number_of_cycles = input("please input the number of cycles")
def aquirefeedback_context(self):
while True:
feedbackMsg = FeedbackMsg()
forcevalue = self.forcefeedback.aquireForce()
torquevalue = self.torquefeedback.aquireForce()
forcedirection = 0
if forcevalue < 0:
forcedirection = 1
else:
forcedirection = 0
forcevalue = abs(forcevalue)
torquedirection = 0
if torquevalue < 0:
torquedirection = 1
else:
torquedirection = 0
torquevalue = abs(torquevalue)
feedbackMsg.set_force_direction(forcedirection)
feedbackMsg.set_force_value(forcevalue)
feedbackMsg.set_torque_direction(torquedirection)
feedbackMsg.set_torque_value(torquevalue)
self.context.append_latest_forcefeedback_msg(feedbackMsg)