コード例 #1
0
class MotorController(object):
    def __init__(self, vel=5000, acc=5000, serial=-1):
        self.stepper = Stepper()
        self.current_position = 0
        self.base_velocity = vel
        self.acceleration = acc
        self.serial = serial

    def attach(self):
        try:
            print "Opening Phidget... (serial:", self.serial, ")"
            self.stepper.openPhidget(self.serial)
        except PhidgetException as e:
            print "Phidget Exception %i: %s" % (e.code, e.details)

        try:
            print "Waiting 5 seconds for attach..."
            self.stepper.waitForAttach(5000)
        except PhidgetException as e:
            print "Phidget Exception %i: %s" % (e.code, e.details)
            self.stepper.closePhidget()

        print "Setting limits..."
        self.current_position = self.stepper.getCurrentPosition(0)
        self.stepper.setCurrentPosition(0, self.current_position)
        self.stepper.setEngaged(0, True)
        self.stepper.setAcceleration(0, self.acceleration)
        self.stepper.setVelocityLimit(0, self.base_velocity)
        self.stepper.setCurrentLimit(0, 1.70)
        print "Done!"


    def drive(self, multiplier, steps):
        self.current_position = self.stepper.getCurrentPosition(0)

        if multiplier > 0:
            dv = int(steps * (abs(multiplier) ** 2))
            print "dv", dv
            self.current_position += dv
        elif multiplier < 0:
            dv = int(steps * (abs(multiplier) ** 2))
            print "dv", dv
            self.current_position -= dv
        else:
            print "dv", 0

        self.stepper.setTargetPosition(0, self.current_position)

    def set_position(self, position):
        self.current_position = position
        self.stepper.setTargetPosition(0, self.current_position)

    def move_position(self, pos_delta, upper_bound, lower_bound):
        self.current_position += pos_delta
        if self.current_position > upper_bound:
            self.current_position = upper_bound
        elif self.current_position < lower_bound:
            self.current_position = lower_bound

        self.stepper.setTargetPosition(0, self.current_position)
コード例 #2
0
ファイル: joeMotor.py プロジェクト: UWMRO/Guider
class PhidgetMotorController(object):
	def __init__(self):
		self.stepper = Stepper()
		self.stepper.openPhidget()
		print('attaching stepper dev ...')
		self.stepper.waitForAttach(10000)
		self.DisplayDeviceInfo()
		

	def DisplayDeviceInfo(self):
    		print("|- %8s -|- %30s -|- %10d -|- %8d -|" % (self.stepper.isAttached(), self.stepper.getDeviceName(), self.stepper.getSerialNum(), self.stepper.getDeviceVersion()))
    		print("Number of Motors: %i" % (self.stepper.getMotorCount()))

	def disconnDev(self):
		self.motorPower(False)
		self.stepper.closePhidget()

	def setupParm(self):
		self.stepper.setAcceleration(0, 90000)
	    	self.stepper.setVelocityLimit(0, 6200)
    		self.stepper.setCurrentLimit(0, 0.3)
		self.stepper.setCurrentPosition(0,0)

	def moveMotor(self, pos = None):
		self.stepper.setTargetPosition(0, int(pos))
		while self.stepper.getCurrentPosition(0) != int(pos) :
			print self.stepper.getCurrentPosition(0)
			time.sleep(.1)

	def motorPower(self, val = False):
		self.stepper.setEngaged(0,val)
コード例 #3
0
class PhidgetMotorController(object):
    def __init__(self):
        self.stepper = Stepper()
        self.stepper.openPhidget()
        print('attaching stepper dev ...')
        self.stepper.waitForAttach(10000)
        self.DisplayDeviceInfo

    def DisplayDeviceInfo(self):
        print("|- %8s -|- %30s -|- %10d -|- %8d -|" %
              (self.stepper.isAttached(), self.stepper.getDeviceName(),
               self.stepper.getSerialNum(), self.stepper.getDeviceVersion()))
        print("Number of Motors: %i" % (self.stepper.getMotorCount()))

    def disconnDev(self):
        #		print "Setting to Home Position"
        #		self.stepper.setTargetPosition(0, 0)
        #		time.sleep(4)
        #		print "Shutting Down"
        self.motorPower(False)
        #		print "Goodbye"
        self.stepper.closePhidget()

    def setupParm(self):
        self.stepper.setAcceleration(0, 30000)
        self.stepper.setVelocityLimit(0, 8000)
        self.stepper.setCurrentLimit(0, 1.0)
        self.stepper.setCurrentPosition(0, 0)

    def moveMotor(self, pos=None):
        self.stepper.setTargetPosition(0, int(pos))
        while self.stepper.getCurrentPosition(0) != int(pos):
            print self.stepper.getCurrentPosition(0)
            time.sleep(.1)

    def motorPower(self, val=False):
        self.stepper.setEngaged(0, val)

    def filterselect(self, num=None):
        print "Moving to filter position %d" % num
        if int(num) <= 6 and int(num) >= 1:
            self.stepper.setTargetPosition(0, int(num) * 6958)
        elif int(num) > 6:
            print "Not Valid Filter Number"
        elif int(num) < 1:
            print "Not Valid Filter Number"
コード例 #4
0
ファイル: JoeMotor.py プロジェクト: UWMRO/Guider
class PhidgetMotorController(object):
	def __init__(self):
		self.stepper = Stepper()
		self.stepper.openPhidget()
		print('attaching stepper dev ...')
		self.stepper.waitForAttach(10000)
		self.DisplayDeviceInfo
		

	def DisplayDeviceInfo(self):
    		print("|- %8s -|- %30s -|- %10d -|- %8d -|" % (self.stepper.isAttached(), self.stepper.getDeviceName(), self.stepper.getSerialNum(), self.stepper.getDeviceVersion()))
    		print("Number of Motors: %i" % (self.stepper.getMotorCount()))

	def disconnDev(self):
#		print "Setting to Home Position"
#		self.stepper.setTargetPosition(0, 0)
#		time.sleep(4)
#		print "Shutting Down"
		self.motorPower(False)
#		print "Goodbye"
		self.stepper.closePhidget()

	def setupParm(self):
		self.stepper.setAcceleration(0, 30000)
	    	self.stepper.setVelocityLimit(0, 8000)
    		self.stepper.setCurrentLimit(0, 1.0)
		self.stepper.setCurrentPosition(0,0)

	def moveMotor(self, pos = None):
		self.stepper.setTargetPosition(0, int(pos))
		while self.stepper.getCurrentPosition(0) != int(pos) :
			print self.stepper.getCurrentPosition(0)
			time.sleep(.1)

	def motorPower(self, val = False):
		self.stepper.setEngaged(0,val)

	def filterselect(self, num = None):
		print "Moving to filter position %d" % num
		if int(num)<= 6 and int(num)>=1:
			self.stepper.setTargetPosition(0, int(num)*6958)
		elif int(num)>6:
			print "Not Valid Filter Number"
		elif int(num)<1:
			print "Not Valid Filter Number"
コード例 #5
0
 stepper.setCurrentPosition(0,0)
 stepper_1.setCurrentPosition(0,0)
 #Start the stepper motors
 stepper.setEngaged(0,True)
 stepper_1.setEngaged(0,True)
 #Set up the speed, acceleration,and current
 stepper.setAcceleration(0,40543)
 stepper.setCurrentLimit(0,0.26)
 stepper.setVelocityLimit(0,10000)
 stepper_1.setAcceleration(0,100000)
 stepper_1.setCurrentLimit(0,0.26)
 stepper_1.setVelocityLimit(0,60000)
 sleep(2)
 try:
     #report where they are
     print ("the current position for motor 1 is %lf"% (step2angel(stepper.getCurrentPosition(0),1)))
     print ("the current position for motor 2 is %lf"% (step2angel(stepper_1.getCurrentPosition(0),2)))
     while(True):
         target_angel_1 = raw_input("Please enter the target angel for stepper 1:")
         target_angel_2 = raw_input("Please enter the target angel for stepper 2:")
         target_position_1 = angel2step(float(target_angel_1),1)
         target_position_2 = angel2step(float(target_angel_2),2)
         stepper.setTargetPosition(0,target_position_1)
         stepper_1.setTargetPosition(0,target_position_2)
         sleep(2)
         current_1 = step2angel(stepper.getCurrentPosition(0),1)
         current_2 = step2angel(stepper_1.getCurrentPosition(0),2)
         print ("the current position for motor 1 is %lf"% (stepper.getCurrentPosition(0)))
         print ("the current position for motor 2 is %lf"% (stepper_1.getCurrentPosition(0)))
 except KeyboardInterrupt:
     pass
コード例 #6
0
class PhidgetsController(kp.Module):
    def configure(self):
        from Phidgets.Devices.Stepper import Stepper
        from Phidgets.Devices.Encoder import Encoder

        self.current_limit = self.get("current_limit") or 2.5
        self.motor_id = self.get("motor_id") or 0
        self.stepper = Stepper()
        self.encoder = Encoder()
        self.setup(self.motor_id)

    def setup(self, motor_id):
        self.stepper.openPhidget()
        self.stepper.waitForAttach(10000)

        self.encoder.openPhidget()
        self.encoder.waitForAttach(10000)

        self.stepper.setVelocityLimit(motor_id, 1000)
        self.stepper.setAcceleration(motor_id, 5000)
        self.stepper.setCurrentLimit(motor_id, 2.5)

        self.e = 13250.0
        self.s = 70500.0

        self._stepper_dest = 0
        self._encoder_dest = 0

        self.reset_positions()

    def drive_to_angle(self, ang, motor_id=0, relative=False):
        stepper_dest = self._stepper_dest = self.raw_stepper_position(ang)
        self._encoder_dest = self.raw_encoder_position(ang)

        if relative:
            self.reset_positions()

        self.wake_up()
        time.sleep(0.1)

        self.stepper_target_pos = stepper_dest
        self.wait_for_stepper()
        self.log_offset()

        while abs(self.offset) > 1:
            self.log_offset()
            stepper_offset = round(self.offset / self.e * self.s)
            log.debug("Correcting stepper by {0}".format(stepper_offset))
            log.debug("Stepper target pos: {0}".format(
                self.stepper_target_pos))
            log.debug("Stepper pos: {0}".format(self.stepper_pos))
            self.stepper_target_pos = self.stepper_pos + stepper_offset
            self.wait_for_stepper()

        self.log_positions()
        self.stand_by()

    def wait_for_stepper(self):
        while self.stepper_pos != self._stepper_dest:
            time.sleep(0.1)
            self.log_positions()

    def log_offset(self):
        log.debug("Difference (encoder): {0}".format(self.offset))

    @property
    def offset(self):
        return self._encoder_dest - self.encoder_pos

    @property
    def stepper_target_pos(self):
        return self.stepper.getTargetPosition(self.motor_id)

    @stepper_target_pos.setter
    def stepper_target_pos(self, val):
        self._stepper_dest = int(val)
        self.stepper.setTargetPosition(self.motor_id, int(val))

    @property
    def stepper_pos(self):
        return self.stepper.getCurrentPosition(self.motor_id)

    @stepper_pos.setter
    def stepper_pos(self, val):
        self.stepper.setCurrentPosition(self.motor_id, int(val))

    @property
    def encoder_pos(self):
        return self.encoder.getPosition(self.motor_id)

    def raw_stepper_position(self, angle):
        return round(angle * self.s / 360)

    def raw_encoder_position(self, angle):
        return round(angle * self.e / 360)

    def wake_up(self, motor_id=0):
        self.stepper.setEngaged(motor_id, 1)

    def stand_by(self, motor_id=0):
        self.stepper.setEngaged(motor_id, 0)

    def reset_positions(self, motor_id=0):
        self.stepper.setCurrentPosition(motor_id, 0)
        self.encoder.setPosition(motor_id, 0)

    def log_positions(self, motor_id=0):
        log.info("Stepper position: {0}\nEncoder position:{1}".format(
            self.stepper_pos / self.s * 360, self.encoder_pos / self.e * 360))
コード例 #7
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()
コード例 #8
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
コード例 #9
0
#Here We start our code to move the motor as we expected
try:
    #set up the zero position
    stepper.setCurrentPosition(0,0)
    stepper_1.setCurrentPosition(0,0)
    #Start the stepper motors
    stepper.setEngaged(0,True)
    stepper_1.setEngaged(0,True)
    #Set up the speed, acceleration,and current
    setup_limit(stepper,0,1645*10,1.5,1645*10)#speed normally 4000
    setup_limit(stepper_1,0,8597*10,1.0,8597*10)
    sleep(2)
    try:
        #report where they are
        print ("the current position for motor 1 is %lf"% (step2angel(stepper.getCurrentPosition(0),1)))
        print ("the current position for motor 2 is %lf"% (step2angel(stepper_1.getCurrentPosition(0),2)))
        while(True):
            model = raw_input("Please choose model, 1 for FK, 2 for IK, 3 for back to initial position, 4 for exit")
            if model == "1":
                target_degree_theta1= float(raw_input("Please enter the degree of bottom motor:"))
                target_degree_theta2= float(raw_input("Please enter the degree of upper motor:"))
                if target_degree_theta2 <= 0:
                    print "Warnning! Cannot caululate FK"
                elif target_degree_theta1 > 0:
                    target_position_stepper = angel2step(target_degree_theta1,1)
                    target_position_stepper_1 = angel2step(target_degree_theta2,2)
                    stepper.setTargetPosition(0,target_position_stepper)
                    stepper_1.setTargetPosition(0,-1*target_position_stepper_1)
                    target_degree_theta1 = math.radians(float(target_degree_theta1))
                    target_degree_theta2 = math.radians(float(target_degree_theta2))
コード例 #10
0
            print("Disengaging the motor ...")
            stepper.setEngaged(0, False)
            vel = int(raw_input("Specify Velocity: "))
            stepper.setVelocityLimit(0, vel)
            acc = int(raw_input("Specify Acceleration: "))
            stepper.setAcceleration(0, acc)
            amp = float(raw_input("Specify Amperage: "))
            stepper.setCurrentLimit(0, amp)
            print("Engaging the motor ...")
            stepper.setEngaged(0, True)
            sleep(2)
            while True:
                try:
                    q = int(raw_input("Set target position: "))
                    stepper.setTargetPosition(0, q)
                    while stepper.getCurrentPosition(0) != q:
                        pass
                    sleep(2)
                except KeyboardInterrupt:
                    break
        except Exception as e:
            print str(e)
            break
except PhidgetException as e:
    print("Phidget Exception %i: %s" % (e.code, e.details))
    print("Exiting....")
    exit(1)
except KeyboardInterrupt:
    print("Closing...")
    try:
        stepper.setEngaged(0, False)
コード例 #11
0
    sleep(1)
    
    print("Set the motor as engaged...")
    stepper.setEngaged(0, True)
    sleep(1)

    print("The motor will run until it reaches the set goal position...")
    
    stepper.setAcceleration(0, 300000)   #87,543 120,536
    stepper.setVelocityLimit(0, 250000) #6200 414287 250000=MAX
    stepper.setCurrentLimit(0, 1.60) #.26 #MAX 1.6
    sleep(2)# 2
    
    print("Will now move to position 20000...")
    stepper.setTargetPosition(0, 10*318000)# 400000=MORE THAN 1
    while (stepper.getCurrentPosition(0) != 400000):
        try:
            pass
        except KeyboardInterrupt:
            print("key was pressed!!!")
            exit(1)
            break
    #sleep(.1)
    
    #print("Will now move back to positon 0...")
    #stepper.setTargetPosition(0, 0)
    #while stepper.getCurrentPosition(0) != 0:
    #   pass
    
except PhidgetException as e:
    print("Phidget Exception %i: %s" % (e.code, e.details))
コード例 #12
0
    sleep(1)

    print("Set the motor as engaged...")
    stepper.setEngaged(0, True)
    sleep(1)

    print("The motor will run until it reaches the set goal position...")

    stepper.setAcceleration(0, 87543)
    stepper.setVelocityLimit(0, 6200)
    stepper.setCurrentLimit(0, 0.26)
    sleep(2)

    print("Will now move to position 20000...")
    stepper.setTargetPosition(0, 20000)
    while stepper.getCurrentPosition(0) != 20000:
        pass

    sleep(2)

    print("Will now move back to positon 0...")
    stepper.setTargetPosition(0, 0)
    while stepper.getCurrentPosition(0) != 0:
        pass

except PhidgetException as e:
    print("Phidget Exception %i: %s" % (e.code, e.details))
    print("Exiting....")
    exit(1)

print("Press Enter to quit....")
コード例 #13
0
ファイル: Stepper-simple.py プロジェクト: COS518/mobot
    sleep(1)
    
    print("Set the motor as engaged...")
    stepper.setEngaged(0, True)
    sleep(1)
    
    print("The motor will run until it reaches the set goal position...")
    
    stepper.setAcceleration(0, 87543)
    stepper.setVelocityLimit(0, 40000)
    stepper.setCurrentLimit(0, .7)
    sleep(2)
    
    print("Will now move to position 320000...")
    stepper.setTargetPosition(0, 288000)
    while stepper.getCurrentPosition(0) != 288000:
        pass
    
    sleep(2)
    
    print("Will now move back to positon 0...")
    stepper.setTargetPosition(0, 0)
    while stepper.getCurrentPosition(0) != 0:
        pass
    
except PhidgetException as e:
    print("Phidget Exception %i: %s" % (e.code, e.details))
    print("Exiting....")
    exit(1)

print("Press Enter to quit....")
コード例 #14
0
ファイル: phidget_stepper.py プロジェクト: teiche/asi
class PhidgetStepperSlider(AbstractSlider):
    def __init__(self):
        super(PhidgetStepperSlider, self).__init__()

        logger.info("Connecting to Phidget stepper driver...")
        try:
            self.stepper = Stepper()
        except RuntimeError as e:
            logger.error("Could not connect to Phidgets stepper driver: {s}".format(s=e.details))
            sys.exit(1)

        logger.info("Opening Phidget stepper driver...")

        try:
            self.stepper.openPhidget()
        except PhidgetException as e:
            logger.error("Could not open Phidget stepper driver: {e}".format(e=e.details))
            sys.exit(1)

        logger.info("Waiting for Phidget to attach...")
        try:
            self.stepper.waitForAttach(1000)
        except PhidgetException as e:
            logger.error("Could not wait for Phidget to attach: {e}".format(e=e.details))
            sys.exit(1)

        logger.info("Successfully connected to Phidget stepper driver.")
        logger.info("    Device Name: " + self.stepper.getDeviceName())
        logger.info("    Serial Number: " + str(self.stepper.getSerialNum()))
        logger.info("    Device Version: " + str(self.stepper.getDeviceVersion()))
                
        # Assume the slider is currently at the acquisition camera
        self.stepper.setCurrentPosition(config.slider_motor_id, config.slider_acquis_pos)

    def _set_target(self, target):
        self.target = target
        self.stepper.setTargetPosition(config.slider_motor_id, target)
        
    def to_acquisition(self):
        logger.info("Slider to acquisition camera")
        
        self.stepper.setEngaged(config.slider_motor_id, True)
        self._set_target(config.slider_acquis_pos)

    def to_science(self):
        logger.info("Slider to science camera")

        self.stepper.setEngaged(config.slider_motor_id, True)
        self._set_target(config.slider_sci_pos)

    def get_pos(self):
        pos = self.stepper.getCurrentPosition(0)
        
        if pos == config.slider_acquis_pos:
            return self.ACQUISITOIN

        elif pos == config.slider_sci_pos:
            return self.SLIDER

        else:
            return self.MOVING

    def ready(self):
        return self.target == self.stepper.getCurrentPosition(0)
コード例 #15
0
    sleep(0.1)

    print("The motor will run until it reaches the set goal position...")

    stepper.setAcceleration(0, 87543)

    stepper.setVelocityLimit(0, 6200)

    stepper.setCurrentLimit(0, 0.26)

    sleep(0.2)

    print("Will now move to position primaryTargetPosition...")
    stepper.setTargetPosition(0, primaryTargetPosition)

    while stepper.getCurrentPosition(0) != primaryTargetPosition:

        pass

    sleep(0.2)
    """
    print("Will now move back to positon 0...")

    stepper.setTargetPosition(0, 0)

    while stepper.getCurrentPosition(0) != 0:

        pass
    """
except PhidgetException as e:
    print("Phidget Exception %i: %s" % (e.code, e.details))
コード例 #16
0
ファイル: pulse.py プロジェクト: dborrero/Reed-Taylor-Couette
     info = literal_eval(comm.read())
 if info['quit'] == True:
     quit = True
     break
 elif info['reset'] == True:
     newDesiredVelocity1 = info['outerRate'] * 16 * 200
     oldDesiredVelocity1 = desiredVelocity1
     pulseAmount = info['pulseAmount']
     #stepper.setVelocityLimit(0, desiredVelocity)
     print(info)
     if newDesiredVelocity1 > oldDesiredVelocity1:
     	accel1 = int(stepper1.getAcceleration(0) / 200) # Accelerate at half speed to reduce stall chance.
     else:
     	accel1 = -int(stepper1.getAcceleration(0) / 200)
     if oldDesiredVelocity1 == 0:
        stepper1.setTargetPosition(0, stepper1.getCurrentPosition(0) + 500000)
        stepper1.setVelocityLimit(0, 2)
     for v in range(int(oldDesiredVelocity1), int(newDesiredVelocity1), accel1):
     	desiredVelocity1 = v
     	print("Ramp 1 to: %i" % desiredVelocity1)
     	sleep(0.01)
     desiredVelocity1 = newDesiredVelocity1
     if desiredVelocity1 == 0:
        for t in range(100):
             stepper1.setTargetPosition(0, stepper1.getCurrentPosition(0))
             sleep(0.01)
     stepper2.setTargetPosition(0, stepper2.getTargetPosition(0) - pulseAmount)
     while stepper2.getCurrentPosition(0) > stepper2.getTargetPosition(0) + 1:
         pass
     info['reset'] = False
     with open('comm','r+') as comm:
コード例 #17
0
ファイル: FilterWheel.py プロジェクト: UWMRO/TCC
class FilterMotor(object):
	def __init__(self):
                self.motorProtocol = None
		self.stepper = None

                # code brought in my OJF
                self.SerialPort = None
                self.SerialPortAddress = '/dev/ttyACM0'
                
		self.dict = {"velocity":None, "amp":None, "acceleration":None, "currentPos":None, "power":None, "hall":None, "commandedPos":None, "filterDelta":6860, "ID":None, "home":False, "filterPos":None, "filterCount":None, "moving":False}

	def DisplayDeviceInfo(self):
		#==> rename to getDeviceInfo
    		print("|- %8s -|- %30s -|- %10d -|- %8d -|" % (self.stepper.isAttached(), self.stepper.getDeviceName(), self.stepper.getSerialNum(), self.stepper.getDeviceVersion()))
    		print("Number of Motors: %i" % (self.stepper.getMotorCount()))
		return

	def connDev(self):
		"""connect to device and open serial connection to arduino"""
		self.stepper = Stepper()
                self.stepper.openPhidget()
                self.stepper.waitForAttach(10000)

		self.setParm(20000,5000,0.9)

                # code brought in by OJF
                self.SerialPort=serial.Serial(self.SerialPortAddress, 9600, timeout = 2)

		time.sleep(2)
		print "Stepper Driver Connected"
		self.status()
		return 1

	def disconnDev(self):
		time.sleep(1)
		self.motorPower(False)
		self.stepper.closePhidget()
		print "Stepper Driver Disconnected"
                # line added by OJF
                self.SerialPort.close()
		return

	def motorPower(self, val = False):
		self.stepper.setEngaged(0,val)
		if val == False:
			self.dict['moving'] = False
		return

	def setParm(self, acc, vel, cur):
		self.stepper.setAcceleration(0, acc) 
	    	self.stepper.setVelocityLimit(0, vel)
		self.stepper.setCurrentLimit(0,cur)
		if cur>1.4:				
			print "Cannot set current above 1.5. Current set to 0.5"
			return
		return
 
	def status(self):
		self.dict['power'] = self.stepper.getEngaged(0)
		self.dict['currentPos'] = int(self.stepper.getCurrentPosition(0))
		self.dict['acceleration'] =self.stepper.getAcceleration(0)
		self.dict['velocity'] = self.stepper.getVelocityLimit(0)
		self.dict['amp'] = self.stepper.getCurrentLimit(0)
                
                # request Hall effect sensor from Arduino
                self.SerialPort.write('s')
                self.dict['hall'] = self.SerialPort.readline().rstrip('\r\n').split(',')

		self.dict['filterPos'] = int(self.dict['currentPos'])/int(self.dict['filterDelta'])
		return self.dict

	def setPos(self, pos = None):
		self.stepper.setCurrentPosition(0, int(pos))	
		return

	def moveMotor(self, pos = None):
		self.dict['moving'] = True
		self.motorPower(True)
		self.stepper.setTargetPosition(0, int(pos))
		self.dict["commandedPos"] = pos
		return

	def nudge(self, mov):
		x = self.stepper.getCurrentPosition(0) + mov 
		self.movemotor(x)
		time.sleep(0.5)
		print "New Position:", self.stepper.getCurrentPosition(0)
		return

	def test3(self):
		x = 0
		while x != 1:
			continue
		print "hi" 

	def moveFilter(self, num = None):
		self.dict['moving'] = True
		delta = int(self.dict['filterDelta'])
		print "Moving to filter position %d" % num
		tpos = num*delta
		self.moveMotor(tpos)
		while tpos != self.dict['currentPos']:
			self.status()
			print 'moving', self.dict['currentPos']
			time.sleep(1)
		if tpos == self.dict['currentPos']:
			self.status()
			if int(self.dict['hall'][0]) == 0:
				print 'move complete', self.dict
				self.motorPower(False)
				print self.status()
				return True
			if int(self.dict['hall'][0]) == 1:
				print 'move incomplete, intiate find', self.dict
				results = self.findPos()
				self.motorStop()
				print self.status()
                                if(results == True):
                                        return True  # Adjustment successful
                                else:
                                        return False  # Adjustment failed


	def findPos(self):
                self.motorProtocol.sendData("findPos 1")  # Sends to server that it is addjusting filter position
		startPos = self.dict['currentPos']
		for x in range(0,200,25):
			newPos = x + startPos
			self.moveMotor(newPos)
			time.sleep(1)
			self.status()
			if int(self.dict['hall'][0]) == 0:
				print 'positive position found', self.dict
				self.setPos(int(startPos))
				return True  # Successfully found filter position
			
		for x in range(0,200,25):
                        newPos = x - startPos
                        self.moveMotor(newPos)
                        time.sleep(1)
                        self.status()
                        if int(self.dict['hall'][0]) == 0:
                                print 'negative position found', self.dict
				self.setPos(int(startPos))
                                return True  # Successfully found filter position
		self.motorStop()
                return False  # For when even findPos fails


        def getFilterPosition(self):
                """
                Note: Evora Gui cannot handle a dictionary that is returned in string 
                      format so instead of calling status and getting filter position
                      from there this is called instead.
                Pre:  Takes in nothing.
                Post: Returns the filter position as an integer between 0 and 5 that will
                      be parsed.
                """
                filter = int(self.dict['currentPos'])/int(self.dict['filterDelta'])
                return "getFilter " + str(filter) 

	def motorStop(self):
		self.motorPower(False)
		self.dict['moving'] = False
		return

	def home(self): 
                """
                Return 1 for True and 0 for False
                """
		crossArr = [0]
		pastHome = 0
		previousPos = 0
		try:
			print "starting home sequence"
			self.setPos(0)
			self.moveMotor(100000)
			time.sleep(.2)
			self.status()
			while int(self.dict['currentPos']) < int(self.dict['commandedPos']):
				self.status()
				if self.dict['hall'][0] == '0':
					if self.dict['hall'][0] == '0' and self.dict['hall'][1] == '0':
						if pastHome == 0: 
							pastHome = pastHome + 1
							print 'first pass', self.status()
							previousPos = self.dict['currentPos']
						if  self.dict['currentPos'] - previousPos >  3000:
							pastHome = pastHome + 1
							print 'second pass', self.status()
						if pastHome == 2:
							self.motorStop()
							self.setPos(int(100))
							time.sleep(2)
							print self.status()
							break
		
					if self.dict['currentPos'] - previousPos >  3000:
						#print self.dict['position'] - previousPos
						crossArr.append(self.dict['currentPos'] - previousPos)
						#print self.status(), crossArr
						previousPos = self.dict['currentPos']
				if self.dict['currentPos'] == self.dict['commandedPos']:
                                	raise Exception

			del crossArr[0]
			del crossArr[0]
			self.dict['filterDelta'] = int(np.mean(crossArr))
			print "homed", self.status()
			self.moveMotor(0)
			time.sleep(1)
			self.dict["home"] = True
		except:
			self.dict["home"] = False
			self.motorPower(False)
			print "==>FAILED TO HOME!!!!!"
			return "home 0"  # returning a boolean has some issues when coming out client side
		self.motorPower(False)
		self.dict['filterCount'] = 0
		print "==> HOME SUCCESSFUL"
		return "home 1"  # returning a boolean has some issues when coming out client side
コード例 #18
0
            newDesiredVelocity2 = abs(info['innerRate']) * 16 * 200
            oldDesiredVelocity2 = desiredVelocity2
            #stepper.setVelocityLimit(0, desiredVelocity)
            #stepper.setTargetPosition(0, stepper.getTargetPosition(0) + 500000)
            print(info)
            if newDesiredVelocity1 > oldDesiredVelocity1:
            	accel1 = int(stepper1.getAcceleration(0) / 200) # Accelerate at half speed to reduce stall chance.
            else:
            	accel1 = -int(stepper1.getAcceleration(0) / 200)
            if newDesiredVelocity2 > oldDesiredVelocity2:
            	accel2 = int(stepper2.getAcceleration(0) / 200)
            else:
            	accel2 = -int(stepper2.getAcceleration(0) / 200)
            if oldDesiredVelocity1 == 0 and desiredVelocity1 != 0:
                if forwards1:
		    		stepper1.setTargetPosition(0, stepper1.getCurrentPosition(0) + 500000)
                else:
		    		stepper1.setTargetPosition(0, stepper1.getCurrentPosition(0) - 500000)
				stepper1.setVelocityLimit(0, 2)
            for v in range(int(oldDesiredVelocity1), int(newDesiredVelocity1), accel1):
            	desiredVelocity1 = v
            	#print("Ramp 1 to: %i" % desiredVelocity1)
            	sleep(0.01)
            desiredVelocity1 = newDesiredVelocity1
            if desiredVelocity1 == 0:
            	for t in range(100):
                    stepper1.setTargetPosition(0, stepper1.getCurrentPosition(0))
                    sleep(0.01)
            if oldDesiredVelocity2 == 0 and desiredVelocity2 != 0:
                if forwards2:
		    		stepper2.setTargetPosition(0, stepper2.getCurrentPosition(0) + 500000)
コード例 #19
0
ファイル: Stepper-simple.py プロジェクト: larsendt/xhab-rogr
    sleep(1)
    
    print("Set the motor as engaged...")
    stepper.setEngaged(0, True)
    sleep(1)
    
    print("The motor will run until it reaches the set goal position...")
    
    stepper.setAcceleration(0, 87543)
    stepper.setVelocityLimit(0, 12400)
    stepper.setCurrentLimit(0, 1.70)
    sleep(2)
    
    print("Will now move to position 20000...")
    stepper.setTargetPosition(0, 16000)
    while stepper.getCurrentPosition(0) != 16000:
        pass
    ''' 
    sleep(2)
    
    print("Will now move back to positon 0...")
    stepper.setTargetPosition(0, 0)
    while stepper.getCurrentPosition(0) != 0:
        pass
    '''
except PhidgetException as e:
    print("Phidget Exception %i: %s" % (e.code, e.details))
    print("Exiting....")
    exit(1)

print("Press Enter to quit....")
コード例 #20
0
class FilterMotor(object):
    def __init__(self):
        """Add docs to all functions"""
        self.motorProtocol = None
        self.stepper = None
        self.fw = fw_io.FWIO()
        self.dict = {
            "velocity": None,
            "amp": None,
            "acceleration": None,
            "currentPos": None,
            "power": None,
            "hall": None,
            "commandedPos": None,
            "filterDelta": 6860,
            "ID": None,
            "home": False,
            "filterPos": None,
            "filterCount": None,
            "moving": False
        }

    def DisplayDeviceInfo(self):
        #==> rename to getDeviceInfo
        print("|- %8s -|- %30s -|- %10d -|- %8d -|" %
              (self.stepper.isAttached(), self.stepper.getDeviceName(),
               self.stepper.getSerialNum(), self.stepper.getDeviceVersion()))
        print("Number of Motors: %i" % (self.stepper.getMotorCount()))
        return

    def connDev(self):
        """connect to device and open serial connection to arduino"""
        self.stepper = Stepper()
        self.stepper.openPhidget()
        self.stepper.waitForAttach(10000)

        self.setParm(20000, 5000, 0.9)

        self.fw.openPort()
        time.sleep(2)
        print "Stepper Driver Connected"
        self.status()
        return 1

    def disconnDev(self):
        time.sleep(1)
        self.motorPower(False)
        self.stepper.closePhidget()
        print "Stepper Driver Disconnected"
        return

    def motorPower(self, val=False):
        self.stepper.setEngaged(0, val)
        if val == False:
            self.dict['moving'] = False
        return

    def setParm(self, acc, vel, cur):
        self.stepper.setAcceleration(0, acc)
        self.stepper.setVelocityLimit(0, vel)
        self.stepper.setCurrentLimit(0, cur)
        if cur > 1.4:
            print "Cannot set current above 1.5. Current set to 0.5"
            return
        return

    def status(self):
        self.dict['power'] = self.stepper.getEngaged(0)
        self.dict['currentPos'] = int(self.stepper.getCurrentPosition(0))
        self.dict['acceleration'] = self.stepper.getAcceleration(0)
        self.dict['velocity'] = self.stepper.getVelocityLimit(0)
        self.dict['amp'] = self.stepper.getCurrentLimit(0)
        self.dict['hall'] = self.fw.getStatus()
        self.dict['filterPos'] = int(self.dict['currentPos']) / int(
            self.dict['filterDelta'])
        return self.dict

    def setPos(self, pos=None):
        self.stepper.setCurrentPosition(0, int(pos))
        return

    def moveMotor(self, pos=None):
        self.dict['moving'] = True
        self.motorPower(True)
        self.stepper.setTargetPosition(0, int(pos))
        self.dict["commandedPos"] = pos
        return

    def nudge(self, mov):
        x = self.stepper.getCurrentPosition(0) + mov
        self.movemotor(x)
        time.sleep(0.5)
        print "New Position:", self.stepper.getCurrentPosition(0)
        return

    def test3(self):
        x = 0
        while x != 1:
            continue
        print "hi"

    def moveFilter(self, num=None):
        self.dict['moving'] = True
        delta = int(self.dict['filterDelta'])
        print "Moving to filter position %d" % num
        tpos = num * delta
        self.moveMotor(tpos)
        while tpos != self.dict['currentPos']:
            self.status()
            print 'moving', self.dict['currentPos']
            time.sleep(1)
        if tpos == self.dict['currentPos']:
            self.status()
            if int(self.dict['hall'][0]) == 0:
                print 'move complete', self.dict
                self.motorPower(False)
                print self.status()
                return True
            if int(self.dict['hall'][0]) == 1:
                print 'move incomplete, intiate find', self.dict
                results = self.findPos()
                self.motorStop()
                print self.status()
                if (results == True):
                    return True  # Adjustment successful
                else:
                    return False  # Adjustment failed

    def findPos(self):
        self.motorProtocol.sendData(
            "findPos 1"
        )  # Sends to server that it is addjusting filter position
        startPos = self.dict['currentPos']
        for x in range(0, 200, 25):
            newPos = x + startPos
            self.moveMotor(newPos)
            time.sleep(1)
            self.status()
            if int(self.dict['hall'][0]) == 0:
                print 'positive position found', self.dict
                self.setPos(int(startPos))
                return True  # Successfully found filter position

        for x in range(0, 200, 25):
            newPos = x - startPos
            self.moveMotor(newPos)
            time.sleep(1)
            self.status()
            if int(self.dict['hall'][0]) == 0:
                print 'negative position found', self.dict
                self.setPos(int(startPos))
                return True  # Successfully found filter position
        self.motorStop()
        return False  # For when even findPos fails

    def getFilterPosition(self):
        """
                Note: Evora Gui cannot handle a dictionary that is returned in string 
                      format so instead of calling status and getting filter position
                      from there this is called instead.
                Pre:  Takes in nothing.
                Post: Returns the filter position as an integer between 0 and 5 that will
                      be parsed.
                """
        filter = int(self.dict['currentPos']) / int(self.dict['filterDelta'])
        return "getFilter " + str(filter)

    def motorStop(self):
        self.motorPower(False)
        self.dict['moving'] = False
        return

    def home(self):
        """
                Return 1 for True and 0 for False
                """
        crossArr = [0]
        pastHome = 0
        previousPos = 0
        try:
            print "starting home sequence"
            self.setPos(0)
            self.moveMotor(100000)
            time.sleep(.2)
            self.status()
            while int(self.dict['currentPos']) < int(
                    self.dict['commandedPos']):
                self.status()
                if self.dict['hall'][0] == '0':
                    if self.dict['hall'][0] == '0' and self.dict['hall'][
                            1] == '0':
                        if pastHome == 0:
                            pastHome = pastHome + 1
                            print 'first pass', self.status()
                            previousPos = self.dict['currentPos']
                        if self.dict['currentPos'] - previousPos > 3000:
                            pastHome = pastHome + 1
                            print 'second pass', self.status()
                        if pastHome == 2:
                            self.motorStop()
                            self.setPos(int(100))
                            time.sleep(2)
                            print self.status()
                            break

                    if self.dict['currentPos'] - previousPos > 3000:
                        #print self.dict['position'] - previousPos
                        crossArr.append(self.dict['currentPos'] - previousPos)
                        #print self.status(), crossArr
                        previousPos = self.dict['currentPos']
                if self.dict['currentPos'] == self.dict['commandedPos']:
                    raise Exception

            del crossArr[0]
            del crossArr[0]
            self.dict['filterDelta'] = int(np.mean(crossArr))
            print "homed", self.status()
            self.moveMotor(0)
            time.sleep(1)
            self.dict["home"] = True
        except:
            self.dict["home"] = False
            self.motorPower(False)
            print "==>FAILED TO HOME!!!!!"
            return "home 0"  # returning a boolean has some issues when coming out client side
        self.motorPower(False)
        self.dict['filterCount'] = 0
        print "==> HOME SUCCESSFUL"
        return "home 1"  # returning a boolean has some issues when coming out client side