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 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 __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 __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 __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 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 __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 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
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)
# print("Velocity 2 adjusted to %i" % stepper2.getVelocityLimit(0))
# print("Stepper 2 mean measured velocity %i" % (sum(velocityMeasurements2) / len(velocityMeasurements2)))
# timer2 = timeDelayBetweenAdjustments
# velocityMeasurements2 = []
#Create an encoder object
try:
encoder = Encoder()
except RuntimeError as e:
print("Runtime Exception: %s" % e.details)
print("Exiting....")
exit(1)
#Create a stepper object
try:
stepper1 = Stepper()
except RuntimeError as e:
print("Runtime Exception: %s" % e.details)
print("Exiting....")
exit(1)
#Create a stepper object
try:
stepper2 = Stepper()
except RuntimeError as e:
print("Runtime Exception: %s" % e.details)
print("Exiting....")
exit(1)
#Main Program Code
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()
# while compTime > absTime:
# sleep(1)
# primaryTargetPosition = -50000
#INITIAL PRIMARY AXIS MOVEMENT
#Main Program Code
#Create a stepper object
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)
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)
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)
def stepper_init():
#Create the two steppers
try:
S1 = Stepper()
S2 = Stepper()
except RuntimeError as e:
print("Runtime Exception:%s"% e.details)
print("Exiting....")
exit(1)
#Setup the Handler
try:
setup_handeller(S1)
setup_handeller(S2)
except RuntimeError as e:
print("Runtime Exception:%s"% e.details)
print("Exiting....")
exit(1)
print("Opening Phidget Object")
#Open the phidget device
try:
S1.openPhidget(serial = Phidget_77)
S2.openPhidget(serial = Phidget_75)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
print("Waiting for attach....")
try:
S1.waitForAttach(10000)
S2.waitForAttach(10000)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
try:
S1.closePhidget()
S2.closePhidget()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
print("Exiting....")
exit(1)
print("initialization done")
return S1,S2
#Movement List - Position 0 is motor 0 - Position 1 is motor 1 ... made sense ...
MOVE=[[100,1650],[-475,1650],[-850,1650],[-1225,1650],[-1600,1650],[-100, 1250],[-475,1250],[-850,1250],\
[-1225,1250],[-1600,1250],[-100, 850],[-475,850],[-850,850],[-1225,850],[-1600,850],[-100, 450],\
[-475,450],[-850,450],[-1225,450],[-1600,450],[-100, 50],[-475,50],[-850,50],[-1225,50],[-1600,50]]
###Create Stepper and Servo Objects
try:
advancedServo = AdvancedServo()
except RuntimeError as e:
print("Runtime Exception: %s" % e.details)
print("Exiting....")
exit(1)
try:
stepper = Stepper()
except RuntimeError as e:
print("Runtime Exception: %s" % e.details)
print("Exiting....")
exit(1)
###Open Phidgets
print("Opening phidget object....")
try:
advancedServo.openPhidget()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
# Converts a number of degrees to the corresponding number of motor steps
# Will return an exact number of steps, since the motor cannot actually turn a fractional number of steps
def DegreesToSteps(degrees):
# Motor actually moves in 1/16ths of full steps
return int(round((degrees * 16)/ MOTOR_STEP_ANGLE))
# Converts a number of motor steps to the number of degrees the motor would turn
def StepsToDegrees(steps):
# Motor actually moves in 1/16ths of full steps
return ((steps * MOTOR_STEP_ANGLE) / 16)
##################################################
# General control of both motors:
# Create stepper phidget objects
pan = Stepper() # Horizontal pan
tilt = Stepper() # Vertical tilt
# Initialization
# Creates and opens stepper objects, waits for attachment, and sets default motor parameters
# Returns motor objects in a tuple (pan, tilt)
# NOTE: Will WAIT for attachment and EXIT if any exception is thrown
# NOTE 2: 50ms sleep included because the phidget takes time to process commands
def InitializeMotors():
# Open objects and wait for attachment (should be immediate since they are already plugged in)
if (PAN_ENABLED):
try:
pan.openPhidget(MOTOR_PAN_ID)
except PhidgetException, e:
print "InitializeMotors: Pan: Unable to open phidget"
print "Phidget exception: ", e.details
#Phidget specific imports
from Phidgets.PhidgetException import PhidgetErrorCodes, PhidgetException
from Phidgets.Devices.Stepper import Stepper
from kinematics import *
from stepper_motor_setup import *
import math
import numpy as np
#Define several important factors about the two stepper motors
STEPPER_1_CIRCLE = 16457
STEPPER_2_CIRCLE = 85970
h2 = 1518.0
r1 = 30.0
r2 = 35.0
try:
stepper = Stepper()
#Create a new device object
stepper_1 = Stepper()
except RuntimeError as e:
print("Runtime Exception:%s"% e.details)
print("Exiting....")
exit(1)
#Information Display Function
def DisplayDeviceInfo():
print("|------------|----------------------------------|--------------|------------|")
print("|- Attached -|- Type -|- Serial No. -|- Version -|")
print("|------------|----------------------------------|--------------|------------|")
print("|- %8s -|- %30s -|- %10d -|- %8d -|" % (stepper.isAttached(), stepper.getDeviceName(), stepper.getSerialNum(), stepper.getDeviceVersion()))
print("|------------|----------------------------------|--------------|------------|")
print("Number of Motors: %i" % (stepper.getMotorCount()))
CONTROL_SIGNAL_RESET_STEPPER = -1
CONTROL_SIGNAL_DISENGAGE_STEPPER = -2
CONTROL_SIGNAL_ENGAGE_STEPPER = -3
CONTROL_SIGNAL_CREATE_NOISE = -4
STEPPER_MAX_POSITION = 5500
MAX_IDLE_TIME_SECS = 120
STEPPER_NOISE_STEPS = 50
lastStepTimeSecs = -1
try:
stepper = Stepper()
except RuntimeError as e:
print("Runtime Exception: %s" % e.details)
print("Exiting....")
exit(1)
#Information Display Function
def DisplayDeviceInfo():
print(
"|------------|----------------------------------|--------------|------------|"
)
print(
"|- Attached -|- Type -|- Serial No. -|- Version -|"
)
print(
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)
def __init__(self):
self.stepper = Stepper()
self.stepper.openPhidget()
print('attaching stepper dev ...')
self.stepper.waitForAttach(10000)
self.DisplayDeviceInfo
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"
#Basic imports
from ctypes import *
import sys
import random
from time import sleep
#Phidget specific imports
from Phidgets.PhidgetException import PhidgetErrorCodes, PhidgetException
from Phidgets.Events.Events import AttachEventArgs, DetachEventArgs, ErrorEventArgs, InputChangeEventArgs, CurrentChangeEventArgs, StepperPositionChangeEventArgs, VelocityChangeEventArgs, OutputChangeEventArgs, SensorChangeEventArgs
from Phidgets.Devices.Stepper import Stepper
from Phidgets.Devices.InterfaceKit import InterfaceKit
from Phidgets.Phidget import PhidgetID
from Phidgets.Devices.TextLCD import TextLCD, TextLCD_ScreenSize
#Create a stepper object
try:
stepper = Stepper()
stepper2 = Stepper()
stepper3 = Stepper()
except RuntimeError as e:
print("Runtime Exception: %s" % e.details)
print("Exiting....")
exit(1)
#Create an interfacekit object
try:
interfaceKit = InterfaceKit()
except RuntimeError as e:
print("Runtime Exception: %s" % e.details)
print("Exiting....")
exit(1)
from Phidgets.Devices.Stepper import Stepper
from ..global_constants import TED_STEPPER_INDEX
controller = Stepper()
controller.openPhidget()
#Converts a number of steps into the equivalent degrees
def step2deg(steps, asBearing=False):
#If asBearing is True, this returns the bearing in degrees of the given step
#position
steps = steps % 400 if asBearing else steps
return float(steps * 0.9)
#Converts degrees to the nearest amount of steps needed
#Also returns the actual degrees that the steps represent, since it is rounded
def deg2step(degrees, asPosition=True):
#This is very important, as not only does it simplify the math, but it also
#prevents cable wrapping
degrees = degrees % 360 if asPosition else degrees
steps = int(round(degrees / 0.9))
return steps, step2deg(steps)
#Class to control a Phidgets stepper
class CameraStepper:
def __init__(self, index=TED_STEPPER_INDEX):
self.index = index
controller.setEngaged(self.index, True)
__version__ = '2.1.8'
__date__ = 'May 17 2010'
#Basic imports
#import curses
from ctypes import *
import sys
from time import sleep
#Phidget specific imports
from Phidgets.PhidgetException import PhidgetErrorCodes, PhidgetException
from Phidgets.Events.Events import AttachEventArgs, DetachEventArgs, ErrorEventArgs, InputChangeEventArgs, CurrentChangeEventArgs, StepperPositionChangeEventArgs, VelocityChangeEventArgs
from Phidgets.Devices.Stepper import Stepper
#Create a stepper object
try:
stepper = Stepper()
except RuntimeError as e:
print("Runtime Exception: %s" % e.details)
print("Exiting....")
exit(1)
#Information Display Function
def DisplayDeviceInfo():
print("|------------|----------------------------------|--------------|------------|")
print("|- Attached -|- Type -|- Serial No. -|- Version -|")
print("|------------|----------------------------------|--------------|------------|")
print("|- %8s -|- %30s -|- %10d -|- %8d -|" % (stepper.isAttached(), stepper.getDeviceName(), stepper.getSerialNum(), stepper.getDeviceVersion()))
print("|------------|----------------------------------|--------------|------------|")
print("Number of Motors: %i" % (stepper.getMotorCount()))
#Event Handler Callback Functions
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
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))
from Phidgets.PhidgetException import PhidgetErrorCodes, PhidgetException
from Phidgets.Events.Events import AttachEventArgs, DetachEventArgs, ErrorEventArgs, InputChangeEventArgs, CurrentChangeEventArgs, StepperPositionChangeEventArgs, VelocityChangeEventArgs
from Phidgets.Devices.Stepper import Stepper
from Phidgets.Phidget import PhidgetLogLevel
from kinematics import *
from stepper_motor_setup import *
import math
#Define several important factors about the two stepper motors
STEPPER_1_CIRCLE = 16457
STEPPER_2_CIRCLE = 85970
h2 = 1550.0
r1 = 30.0
r2 = 35.0
try:
stepper = Stepper()
#Create a new device object
stepper_1 = Stepper()
except RuntimeError as e:
print("Runtime Exception:%s"% e.details)
print("Exiting....")
exit(1)
#Information Display Function
def DisplayDeviceInfo():
print("|------------|----------------------------------|--------------|------------|")
print("|- Attached -|- Type -|- Serial No. -|- Version -|")
print("|------------|----------------------------------|--------------|------------|")
print("|- %8s -|- %30s -|- %10d -|- %8d -|" % (stepper.isAttached(), stepper.getDeviceName(), stepper.getSerialNum(), stepper.getDeviceVersion()))
print("|------------|----------------------------------|--------------|------------|")
print("Number of Motors: %i" % (stepper.getMotorCount()))
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
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
__author__ = 'Trevor Stanhope'
__version__ = '0.1'
__date__ = 'Nov 4 2014'
# Imports
from ctypes import *
import sys
from time import sleep
from Phidgets.PhidgetException import PhidgetErrorCodes, PhidgetException
from Phidgets.Events.Events import AttachEventArgs, DetachEventArgs, ErrorEventArgs, InputChangeEventArgs, CurrentChangeEventArgs, StepperPositionChangeEventArgs, VelocityChangeEventArgs
from Phidgets.Devices.Stepper import Stepper
# Create a stepper object
try:
stepper = Stepper()
except RuntimeError as e:
print("Runtime Exception: %s" % e.details)
print("Exiting....")
exit(1)
# Information Display Function
def DisplayDeviceInfo():
print("%8s, %30s, %10d, %8d" %
(stepper.isAttached(), stepper.getDeviceName(),
stepper.getSerialNum(), stepper.getDeviceVersion()))
print("Number of Motors: %i" % (stepper.getMotorCount()))
# Event Handler Callback Functions
def __init__(self):
self.stepper = Stepper()
self.stepper.openPhidget()
print('attaching stepper dev ...')
self.stepper.waitForAttach(10000)
self.DisplayDeviceInfo()
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"
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))
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)
#Import severl libraries
from ctypes import *
import sys
from time import sleep
#Phidget specific imports
from Phidgets.PhidgetException import PhidgetErrorCodes, PhidgetException
from Phidgets.Events.Events import AttachEventArgs, DetachEventArgs, ErrorEventArgs, InputChangeEventArgs, CurrentChangeEventArgs, StepperPositionChangeEventArgs, VelocityChangeEventArgs
from Phidgets.Devices.Stepper import Stepper
from Phidgets.Phidget import PhidgetLogLevel
from kinematics import *
#Define several important factors about the two stepper motors
STEPPER_1_CIRCLE = 16457
STEPPER_2_CIRCLE = 85970
try:
stepper = Stepper()
#Create a new device object
stepper_1 = Stepper()
except RuntimeError as e:
print("Runtime Exception:%s"% e.details)
print("Exiting....")
exit(1)
#Information Display Function
def DisplayDeviceInfo():
print("|------------|----------------------------------|--------------|------------|")
print("|- Attached -|- Type -|- Serial No. -|- Version -|")
print("|------------|----------------------------------|--------------|------------|")
print("|- %8s -|- %30s -|- %10d -|- %8d -|" % (stepper.isAttached(), stepper.getDeviceName(), stepper.getSerialNum(), stepper.getDeviceVersion()))
print("|------------|----------------------------------|--------------|------------|")
print("Number of Motors: %i" % (stepper.getMotorCount()))
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
from Phidgets.Devices.Stepper import Stepper # Global constants MOTOR_PAN_ID = 346596 MOTOR_TILT_ID = 346561 MOTOR_ATTACH_WAIT_TIME_MS = 2000 MOTOR_ACCEL_LIMIT = 100000 MOTOR_VELOCITY_LIMIT = 80000 MOTOR_CURRENT_LIMIT = 1.0 MOTOR_360_DEGREES = 318424 # Initialization # Create phidget objects pan = Stepper() # Horizontal pan tilt = Stepper() # Vertical tilt # Open objects and wait for attachment (should be immediate since they are already plugged in) pan.openPhidget(MOTOR_PAN_ID) pan.waitForAttach(MOTOR_ATTACH_WAIT_TIME_MS) tilt.openPhidget(MOTOR_TILT_ID) tilt.waitForAttach(MOTOR_ATTACH_WAIT_TIME_MS) # Initialize motor parameters pan.setAcceleration(0, MOTOR_ACCEL_LIMIT) pan.setVelocityLimit(0, MOTOR_VELOCITY_LIMIT) pan.setCurrentLimit(0, MOTOR_CURRENT_LIMIT) tilt.setAcceleration(0, MOTOR_ACCEL_LIMIT)
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)
import BaseHTTPServer
from time import sleep
import sys
import serial
from Phidgets.Devices.Stepper import Stepper
connUSB = '/dev/ttyACM0'
if len(sys.argv) > 1:
connUSB = sys.argv[1]
connArduino = serial.Serial(connUSB, 9600)
# connArduino = serial.Serial('/dev/tty.usbmodemc041', 9600)
connPhidget = Stepper()
class HTTPHandler(BaseHTTPServer.BaseHTTPRequestHandler):
def do_GET(self):
command = self.path[1:].split('/')
if command[0][0] == 'd' or command[0][0] == 't':
connArduino.write(command[0][0] + command[1] + '\n')
print 'MOVE: %s %s units' % (command[0][0], command[1])
elif command[0][0] == 'r':
print 'MOVE: Rotating %s degrees' % command[1]
connPhidget.openPhidget()
connPhidget.waitForAttach(10000)
connPhidget.setCurrentPosition(0, 0)
# sleep(1)
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))
__author__ = 'Adam Stelmack'
__version__ = '2.1.8'
__date__ = 'May 17 2010'
#Basic imports
from ctypes import *
import sys
from time import sleep
#Phidget specific imports
from Phidgets.PhidgetException import PhidgetErrorCodes, PhidgetException
from Phidgets.Events.Events import AttachEventArgs, DetachEventArgs, ErrorEventArgs, InputChangeEventArgs, CurrentChangeEventArgs, StepperPositionChangeEventArgs, VelocityChangeEventArgs
from Phidgets.Devices.Stepper import Stepper
#Create a stepper object
try:
stepper = Stepper()
except RuntimeError as e:
print("Runtime Exception: %s" % e.details)
print("Exiting....")
exit(1)
#Information Display Function
def DisplayDeviceInfo():
print("|------------|----------------------------------|--------------|------------|")
print("|- Attached -|- Type -|- Serial No. -|- Version -|")
print("|------------|----------------------------------|--------------|------------|")
print("|- %8s -|- %30s -|- %10d -|- %8d -|" % (stepper.isAttached(), stepper.getDeviceName(), stepper.getSerialNum(), stepper.getDeviceVersion()))
print("|------------|----------------------------------|--------------|------------|")
print("Number of Motors: %i" % (stepper.getMotorCount()))
#Event Handler Callback Functions
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
