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