K = 4.24e-7 # Magnet's constant (K) || Units { G^2.m^6}
#K = 1.09e-6
dx = 1e-7 # Differential step size (Needed for solver)
calcPos = [] # Empty array to hold calculated positions
# Create a queue for retrieving data from the thread.
Q_getData = qu.Queue(maxsize=0)
# Establish connection with Arduino
DEVC = "Arduino" # Device Name (not very important)
PORT = 6 # Port number (VERY important)
BAUD = 115200 # Baudrate (VERY VERY important)
# Error handling in case serial communcation fails (1/2).
try:
IMU = createUSBPort(DEVC, PORT, BAUD) # Create serial connection
if IMU.is_open == False: # Make sure port is open
IMU.open()
print("Serial Port OPEN")
# Determine initial guess based on magnet's location.
initialGuess = np.array((0.005, 0.050, -0.050), dtype='float64')
# Error handling in case serial communcation fails (2/2).
except Exception as e:
print("Could NOT open serial port")
print("Error type %s" % str(type(e)))
print("Error Arguments " + str(e.args))
sleep(2.5)
quit() # Shutdown entire program
### Declare arrays to store values:
# Computed values of H
H_1 = [[] for i in range(3)] # [0]:Hx || [1]:Hy || [2]:Hz
H_2 = [[] for i in range(3)] # [0]:Hx || [1]:Hy || [2]:Hz
# Numerically evaluated angles
angles = [[] for i in range(4)] # [0]:alpha || [1]:beta || [2]:gamma || [3]:phi
# Location in virtual space
virLoc_1 = [[] for i in range(2)] # [0]:x || [1]:y
virLoc_2 = [[] for i in range(2)] # [0]:x || [1]:y
# Location in our real physical meaningful(less?) world
reaLoc_1 = [[] for i in range(3)] # [0]:x || [1]:y || [3]:z
reaLoc_2 = [[] for i in range(3)] # [0]:x || [1]:y || [3]:z
# Establish connection with Arduino
try:
ser = createUSBPort( "Arduino", 6, 115200 )
if ser.is_open == False:
ser.open()
print( "Serial Port OPEN" )
except Exception as e:
print( "Could NOT open serial port" )
print( "Error type %s" %str(type(e)) )
print( " Error Arguments " + str(e.args) )
sleep( 5 )
quit()
### Sensor 1 readings
##B_x_1 = random.random()*5
##B_y_1 = random.random()*5
##B_z_1 = random.random()*5
# ************************************************************************
# ===========================> SETUP PROGRAM <===========================
# ************************************************************************
# Useful variables
global CALIBRATING
CALIBRATING = True # Boolean to indicate that device is calibrating
READY = False # Give time for user to palce magnet
# Establish connection with Arduino
DEVC = "Arduino"
PORT = 29
BAUD = 115200
try:
IMU = createUSBPort( DEVC, PORT, BAUD )
if IMU.is_open == False:
IMU.open()
print( "Serial Port OPEN" )
except Exception as e:
print( "Could NOT open serial port" )
print( "Error type %s" %str(type(e)) )
print( "Error Arguments " + str(e.args) )
sleep( 5 )
quit()
# ************************************************************************
# =========================> MAKE IT ALL HAPPEN <=========================
# ************************************************************************
R = cv2.bitwise_and( R, R, mask=A )
overlayImg = cv2.merge( [B, G, R, A] )
# Setup camera
stream = PiVideoStream( resolution=(384, 288) ).start()
normalDisplay = True
sleep( 1.0 )
# Setup window and mouseCallback event
cv2.namedWindow( ver, cv2.WND_PROP_FULLSCREEN )
cv2.setWindowProperty( ver, cv2.WND_PROP_FULLSCREEN, cv2.WINDOW_FULLSCREEN )
cv2.setMouseCallback( ver, control )
# Initialize ToF sensor
deviceName, port, baudRate = "VL6180", 0, 115200
ToF = createUSBPort( deviceName, port, baudRate, 3 )
if ToF.is_open == False:
ToF.open()
sleep( 0.5 )
inChar = (ToF.read(size=1).strip('\0')).strip('\n')
ToF.write('2')
while inChar is not 'y':
inChar = (ToF.read(size=1).strip('\0')).strip('\n')
# If debug flag is invoked
if args["debug"]:
print( inChar )
print( fullStamp() + " Distance Readings Initiated" )
ToF_Dist = 0 # Initialize to OFF
# Create a queue for retrieving data from thread
##K = 1.615e-7 # Small magnet's constant (K) || Units { G^2.m^6}
K = 1.09e-6 # Big magnet's constant (K) || Units { G^2.m^6}
dx = 1e-7 # Differential step size (Needed for solver)
# Surgical tool dimensions
Lt = 318 # length of the surgical tool
# Establish connection with Arduino
DEVC = "Arduino" # Device Name (not very important)
PORTPREFIX = "COM"
PORTNUM = 3 # Port number (VERY important)
BAUD = 115200 # Baudrate (VERY VERY important)
# Error handling in case serial communcation fails (1/2)
try:
IMU = createUSBPort(DEVC, PORTPREFIX, PORTNUM,
BAUD) # Create serial connection
if IMU.is_open == False: # Make sure port is open
IMU.open()
print("Serial Port OPEN")
initialGuess = findIG(
getData(IMU)) # Determine initial guess based on magnet's location
# Error handling in case thread spawning fails (2/2)
except Exception as e:
print("Could NOT open serial port")
print("Error type {}".format(type(e)))
print("Error Arguments {}".format(e.args))
sleep(2.5)
quit() # Shutdown entire program
