def StepperCreate():
try:
stepper = Stepper()
except RuntimeError as e:
print("Runtime Exception: %s" % e.details)
print("Exiting....")
exit(1)
try:
#logging example, uncomment to generate a log file
#stepper.enableLogging(PhidgetLogLevel.PHIDGET_LOG_VERBOSE, "phidgetlog.log")
stepper.setOnAttachHandler(StepperAttached)
stepper.setOnDetachHandler(StepperDetached)
stepper.setOnErrorhandler(StepperError)
stepper.setOnCurrentChangeHandler(StepperCurrentChanged)
stepper.setOnInputChangeHandler(StepperInputChanged)
stepper.setOnPositionChangeHandler(StepperPositionChanged)
stepper.setOnVelocityChangeHandler(StepperVelocityChanged)
# stepper2.setOnAttachHandler(StepperAttached)
# stepper2.setOnDetachHandler(StepperDetached)
# stepper2.setOnErrorhandler(StepperError)
# stepper2.setOnCurrentChangeHandler(StepperCurrentChanged)
# stepper2.setOnInputChangeHandler(StepperInputChanged)
# stepper2.setOnPositionChangeHandler(StepperPositionChanged)
# stepper2.setOnVelocityChangeHandler(StepperVelocityChanged)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
print("Opening phidget object....")
return stepper
print("Stepper %i: Input %i -- State: %s" % (source.getSerialNum(), e.index, e.state))
def StepperPositionChanged(e):
source = e.device
print("Stepper %i: Motor %i -- Position: %f" % (source.getSerialNum(), e.index, e.position))
def StepperVelocityChanged(e):
source = e.device
print("Stepper %i: Motor %i -- Velocity: %f" % (source.getSerialNum(), e.index, e.velocity))
#Main Program Code
try:
stepper.setOnAttachHandler(StepperAttached)
stepper.setOnDetachHandler(StepperDetached)
stepper.setOnErrorhandler(StepperError)
stepper.setOnCurrentChangeHandler(StepperCurrentChanged)
stepper.setOnInputChangeHandler(StepperInputChanged)
stepper.setOnPositionChangeHandler(StepperPositionChanged)
stepper.setOnVelocityChangeHandler(StepperVelocityChanged)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
print("Opening phidget object....")
try:
stepper.openPhidget()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
source = e.device
# print("Stepper %i: Motor %i -- Position: %f" % (source.getSerialNum(), e.index, e.position))
def StepperVelocityChanged(e):
source = e.device
print("Stepper %i: Motor %i -- Velocity: %f" %
(source.getSerialNum(), e.index, e.velocity))
# Main Program Code
try:
stepper.setOnAttachHandler(StepperAttached)
stepper.setOnDetachHandler(StepperDetached)
stepper.setOnErrorhandler(StepperError)
stepper.setOnCurrentChangeHandler(StepperCurrentChanged)
stepper.setOnInputChangeHandler(StepperInputChanged)
stepper.setOnPositionChangeHandler(StepperPositionChanged)
stepper.setOnVelocityChangeHandler(StepperVelocityChanged)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
# Initialize Stepper
print("Opening phidget object....")
try:
stepper.openPhidget()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
class phidgets_stepper(motor_stage):
## Constructor
#
def __init__(self):
motor_stage.__init__(self, 1)
self.Initialized = True
try:
self.stepper = Stepper()
except RuntimeError as e:
print("Runtime Exception: %s" % e.details)
self.Initialized = False
try:
self.stepper.setOnAttachHandler(self.StepperAttached)
self.stepper.setOnDetachHandler(self.StepperDetached)
self.stepper.setOnErrorhandler(self.StepperError)
self.stepper.setOnCurrentChangeHandler(self.StepperCurrentChanged)
self.stepper.setOnInputChangeHandler(self.StepperInputChanged)
self.stepper.setOnPositionChangeHandler(self.StepperPositionChanged)
self.stepper.setOnVelocityChangeHandler(self.StepperVelocityChanged)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
self.Initialized = False
print("Opening phidget object....")
try:
self.stepper.openPhidget()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
self.Initialized = False
print("Waiting for attach....")
try:
self.stepper.waitForAttach(10000)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
try:
self.stepper.closePhidget()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
self.Initialized = False
self.Initialized = False
else:
self.DisplayDeviceInfo()
try:
self.stepper.setAcceleration(0,4000)
self.stepper.setVelocityLimit(0, 900)
self.stepper.setCurrentLimit(0, 0.700)
sleep(1)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
if(self.Initialized): self.home()
def __del__(self):
try:
self.stepper.setEngaged(0,False)
self.stepper.closePhidget()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
#Information Display Function
def DisplayDeviceInfo(self):
print("|------------|----------------------------------|--------------|------------|")
print("|- Attached -|- Type -|- Serial No. -|- Version -|")
print("|------------|----------------------------------|--------------|------------|")
print("|- %8s -|- %30s -|- %10d -|- %8d -|" % (self.stepper.isAttached(), self.stepper.getDeviceName(), self.stepper.getSerialNum(), self.stepper.getDeviceVersion()))
print("|------------|----------------------------------|--------------|------------|")
print("Number of Motors: %i" % (self.stepper.getMotorCount()))
#Event Handler Callback Functions
def StepperAttached(self,e):
attached = e.device
print("Stepper %i Attached!" % (attached.getSerialNum()))
def StepperDetached(self,e):
detached = e.device
print("Stepper %i Detached!" % (detached.getSerialNum()))
def StepperError(self,e):
try:
source = e.device
print("Stepper %i: Phidget Error %i: %s" % (source.getSerialNum(), e.eCode, e.description))
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
def StepperCurrentChanged(self,e):
source = e.device
#print("Stepper %i: Motor %i -- Current Draw: %6f" % (source.getSerialNum(), e.index, e.current))
def StepperInputChanged(self,e):
source = e.device
print("Stepper %i: Input %i -- State: %s" % (source.getSerialNum(), e.index, e.state))
def StepperPositionChanged(self,e):
source = e.device
print("Stepper %i: Motor %i -- Position: %f" % (source.getSerialNum(), e.index, e.position))
def StepperVelocityChanged(self,e):
source = e.device
print("Stepper %i: Motor %i -- Velocity: %f" % (source.getSerialNum(), e.index, e.velocity))
## Tests whether the device file of the motor stage is open or not
#
# @return Boolean, whether the device file is open or not
def is_open(self):
retvalue = False
try:
retvalue = self.stepper.isAttached()
except PhidgetException as e:
retvalue = False
return (retvalue and self.Initialized)
## Tests whether communication with the device is established
#
# @return Boolean, whether the device answers or not.
def test_communication(self):
return self.is_open()
## Moves the motor stage to a specific coordinate
#
# This function is supposed to command the motor stage
# to move to a specific point which is given in the argument.
# The function should wait until the position is reached
# until it returns. If the position cannot be reached, False
# should be returned.
# @param coordinates Tuple of floating point coordinates that
# descripe the position where the motor stage is supposed to
# move. The number of items in the tuple has to match the
# number of dimensions of the motor stage.
# @return Boolean, whether or not the position was reached or not
def move_absolute(self, coordinates):
if not self.is_open():
return None
if type(coordinates) != list or len(coordinates) != self.dimensions:
return 0
self.stepper.setTargetPosition(0,coordinates[0])
self.stepper.setEngaged(0,True)
sleep(1)
while self.stepper.getCurrentPosition(0) != coordinates[0]:
pass
sleep(1)
retvalue = self.stepper.getCurrentPosition(0) == coordinates[0]
return retvalue
## Moves the motor stage to a specific coordinate relative
# to the current one
#
# This function is supposed to command the motor stage
# to move to a specific point relative to the current point
# which is given in the argument.
# The function should wait until the position is reached
# until it returns. If the position cannot be reached, False
# should be returned.
# @param coordinates Tuple of floating point coordinates that
# descripe the position relative to the current position
# where the motor stage is supposed to move. The number of
# items in the tuple has to match the number of dimensions
# of the motor stage.
# @return Boolean, whether or not the position was reached or not
def move_relative(self, coordinates):
if not self.is_open():
return None
if type(coordinates) != list or len(coordinates) != self.dimensions:
return 0
absolute_cords = coordinates
absolute_cords[0] += self.stepper.getCurrentPosition(0)
return self.move_absolute(absolute_cords)
## Sets the acceleration of the motor stage
#
# This function should communicate with the device
# and set the acceleration of the stage. The devices
# that do not support this should return False.
# @param acceleration Value that the acceleration should
# be set to
# @return Boolean, whether the operation was successful
# or not. If the stage does not support this, it should
# return False.
def set_acceleration(self, acceleration):
if not self.is_open():
return None
if( (acceleration >= self.stepper.getAccelerationMin()) and (acceleration <= self.stepper.getAccelerationMax()) ):
self.Acceleration = acceleration
try:
self.stepper.setAcceleration(0,self.Acceleration)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
def home_switch(self):
if not self.is_open():
return False
return self.stepper.getInputState(0)
## Moves the device to the home position
#
# This function is supposed to move the motor stage to
# its home position to reset its coordinates. The function
# should not return until the movement is complete.
# @return Boolean, whether the position was reached or not
def home(self):
#self.stepper.setAcceleration(0,4000)
#self.stepper.setVelocityLimit(0, 900)
if not self.is_open():
return False
if not self.move_absolute([0]):
return False
if self.home_switch():
#self.stepper.setTargetPosition(0,-500)
self.stepper.setEngaged(0,True)
#while self.stepper.getCurrentPosition(0) != -500:
# pass
#sleep(1)
self.stepper.setCurrentPosition(0,0)
sleep(1)
if not self.home_switch():
self.stepper.setTargetPosition(0,17000)
self.stepper.setEngaged(0,True)
beggining_set=False
home_beggining=0
end_set=False
home_end=0
while self.stepper.getCurrentPosition(0) != 16000:
if (self.stepper.getInputState(0) and not beggining_set):
print "Found home beggining!"
home_beggining = self.stepper.getCurrentPosition(0)
beggining_set = True
elif (beggining_set and not end_set and not self.stepper.getInputState(0)):
print "Found home end!"
home_end = self.stepper.getCurrentPosition(0)
end_set = True
self.stepper.setTargetPosition(0, home_end+100)
elif (beggining_set and end_set and self.stepper.getCurrentPosition(0) !=
(home_end+100)):
self.stepper.setTargetPosition(0, (home_beggining+home_end)/2-50)
sleep(1)
break
if not self.home_switch():
print("Cannot find home position!")
return False
else:
print("position is zero")
self.stepper.setCurrentPosition(0,0)
sleep(1)
return True
## Resets the device and reinitialises it
#
# This function is supposed to bring the device in a known
# state from which other operations can be performed.
# @return Boolean, whether the operation was successful or not
def reset(self):
return self.home()
class RowAssist:
def pretty_print(self, task, msg, *args):
try:
date = datetime.strftime(datetime.now(), "%H:%M:%S.%f")
output = "%s\t%s\t%s" % (date, task, msg)
print output
except:
pass
def __init__(self, config_file):
# Load Config
self.pretty_print("CONFIG", "Loading %s" % config_file)
self.config = json.loads(open(config_file).read())
for key in self.config:
try:
getattr(self, key)
except AttributeError as error:
setattr(self, key, self.config[key])
self.bgr1 = np.zeros((640, 480, 3), np.uint8)
self.bgr2 = np.zeros((640, 480, 3), np.uint8)
self.cv_speed = 0.0
# Initializers
self.run_threads = True
self.init_log() # it's best to run the log first to catch all events
self.init_camera()
self.init_cv_groundspeed()
self.init_stepper()
self.init_controller()
self.init_pid()
self.init_gps()
self.init_qlearner()
self.init_display()
## Initialize Display
def init_display(self):
thread.start_new_thread(self.update_display, ())
## Initialize Ground Speed Matcher
def init_cv_groundspeed(self):
"""
Initialize CV GroundSpeed
"""
self.pretty_print('GSV6', 'Initializing Groundspeed Matcher ...')
try:
self.CVGS = CVGS.CVGS()
thread.start_new_thread(self.update_groundspeed, ())
except Exception as e:
raise e
# Initialize QLearner
def init_qlearner(self):
"""
Initialize Q-Learner
"""
self.pretty_print('QLRN', 'Initializing Q-Learner ...')
try:
"""
X: STEERING_WHEEL POSITION
Y: ERROR
"""
self.qmatrix = np.zeros((self.OUTPUT_MAX, self.CAMERA_WIDTH, 3), np.uint8)
except Exception as e:
raise e
# Initialize Camera
def init_camera(self):
"""
Initialize Camera
"""
# Setting variables
self.pretty_print('CAM', 'Initializing CV Variables ...')
self.CAMERA_CENTER = self.CAMERA_WIDTH / 2
self.pretty_print('CAM', 'Camera Width: %d px' % self.CAMERA_WIDTH)
self.pretty_print('CAM', 'Camera Height: %d px' % self.CAMERA_HEIGHT)
self.pretty_print('CAM', 'Camera Center: %d px' % self.CAMERA_CENTER)
self.pretty_print('CAM', 'Camera Depth: %d cm' % self.CAMERA_DEPTH)
self.pretty_print('CAM', 'Camera FOV: %f rad' % self.CAMERA_FOV)
self.pretty_print('INIT', 'Image Center: %d px' % self.CAMERA_CENTER)
self.GROUND_WIDTH = 2 * self.CAMERA_DEPTH * np.tan(self.CAMERA_FOV / 2.0)
self.pretty_print('CAM', 'Ground Width: %d cm' % self.GROUND_WIDTH)
self.pretty_print('CAM', 'Error Tolerance: +/- %d cm' % self.ERROR_TOLERANCE)
self.PIXEL_PER_CM = self.CAMERA_WIDTH / self.GROUND_WIDTH
self.pretty_print('CAM', 'Pixel-per-cm: %d px/cm' % self.PIXEL_PER_CM)
self.PIXEL_RANGE = int(self.PIXEL_PER_CM * self.ERROR_TOLERANCE)
self.pretty_print('CAM', 'Pixel Range: +/- %d px' % self.PIXEL_RANGE)
self.PIXEL_MIN = self.CAMERA_CENTER - self.PIXEL_RANGE
self.PIXEL_MAX = self.CAMERA_CENTER + self.PIXEL_RANGE
# Set Thresholds
self.threshold_min = np.array([self.HUE_MIN, self.SAT_MIN, self.VAL_MIN], np.uint8)
self.threshold_max = np.array([self.HUE_MAX, self.SAT_MAX, self.VAL_MAX], np.uint8)
# Attempt to set each camera index/name
self.pretty_print('CAM', 'Initializing Camera ...')
self.camera = None
self.bgr = None
self.mask = None
try:
cam = cv2.VideoCapture(0)
cam.set(cv.CV_CAP_PROP_SATURATION, self.CAMERA_SATURATION)
cam.set(cv.CV_CAP_PROP_FRAME_HEIGHT, self.CAMERA_HEIGHT)
cam.set(cv.CV_CAP_PROP_FRAME_WIDTH, self.CAMERA_WIDTH)
(s, bgr) = cam.read()
self.bgr = bgr
self.camera = cam
except Exception as error:
self.pretty_print('CAM', 'ERROR: %s' % str(error))
# Initialize PID Controller
def init_pid(self):
self.pretty_print('PID', 'Initializing Control System')
# Initialize variables
try:
self.estimated = 0
self.projected = 0
self.pretty_print('PID', 'Default number of samples: %d' % self.NUM_SAMPLES)
self.offset_history = [0] * self.NUM_SAMPLES
self.pretty_print('PID', 'Setup OK')
except Exception as error:
self.pretty_print('PID', 'ERROR: %s' % str(error))
# Generate control sys
if self.ALGORITHM == 'PID':
self.sys = fpid.PID(P=self.P_COEF, I=self.I_COEF, D=self.D_COEF)
elif self.ALGORITHM == 'FUZZY':
self.sys = fpid.FPID()
# Initialize Log
def init_log(self):
self.pretty_print('LOG', 'Initializing Log')
self.LOG_NAME = datetime.strftime(datetime.now(), self.LOG_FORMAT)
self.pretty_print('LOG', 'New log file: %s' % self.LOG_NAME)
gps_params = ['gps_lat', 'gps_long', 'gps_speed']
nongps_params = ['time', 'cv_speed', 'est', 'avg', 'diff', 'output', 'steps','encoder','angle']
if self.GPS_ON:
self.log_params = gps_params + nongps_params
else:
self.log_params = nongps_params
try:
self.log = open('logs/' + self.LOG_NAME + '.csv', 'w')
self.log.write('OUTPUT_MIN ' + str(self.OUTPUT_MIN) + '\n')
self.log.write('OUTPUT_MAX ' + str(self.OUTPUT_MAX) + '\n')
self.log.write('P_COEF ' + str(self.P_COEF) + '\n')
self.log.write('I_COEF ' + str(self.I_COEF) + '\n')
self.log.write('D_COEF ' + str(self.D_COEF) + '\n')
self.log.write('VELOCITY ' + str(self.VELOCITY) + '\n')
self.log.write('ACCELERATION ' + str(self.ACCELERATION) + '\n')
self.log.write(','.join(self.log_params + ['\n']))
self.pretty_print('LOG', 'Setup OK')
self.vid_writer = cv2.VideoWriter('logs/' + self.LOG_NAME + '.avi', cv.CV_FOURCC('M', 'J', 'P', 'G'), self.CAMERA_FPS, (self.CAMERA_WIDTH, self.CAMERA_HEIGHT), True)
except Exception as error:
raise error
# Initialize Controller
def init_controller(self):
self.pretty_print('CTRL', 'Initializing controller ...')
try:
self.pretty_print('CTRL', 'Device: %s' % str(self.SERIAL_DEVICE))
self.pretty_print('CTRL', 'Baud Rate: %s' % str(self.SERIAL_BAUD))
self.controller = serial.Serial(self.SERIAL_DEVICE, self.SERIAL_BAUD, timeout=0.05)
self.angle = 0
self.angle_rate = 0
self.encoder = 0
self.encoder_rate = 0
self.encoder_rate_prev = 0
thread.start_new_thread(self.update_controller, ())
self.pretty_print('CTRL', 'Setup OK')
except Exception as error:
self.pretty_print('CTRL', 'ERROR: %s' % str(error))
exit(1)
# Initialize Stepper
def init_stepper(self):
# Constants
self.pretty_print('STEP', 'Output Minimum: %d' % self.OUTPUT_MIN)
self.pretty_print('STEP', 'Output Maximum: %d' % self.OUTPUT_MAX)
# Create
try:
self.pretty_print("STEP", "Creating phidget object....")
self.stepper = Stepper()
except PhidgetException as e:
self.pretty_print("STEP", "Phidget Exception %i: %s" % (e.code, e.details))
# Open
try:
self.pretty_print("STEP", "Opening phidget object....")
self.stepper.openPhidget()
except PhidgetException as e:
self.pretty_print("STEP", "Phidget Exception %i: %s" % (e.code, e.details))
# Settings
try:
self.pretty_print("STEP", "Configuring stepper settings ...")
self.stepper.setOnAttachHandler(self.StepperAttached)
self.stepper.setOnDetachHandler(self.StepperDetached)
self.stepper.setOnErrorhandler(self.StepperError)
self.stepper.setOnCurrentChangeHandler(self.StepperCurrentChanged)
self.stepper.setOnInputChangeHandler(self.StepperInputChanged)
self.stepper.setOnPositionChangeHandler(self.StepperPositionChanged)
self.stepper.setOnVelocityChangeHandler(self.StepperVelocityChanged)
except PhidgetException as e:
self.pretty_print("STEP", "Phidget Exception %i: %s" % (e.code, e.details))
# Attach
try:
self.pretty_print("STEP", "Attaching stepper motor ...")
self.stepper.waitForAttach(1000)
except PhidgetException as e:
self.pretty_print("STEP", "Phidget Exception %i: %s" % (e.code, e.details))
try:
self.stepper.closePhidget()
except PhidgetException as e:
self.pretty_print("STEP", "Phidget Exception %i: %s" % (e.code, e.details))
exit(1)
else:
self.DisplayDeviceInfo()
# Engage
try:
self.pretty_print("STEP", "Engaging stepper motor ...")
self.output = (self.OUTPUT_MIN + self.OUTPUT_MAX) / 2
self.stepper.setCurrentPosition(0, (self.OUTPUT_MIN + self.OUTPUT_MAX) / 2)
self.stepper.setTargetPosition(0, (self.OUTPUT_MIN + self.OUTPUT_MAX) / 2)
self.stepper.setEngaged(0, False)
self.stepper.setVelocityLimit(0, self.VELOCITY)
self.stepper.setAcceleration(0, self.ACCELERATION)
self.stepper.setCurrentLimit(0, self.AMPS)
self.stepper.setEngaged(0, True)
except Exception as error:
self.pretty_print("STEP", "ERROR: %s" % str(error))
exit(2)
# Initialize GPS
def init_gps(self):
""" Initialize GPS """
self.pretty_print('GPS', 'Initializing GPS ...')
self.gps_latitude = 0
self.gps_longitude = 0
self.gps_altitude = 0
self.gps_speed = 0
try:
self.gps = serial.Serial(self.GPS_DEVICE, self.GPS_BAUD)
thread.start_new_thread(self.update_gps, ())
self.pretty_print("GPS", "GPS connected")
except Exception as err:
self.pretty_print('GPS', 'WARNING: GPS not available! %s' % str(err))
## Update Learner
def update_learner(self, ph1, e, group):
self.qmatrix[ph1,e,:] = self.qmatrix[ph1,e,:] + group
return group
## Capture Image
def capture_image(self):
"""
1. Attempt to capture an image
2. Repeat for each capture interface
"""
try:
(s, bgr) = self.camera.read()
if s is False:
self.pretty_print('CAM', 'ERROR: Capture failed')
bgr = None
except KeyboardInterrupt:
raise KeyboardInterrupt
except Exception as e:
raise e
self.bgr2 = self.bgr1
self.bgr1 = bgr # Update the BGR
return bgr
## Plant Segmentation Filter
def plant_filter(self, bgr):
"""
1. RBG --> HSV
2. Set minimum saturation equal to the mean saturation
3. Set minimum value equal to the mean value
4. Take hues within range from green-yellow to green-blue
"""
if bgr is not None:
try:
hsv = cv2.cvtColor(bgr, cv2.COLOR_BGR2HSV)
self.threshold_min[1] = np.percentile(hsv[:,:,1], 100 * self.SAT_MIN / 256) # overwrite the saturation minima
self.threshold_min[2] = np.percentile(hsv[:,:,2], 100 * self.VAL_MIN / 256) # overwrite the value minima
mask = cv2.inRange(hsv, self.threshold_min, self.threshold_max)
except KeyboardInterrupt:
raise KeyboardInterrupt
except Exception as e:
raise e
return mask
## Find Plants
def find_offset(self, mask):
"""
1. Calculates the column summation of the mask
2. Calculates the 95th percentile threshold of the column sum array
3. Finds indicies which are greater than or equal to the threshold
4. Finds the median of this array of indices
5. Repeat for each mask
"""
if mask is not None:
try:
column_sum = mask.sum(axis=0) # vertical summation
centroid = int(np.argmax(column_sum) - self.CAMERA_CENTER + self.CAMERA_OFFSET)
idx = centroid
except KeyboardInterrupt:
raise KeyboardInterrupt
except Exception as error:
self.pretty_print('OFF', '%s' % str(error))
else:
idx = self.CAMERA_OFFSET
return idx
## Best Guess for row based on multiple offsets from indices
def estimate_error(self, idx):
"""
Calculate errors for estimate, average, and differential
"""
try:
t = range(0, self.T_COEF) # the time frame in the past
t_plus = range(self.T_COEF + 1, self.T_COEF *2) # the time frame in the future
val = int(idx)
self.offset_history.append(val)
while len(self.offset_history) > self.NUM_SAMPLES:
self.offset_history.pop(0)
smoothed_values = sig.savgol_filter(self.offset_history, self.T_COEF, 2)
# Estimated
e = int(smoothed_values[-1]) # get latest
if e > self.CAMERA_WIDTH / 2: e = (self.CAMERA_WIDTH / 2) - 1
elif e < -self.CAMERA_WIDTH / 2: e = -self.CAMERA_WIDTH / 2
# Projected
f = np.polyfit(t, smoothed_values[-self.T_COEF:], deg=self.REGRESSION_DEG)
vals = np.polyval(f, t_plus)
de = vals[-1] # differential error
ie = int(np.mean(vals)) # integral error
if ie > self.CAMERA_WIDTH / 2 - 1: ie = (self.CAMERA_WIDTH / 2) -1
elif ie < -self.CAMERA_WIDTH / 2: ie = -self.CAMERA_WIDTH / 2
except Exception as error:
self.pretty_print("ROW", "Error: %s" % str(error))
return e, ie, de
## Calculate Output (Supports different algorithms)
def calculate_output(self, e, ie, de, v, d_phi, d_phi_prev):
"""
Calculates the PID output for the stepper
Arguments: est, avg, diff, speed
Requires: OUTPUT_MAX, OUTPUT_MIN, CENTER_OUTPUT
Returns: output
"""
p = e * self.P_COEF
i = ie * self.I_COEF
d = de * self.D_COEF
output = self.sys.calculate(e, ie, de)
return int(output)
## Read Controller
def update_controller(self):
""" Get info from controller """
a = time.time()
b = time.time()
while self.run_threads:
event = None
try:
s = self.controller.readline()
event = json.loads(s)
b = time.time() # end timer
if event is not None:
self.encoder = event['a']
self.angle = event['b']
self.encoder_rate = (event['a'] - self.encoder) / (b - a)
self.encoder = event['a']
except Exception as error:
print str(error)
a = time.time() # reset timer
## Set Stepper
def set_stepper(self, vel):
""" Set Stepper, returns the dead-reckoning number of steps/position """
a = time.time()
phi_estimated = self.stepper.getCurrentPosition(0)
try:
if self.stepper.isAttached():
phi_target = phi_estimated + vel
# Set Direction
if vel < -self.DEADBAND:
output = self.OUTPUT_MIN
elif vel > self.DEADBAND:
output = self.OUTPUT_MAX
else:
output = phi_estimated
self.stepper.setTargetPosition(0, output) # SETS THE OUTPUT
# Set Velocity
if abs(vel) > self.VELOCITY_MAX:
vel = self.VELOCITY_MAX
self.stepper.setVelocityLimit(0, abs(vel))
except KeyboardInterrupt:
raise KeyboardInterrupt
except Exception as e:
self.close_stepper()
raise e
b = time.time()
return phi_target
## Get Groundspeed
def get_groundspeed(self, images):
""" Get the current groundspeed """
return self.cv_speed # Return current speed
## Log to Mongo
def write_to_db(self, sample):
""" Write results to the database """
try:
assert self.collection is not None
doc_id = self.collection.insert(sample)
self.pretty_print('DB', 'Doc ID: %s' % str(doc_id))
except KeyboardInterrupt:
raise KeyboardInterrupt
except Exception as e:
raise Exception("Failed to insert document")
return doc_id
## Write to Log
def write_to_log(self, sample):
"""
Write results to the log
"""
a = time.time()
try:
data = [str(sample[k]) for k in self.log_params]
newline = ','.join(data + ['\n'])
self.log.write(newline)
self.vid_writer.write(self.bgr)
except KeyboardInterrupt:
raise KeyboardInterrupt
except Exception as e:
self.pretty_print("LOG", str(e))
raise Exception("Failed to write to file document")
b = time.time()
## Update GPS
def update_gps(self):
"""
1. Get the most recent GPS data
2. Set global variables for lat, long and speed
"""
while self.run_threads:
try:
sentence = self.gps.readline()
sentence_parsed = sentence.rsplit(',')
nmea_type = sentence_parsed[0]
if nmea_type == '$GPVTG':
self.gps_speed = float(sentence_parsed[7])
elif nmea_type == '$GPGGA':
self.gps_latitude = float(sentence_parsed[2])
self.gps_longitude = float(sentence_parsed[4])
self.gps_altitude = float(sentence_parsed[9])
except Exception as e:
self.gps_latitude = 0.0
self.gps_longitude = 0.0
self.gps_altitude = 0.0
self.gps_speed = 0.0
## Estimate groundspeed (THREADED)
def update_groundspeed(self, wait=0.05, hist_length=3):
""" Needs: bgr1 and bgr2 """
self.speed_hist = [0] * hist_length
while self.run_threads:
time.sleep(wait) # don't run too fast
try:
bgr1 = self.bgr1
bgr2 = self.bgr2
except Exception as e:
raise e
try:
if not np.all(bgr1==bgr2):
cv_speed = self.CVGS.get_velocity(bgr1, bgr2, fps=self.CAMERA_FPS)
self.speed_hist.reverse()
self.speed_hist.pop()
self.speed_hist.reverse()
self.speed_hist.append(cv_speed)
self.cv_speed = np.mean(self.speed_hist)
except Exception as error:
self.pretty_print('CVGS', 'CV001: %s' % str(error))
## Update Display (THREADED)
def update_display(self):
""" Flash BGR capture to user """
try:
cv2.namedWindow("test")
while self.run_threads:
try:
bgr = self.bgr
bgr[:, self.CAMERA_WIDTH / 2,:] = 255
# draw estimated position
bgr[:,self.estimated + self.CAMERA_WIDTH / 2, 0] = 0
bgr[:,self.estimated + self.CAMERA_WIDTH / 2, 1] = 255
bgr[:,self.estimated + self.CAMERA_WIDTH / 2, 2] = 0
# draw projected position
bgr[:,self.projected + self.CAMERA_WIDTH / 2, 0] = 0
bgr[:,self.projected + self.CAMERA_WIDTH / 2, 1] = 0
bgr[:,self.projected + self.CAMERA_WIDTH / 2, 2] = 255
cv2.imshow("test", bgr)
if cv2.waitKey(5) == 27:
pass
except Exception as error:
self.pretty_print('DISP', 'ERROR: %s' % str(error))
except Exception as error:
self.pretty_print('DISP', 'ERROR: %s' % str(error))
## Close Controller
def close_stepper(self):
"""
Close Controller
"""
self.pretty_print('SYSTEM', 'Closing Stepper ...')
try:
self.stepper.setEngaged(0, False)
self.stepper.closePhidget()
except Exception as error:
self.pretty_print('STEP', 'ERROR: %s' % str(error))
## Close
def close(self):
"""
Function to shutdown application safely
1. Close windows
2. Disable stepper
3. Release capture interfaces
"""
self.pretty_print('SYSTEM', 'Shutting Down Now!')
try:
self.close_stepper() ## Disable stepper
except Exception as error:
self.pretty_print('STEP', 'ERROR: %s' % str(error))
try:
self.controller.close() ## Disable Arduino
except Exception as error:
self.pretty_print('ARD', 'ERROR: %s' % str(error))
try:
self.camera.release() ## Disable camera
except Exception as error:
self.pretty_print('CAM', 'ERROR: %s' % str(error))
cv2.destroyAllWindows() ## Close windows
## Run
def run(self):
"""
Function for Run-time loop
1. Get initial time
2. Capture image
3. Generate mask filter for plant matter
4. Calculate indices of rows
5. Estimate row from image
6. Get number of samples
7. Calculate lateral error after filtering
8. Send output response to stepper
9. Throttle to desired frequency
10. Log results to DB
11. Display results
"""
while self.run_threads:
try:
a = time.time()
bgr = self.capture_image()
if bgr is not None:
cv_speed = self.get_groundspeed(bgr)
mask = self.plant_filter(bgr)
offset = self.find_offset(mask)
(est, avg, diff) = self.estimate_error(offset)
encoder = self.encoder
encoder_rate = self.encoder_rate
encoder_rate_prev = self.encoder_rate_prev
angle = self.angle
output = self.calculate_output(est, avg, diff, cv_speed, encoder_rate, encoder_rate_prev )
self.encoder_rate_prev = encoder_rate
steps = self.set_stepper(output)
sample = {
'offset' : offset,
'est' : est,
'avg' : avg,
'diff' : diff,
'angle' : angle,
'encoder' : encoder,
'encoder_rate' : encoder_rate,
'output': output,
'steps' : steps,
'time' : datetime.strftime(datetime.now(), self.TIME_FORMAT),
'cv_speed' : cv_speed
}
if self.GPS_ON:
gps_sample = {
'gps_long' : self.gps_longitude,
'gps_lat' : self.gps_latitude,
'gps_alt' : self.gps_altitude,
'gps_speed' : self.gps_speed
}
sample.update(gps_sample)
if self.LOGFILE_ON:
self.write_to_log(sample)
self.output = output
self.bgr = bgr
self.mask = mask
self.estimated = est
self.projected = avg
b = time.time()
if self.VERBOSE:
self.pretty_print("STEP", "%d Hz\t%2.1f km/h\t%d mm\t%2.1f d\t%0.2f d/s" % ((1 / float(b-a)),
cv_speed,
est,
encoder * 360 / float(self.ENCODER_RESOLUTION),
encoder_rate * 360 / float(self.ENCODER_RESOLUTION)))
else:
time.sleep(0.01)
except KeyboardInterrupt as error:
self.run_threads = False
self.close()
break
except UnboundLocalError as error:
print "RUN " + str(error)
except Exception as error:
print "RUN " + str(error)
# Information Display Function
def DisplayDeviceInfo(self):
self.pretty_print("STEP", "%8s, %30s, %10d, %8d" % (self.stepper.isAttached(), self.stepper.getDeviceName(), self.stepper.getSerialNum(), self.stepper.getDeviceVersion()))
self.pretty_print("STEP", "Number of Motors: %i" % (self.stepper.getMotorCount()))
# Event Handler Callback Functions
def StepperAttached(self, e):
attached = e.device
self.pretty_print("STEP", "Stepper %i Attached!" % (attached.getSerialNum()))
def StepperDetached(self, e):
detached = e.device
self.pretty_print("STEP", "Stepper %i Detached!" % (detached.getSerialNum()))
def StepperError(self, e):
try:
source = e.device
self.pretty_print("STEP", "Stepper %i: Phidget Error %i: %s" % (source.getSerialNum(), e.eCode, e.description))
except PhidgetException as e:
self.pretty_print("STEP", "Phidget Exception %i: %s" % (e.code, e.details))
def StepperCurrentChanged(self, e):
source = e.device
#self.pretty_print("STEP", "Stepper %i: Motor %i -- Current Draw: %6f" % (source.getSerialNum(), e.index, e.current))
def StepperInputChanged(self, e):
source = e.device
#self.pretty_print("STEP", "Stepper %i: Input %i -- State: %s" % (source.getSerialNum(), e.index, e.state))
def StepperPositionChanged(self, e):
source = e.device
#self.pretty_print("STEP", "Stepper %i: Motor %i -- Position: %f" % (source.getSerialNum(), e.index, e.position))
def StepperVelocityChanged(self, e):
source = e.device
class Agrivision_Arm:
def __init__(self):
try:
self.stepper = Stepper()
except RuntimeError as e:
print("Runtime Exception: %s" % e.details)
try:
self.stepper.setOnAttachHandler(self.StepperAttached)
self.stepper.setOnDetachHandler(self.StepperDetached)
self.stepper.setOnErrorhandler(self.StepperError)
self.stepper.setOnCurrentChangeHandler(self.StepperCurrentChanged)
self.stepper.setOnInputChangeHandler(self.StepperInputChanged)
self.stepper.setOnPositionChangeHandler(self.StepperPositionChanged)
self.stepper.setOnVelocityChangeHandler(self.StepperVelocityChanged)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
try:
self.stepper.openPhidget()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Waiting for attach....")
try:
self.stepper.waitForAttach(10000)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
try:
self.stepper.closePhidget()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
self.SetParameters()
def SetParameters(self, acceleration=200000, velocity=200000, current=0.75):
try:
self.stepper.setCurrentPosition(0, 0)
self.stepper.setEngaged(0, True) #! INTERESTING
self.stepper.setAcceleration(0, acceleration) #! INTERESTING
self.stepper.setVelocityLimit(0, velocity) #! INTERESTING
self.stepper.setCurrentLimit(0, current) #! INTERESTING
sleep(2)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
## PID STYLE CONTROL SYSTEM
def AdjustPosition(self, position):
try:
print("Will now move to position %d..." % position)
self.stepper.setTargetPosition(0, position)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
def close(self):
try:
self.stepper.setEngaged(0, False)
sleep(1)
self.stepper.closePhidget()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
def DisplayDeviceInfo(self):
print("%8s, %30s, %10d, %8d" % (stepper.isAttached(), stepper.getDeviceName(), stepper.getSerialNum(), stepper.getDeviceVersion()))
print("Number of Motors: %i" % (stepper.getMotorCount()))
def StepperAttached(self, e):
attached = e.device
print("Stepper %i Attached!" % (attached.getSerialNum()))
def StepperDetached(self, e):
detached = e.device
print("Stepper %i Detached!" % (detached.getSerialNum()))
def StepperError(self, e):
try:
source = e.device
print("Stepper %i: Phidget Error %i: %s" % (source.getSerialNum(), e.eCode, e.description))
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
def StepperCurrentChanged(self, e):
source = e.device
print("Stepper %i: Motor %i -- Current Draw: %6f" % (source.getSerialNum(), e.index, e.current))
def StepperInputChanged(self, e):
source = e.device
print("Stepper %i: Input %i -- State: %s" % (source.getSerialNum(), e.index, e.state))
def StepperPositionChanged(self, e):
source = e.device
print("Stepper %i: Motor %i -- Position: %f" % (source.getSerialNum(), e.index, e.position))
def StepperVelocityChanged(self, e):
source = e.device
print("Stepper %i: Motor %i -- Velocity: %f" % (source.getSerialNum(), e.index, e.velocity))
class Agrivision_Arm:
def __init__(self,
acceleration=30000,
velocity=30000,
current=4.0,
position=50000):
try:
self.stepper = Stepper()
except RuntimeError as e:
print("Runtime Exception: %s" % e.details)
try:
self.stepper.setOnAttachHandler(self.StepperAttached)
self.stepper.setOnDetachHandler(self.StepperDetached)
self.stepper.setOnErrorhandler(self.StepperError)
self.stepper.setOnCurrentChangeHandler(self.StepperCurrentChanged)
self.stepper.setOnInputChangeHandler(self.StepperInputChanged)
self.stepper.setOnPositionChangeHandler(
self.StepperPositionChanged)
self.stepper.setOnVelocityChangeHandler(
self.StepperVelocityChanged)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
try:
self.stepper.openPhidget()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Waiting for attach....")
try:
self.stepper.waitForAttach(10000)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
try:
self.stepper.closePhidget()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
self.SetParameters(acceleration, velocity, current, position)
def SetParameters(self,
acceleration=30000,
velocity=30000,
current=4.0,
position=50000):
try:
self.stepper.setCurrentPosition(0, 50000)
self.stepper.setEngaged(0, True) #! INTERESTING
self.stepper.setAcceleration(0, acceleration) #! INTERESTING
self.stepper.setVelocityLimit(0, velocity) #! INTERESTING
self.stepper.setCurrentLimit(0, current) #! INTERESTING
sleep(2)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
## PID STYLE CONTROL SYSTEM
def AdjustPosition(self, position):
try:
print("Will now move to position %d..." % position)
self.stepper.setTargetPosition(0, position)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
def close(self):
try:
self.stepper.setEngaged(0, False)
sleep(1)
self.stepper.closePhidget()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
def DisplayDeviceInfo(self):
print("%8s, %30s, %10d, %8d" %
(stepper.isAttached(), stepper.getDeviceName(),
stepper.getSerialNum(), stepper.getDeviceVersion()))
print("Number of Motors: %i" % (stepper.getMotorCount()))
def StepperAttached(self, e):
attached = e.device
print("Stepper %i Attached!" % (attached.getSerialNum()))
def StepperDetached(self, e):
detached = e.device
print("Stepper %i Detached!" % (detached.getSerialNum()))
def StepperError(self, e):
try:
source = e.device
print("Stepper %i: Phidget Error %i: %s" %
(source.getSerialNum(), e.eCode, e.description))
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
def StepperCurrentChanged(self, e):
source = e.device
print("Stepper %i: Motor %i -- Current Draw: %6f" %
(source.getSerialNum(), e.index, e.current))
def StepperInputChanged(self, e):
source = e.device
print("Stepper %i: Input %i -- State: %s" %
(source.getSerialNum(), e.index, e.state))
def StepperPositionChanged(self, e):
source = e.device
print("Stepper %i: Motor %i -- Position: %f" %
(source.getSerialNum(), e.index, e.position))
def StepperVelocityChanged(self, e):
source = e.device
print("Stepper %i: Motor %i -- Velocity: %f" %
(source.getSerialNum(), e.index, e.velocity))
def AttachStepper(databasepath, serialNumber):
def onAttachHandler(event):
logString = "Stepper Attached " + str(event.device.getSerialNum())
#print(logString)
DisplayAttachedDeviceInfo(event.device)
def onDetachHandler(event):
logString = "Stepper Detached " + str(event.device.getSerialNum())
#print(logString)
DisplayDetachedDeviceInfo(event.device)
event.device.closePhidget()
def onErrorHandler(event):
logString = "Stepper Error " + str(
event.device.getSerialNum()) + ", Error: " + event.description
#print(logString)
DisplayErrorDeviceInfo(event.device)
def onServerConnectHandler(event):
logString = "Stepper Server Connect " + str(
event.device.getSerialNum())
#print(logString)
def onServerDisconnectHandler(event):
logString = "Stepper Server Disconnect " + str(
event.device.getSerialNum())
#print(logString)
def currentChangeHandler(event):
logString = "Stepper Current Changed"
#print(logString)
try:
conn = sqlite3.connect(databasepath)
conn.execute(
"INSERT INTO STEPPER_CURRENTCHANGE VALUES(NULL, DateTime('now'), ?, ?, ?)",
(event.device.getSerialNum(), event.index, event.current))
conn.commit()
conn.close()
except sqlite3.Error as e:
print "An error occurred:", e.args[0]
def inputChangeHandler(event):
logString = "Stepper Input Changed"
#print(logString)
try:
conn = sqlite3.connect(databasepath)
conn.execute(
"INSERT INTO STEPPER_INPUTCHANGE VALUES(NULL, DateTime('now'), ?, ?, ?)",
(event.device.getSerialNum(), event.index, int(event.state)))
conn.commit()
conn.close()
except sqlite3.Error as e:
print "An error occurred:", e.args[0]
def positionChangeHandler(event):
logString = "Stepper Position Changed"
#print(logString)
try:
conn = sqlite3.connect(databasepath)
conn.execute(
"INSERT INTO STEPPER_POSITIONCHANGE VALUES(NULL, DateTime('now'), ?, ?, ?)",
(event.device.getSerialNum(), event.index, event.position))
conn.commit()
conn.close()
except sqlite3.Error as e:
print "An error occurred:", e.args[0]
def velocityChangeHandler(event):
logString = "Stepper Velocity Changed"
#print(logString)
try:
conn = sqlite3.connect(databasepath)
conn.execute(
"INSERT INTO STEPPER_VELOCITYCHANGE VALUES(NULL, DateTime('now'), ?, ?, ?)",
(event.device.getSerialNum(), event.index, event.velocity))
conn.commit()
conn.close()
except sqlite3.Error as e:
print "An error occurred:", e.args[0]
try:
p = Stepper()
p.setOnAttachHandler(onAttachHandler)
p.setOnDetachHandler(onDetachHandler)
p.setOnErrorhandler(onErrorHandler)
p.setOnServerConnectHandler(onServerConnectHandler)
p.setOnServerDisconnectHandler(onServerDisconnectHandler)
p.setOnCurrentChangeHandler(currentChangeHandler)
p.setOnInputChangeHandler(inputChangeHandler)
p.setOnPositionChangeHandler(positionChangeHandler)
p.setOnVelocityChangeHandler(velocityChangeHandler)
p.openPhidget(serialNumber)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting...")
exit(1)
def AttachStepper(databasepath, serialNumber):
def onAttachHandler(event):
logString = "Stepper Attached " + str(event.device.getSerialNum())
#print(logString)
DisplayAttachedDeviceInfo(event.device)
def onDetachHandler(event):
logString = "Stepper Detached " + str(event.device.getSerialNum())
#print(logString)
DisplayDetachedDeviceInfo(event.device)
event.device.closePhidget()
def onErrorHandler(event):
logString = "Stepper Error " + str(event.device.getSerialNum()) + ", Error: " + event.description
#print(logString)
DisplayErrorDeviceInfo(event.device)
def onServerConnectHandler(event):
logString = "Stepper Server Connect " + str(event.device.getSerialNum())
#print(logString)
def onServerDisconnectHandler(event):
logString = "Stepper Server Disconnect " + str(event.device.getSerialNum())
#print(logString)
def currentChangeHandler(event):
logString = "Stepper Current Changed"
#print(logString)
try:
conn = sqlite3.connect(databasepath)
conn.execute("INSERT INTO STEPPER_CURRENTCHANGE VALUES(NULL, DateTime('now'), ?, ?, ?)",
(event.device.getSerialNum(), event.index, event.current))
conn.commit()
conn.close()
except sqlite3.Error as e:
print "An error occurred:", e.args[0]
def inputChangeHandler(event):
logString = "Stepper Input Changed"
#print(logString)
try:
conn = sqlite3.connect(databasepath)
conn.execute("INSERT INTO STEPPER_INPUTCHANGE VALUES(NULL, DateTime('now'), ?, ?, ?)",
(event.device.getSerialNum(), event.index, int(event.state)))
conn.commit()
conn.close()
except sqlite3.Error as e:
print "An error occurred:", e.args[0]
def positionChangeHandler(event):
logString = "Stepper Position Changed"
#print(logString)
try:
conn = sqlite3.connect(databasepath)
conn.execute("INSERT INTO STEPPER_POSITIONCHANGE VALUES(NULL, DateTime('now'), ?, ?, ?)",
(event.device.getSerialNum(), event.index, event.position))
conn.commit()
conn.close()
except sqlite3.Error as e:
print "An error occurred:", e.args[0]
def velocityChangeHandler(event):
logString = "Stepper Velocity Changed"
#print(logString)
try:
conn = sqlite3.connect(databasepath)
conn.execute("INSERT INTO STEPPER_VELOCITYCHANGE VALUES(NULL, DateTime('now'), ?, ?, ?)",
(event.device.getSerialNum(), event.index, event.velocity))
conn.commit()
conn.close()
except sqlite3.Error as e:
print "An error occurred:", e.args[0]
try:
p = Stepper()
p.setOnAttachHandler(onAttachHandler)
p.setOnDetachHandler(onDetachHandler)
p.setOnErrorhandler(onErrorHandler)
p.setOnServerConnectHandler(onServerConnectHandler)
p.setOnServerDisconnectHandler(onServerDisconnectHandler)
p.setOnCurrentChangeHandler(currentChangeHandler)
p.setOnInputChangeHandler(inputChangeHandler)
p.setOnPositionChangeHandler(positionChangeHandler)
p.setOnVelocityChangeHandler(velocityChangeHandler)
p.openPhidget(serialNumber)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting...")
exit(1)
