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