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 __init__(self):
#Instances
self.gripper = Gripper()
self.m = SendMove()
self.t = tf.TransformListener()
self.armPose = ""
#Subscribers
rospy.Subscriber("/tf", TFMessage, self.getToolTF)
rospy.Subscriber("tool_velocity", TwistStamped, self.getToolVelocity)
#Open gripper
self.gripper.moveGripper("Open")
#Go to initial position
self.m.sendMove(self.m.buildMove('j', '', self.m.getPos('test3')))
self.waitForArm()
number = int(message[2:-1])
print("Command: " + mess + " Number: " + str(number))
if __name__ == '__main__':
mes = "Q"
num = 0
# Makes sure the script is run as sudo.
if not os.geteuid() == 0:
sys.exit("\nMust be root to run this script!\n")
# Set up the USB Connection to the Arduino
usb_options = ['/dev/ttyUSB0', '/dev/ttyUSB1', '/dev/ttyUSB2']
usb_options = filter(os.path.exists, usb_options)
assert (len(usb_options) == 1) # Exist if no usb connection.
usb_port = usb_options[0]
# Open the communication thread.
print("Communication Thread Started.")
communication_motor = tcp_server_motor(usb_port)
communication_motor.start()
# Set up and start gripper thread. /--- EXPERIMENTAL ---/
print("Gripper Thread Started.")
pheeno_gripper = Gripper()
communication_gripper = threading.Thread(target=myGripper.gripperMove,
args=(servo, angle))
communication_gripper.start()
#!/usr/bin/env micropython from DriveTrain import DriveTrain from Gripper import Gripper import time driveTrain = DriveTrain() gripper = Gripper() gripper.lowerMotor(30) driveTrain.driveToLine(-20, 8, ["Green", "Blue"], ["Black", "Brown"]) driveTrain.driveForward(-40, 13) driveTrain.turnAngle(-20, 90) driveTrain.followLine(-20, 9, ["Black", "Brown"], 10) driveTrain.driveForward(-40, -40) driveTrain.driveForward(-20, -5) time.sleep(3) driveTrain.followLine(-20, 10, ["Black", "Brown"], 40) driveTrain.turnAngle(20, 180) driveTrain.driveToLine(-20, 10, ["Black", "Brown"],["Black", "Brown"]) gripper.lowerMotor(-30) driveTrain.driveForward(20, 10)
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()
import time
import serial
import socket
import os.path
import sys
from Gripper import Gripper
myGripper = Gripper()
PORT = '12345'
usbOptions = ['/dev/ttyUSB0', '/dev/ttyUSB1', '/dev/ttyUSB2']
usbOptions = filter(os.path.exists, usbOptions)
assert (len(usbOptions) == 1)
usbPort = usbOptions[0]
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.bind(('0.0.0.0', 12345))
s.listen(1)
client, _ = s.accept()
while True:
myGripper.gripperMove(3, 100, 0.1)
try:
usb = serial.Serial(usbPort, 9600, timeout=10)
usb.open()
time.sleep(2) # give the serial time to get opened
except serial.SerialException:
print("Could not connect to requested port")
time.sleep(1)
continue
while True:
