コード例 #1
0
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
コード例 #2
0
ファイル: Stepper-simple.py プロジェクト: larsendt/xhab-rogr
def StepperInputChanged(e):
    source = e.device
    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:
コード例 #3
0
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)

# Initialize Stepper
print("Opening phidget object....")
try:
    stepper.openPhidget()
except PhidgetException as e:
コード例 #4
0
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()
コード例 #5
0
ファイル: row-assist.py プロジェクト: trevstanhope/row-assist
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
コード例 #6
0
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))
コード例 #7
0
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))
コード例 #8
0
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)
コード例 #9
0
ファイル: Stepper.py プロジェクト: njligames/SBCLogger
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)