def __init__(self): # Create an advancedServo object try: self.__advancedServo = AdvancedServo() except RuntimeError as e: print('[ERROR] [ServoControl] Runtime Exception: %s' % e.details) return 1 # set up our event handlers try: # logging example, uncomment to generate a log file #self.__advancedServo.enableLogging(PhidgetLogLevel.PHIDGET_LOG_VERBOSE, 'phidgetlog.log') self.__advancedServo.setOnAttachHandler(self.__attached) self.__advancedServo.setOnDetachHandler(self.__detached) self.__advancedServo.setOnErrorhandler(self.__error) except PhidgetException as e: print('[ERROR] Phidget Exception %i: %s' % (e.code, e.details)) return 1 print('[INFO] [ServoControl] Opening phidget object....') try: self.__advancedServo.openPhidget() except PhidgetException as e: print('[ERROR] Phidget Exception %i: %s' % (e.code, e.details)) return 1 print('[INFO] [ServoControl] Waiting for attach....') try: self.__advancedServo.waitForAttach(10000) except PhidgetException as e: print('[ERROR] Phidget Exception %i: %s' % (e.code, e.details)) try: self.__advancedServo.closePhidget() except PhidgetException as e: print('[ERROR] Phidget Exception %i: %s' % (e.code, e.details)) return 1 return 1 else: # self.__DisplayDeviceInfo() pass
def __openSer(self): try: self._advancedServo = AdvancedServo() except RuntimeError as e: print("Servo - Runtime Exception: %s" % e.details) return False try: self._advancedServo.setOnAttachHandler(self.__onAttachedSer) self._advancedServo.setOnDetachHandler(self.__onDetachedSer) self._advancedServo.setOnErrorhandler(self.__onErrorSer) except PhidgetException as e: print("Servo - Phidget Exception %i: %s" % (e.code, e.details)) return False try: self._advancedServo.openPhidget() except PhidgetException as e: print("Servo - Phidget Exception %i: %s" % (e.code, e.details)) return False self._openSer = True return True
def __init__(self): try: self.advancedServo = AdvancedServo() self.advancedServo.openPhidget() print("Waiting to be attached") self.advancedServo.waitForAttach(10000) print("Initialize Servo") self.advancedServo.setVelocityLimit(0, 180.00) self.advancedServo.setServoType(0, ServoTypes.PHIDGET_SERVO_HITEC_HS322HD) print("Engage Servo") self.advancedServo.setEngaged(0, True) print("Ready") except RuntimeError as e: print("Runtime Exception: %s" % e.details) print("Exiting....") exit(1)
def __openSer(self): try: self._advancedServo = AdvancedServo() except RuntimeError as e: print("Servo - Runtime Exception: %s" % e.details) return False try: self._advancedServo.setOnAttachHandler(self.__onAttachedSer) self._advancedServo.setOnDetachHandler(self.__onDetachedSer) self._advancedServo.setOnErrorhandler(self.__onErrorSer) except PhidgetException as e: print("Servo - Phidget Exception %i: %s" % (e.code, e.details)) return False try: self._advancedServo.openPhidget() except PhidgetException as e: print("Servo - Phidget Exception %i: %s" % (e.code, e.details)) return False self._openSer=True return True
class ServoControl: __ready = False def __init__(self): # Create an advancedServo object try: self.__advancedServo = AdvancedServo() except RuntimeError as e: print('[ERROR] [ServoControl] Runtime Exception: %s' % e.details) return 1 # set up our event handlers try: # logging example, uncomment to generate a log file #self.__advancedServo.enableLogging(PhidgetLogLevel.PHIDGET_LOG_VERBOSE, 'phidgetlog.log') self.__advancedServo.setOnAttachHandler(self.__attached) self.__advancedServo.setOnDetachHandler(self.__detached) self.__advancedServo.setOnErrorhandler(self.__error) except PhidgetException as e: print('[ERROR] Phidget Exception %i: %s' % (e.code, e.details)) return 1 print('[INFO] [ServoControl] Opening phidget object....') try: self.__advancedServo.openPhidget() except PhidgetException as e: print('[ERROR] Phidget Exception %i: %s' % (e.code, e.details)) return 1 print('[INFO] [ServoControl] Waiting for attach....') try: self.__advancedServo.waitForAttach(10000) except PhidgetException as e: print('[ERROR] Phidget Exception %i: %s' % (e.code, e.details)) try: self.__advancedServo.closePhidget() except PhidgetException as e: print('[ERROR] Phidget Exception %i: %s' % (e.code, e.details)) return 1 return 1 else: # self.__DisplayDeviceInfo() pass def engage(self): if self.__ready: try: print('[INFO] [ServoControl] Engaging servo...') self.__advancedServo.setEngaged(0, True) except PhidgetException as e: print('[ERROR] Phidget Exception %i: %s' % (e.code, e.details)) def disengage(self): if self.__ready: try: print('[INFO] [ServoControl] Disengaging servo...') self.__advancedServo.setEngaged(0, False) self.__ready = False except PhidgetException as e: print('[ERROR] Phidget Exception %i: %s' % (e.code, e.details)) def setPosition(self, position): if self.__ready: try: position = np.clip(position, self.__advancedServo.getPositionMin(0), self.__advancedServo.getPositionMax(0)) print('[INFO] [ServoControl] Moving servo to {}'.format( position)) self.__advancedServo.setPosition(0, position) except PhidgetException as e: print('[ERROR] Phidget Exception %i: %s' % (e.code, e.details)) def destroy(self): print('[INFO] [ServoControl] Closing advanced servo phidget...') try: self.disengage() self.__advancedServo.closePhidget() except PhidgetException as e: print('[ERROR] Phidget Exception %i: %s' % (e.code, e.details)) return 1 def __displayDeviceInfo(self): """ Information Display Function """ print( '|------------|----------------------------------|--------------|------------|' ) print( '|- Attached -|- Type -|- Serial No. -|- Version -|' ) print( '|------------|----------------------------------|--------------|------------|' ) print('|- %8s -|- %30s -|- %10d -|- %8d -|' % (self.__advancedServo.isAttached(), self.__advancedServo.getDeviceName(), self.__advancedServo.getSerialNum(), self.__advancedServo.getDeviceVersion())) print( '|------------|----------------------------------|--------------|------------|' ) print('Number of motors: %i' % (self.__advancedServo.getMotorCount())) def __attached(self, e): """ Event Handler Callback Functions """ attached = e.device print('[INFO] [ServoControl] Servo %i Attached!' % (attached.getSerialNum())) try: self.__advancedServo.setServoType( 0, ServoTypes.PHIDGET_SERVO_HITEC_HS322HD) self.__advancedServo.setAcceleration( 0, self.__advancedServo.getAccelerationMax(0)) self.__advancedServo.setVelocityLimit( 0, self.__advancedServo.getVelocityMax(0)) self.__ready = True except PhidgetException as e: print('[ERROR] Phidget Exception %i: %s' % (e.code, e.details)) def __detached(self, e): detached = e.device print('[INFO] [ServoControl] Servo %i Detached!' % (detached.getSerialNum())) self.__ready = False def __error(self, e): try: source = e.device print('[ERROR] Phidget Error %i: %s' % (source.getSerialNum(), e.eCode, e.description)) except PhidgetException as e: print('[ERROR] Phidget Exception %i: %s' % (e.code, e.details))
from Phidgets.Devices.AdvancedServo import AdvancedServo from Phidgets.Devices.Servo import ServoTypes # get servo index argument servo_index = 0 if len(sys.argv) > 1 and int(sys.argv[1]) in range(8) : servo_index = int(sys.argv[1]); print("starting servo index is %s\n" % str(servo_index)) # create advancedServo object try: advancedServo = AdvancedServo() except RuntimeError as e: print("Runtime Exception: %s" % e.details) print("Exiting....") exit(1) # stack to keep current values in currentList = [0,0,0,0,0,0,0,0] velocityList = [0,0,0,0,0,0,0,0] # display information
if e.index == 0: updateSliderInfo(e.value) def interfaceKitOutputChanged(e): source = e.device print("InterfaceKit %i: Output %i: %s" % (source.getSerialNum(), e.index, e.state)) #Create and Initialize AdvancedServo object_______________________ print("Initialzing Servo...") try: advancedServo = AdvancedServo() except RuntimeError as e: print("Runtime Exception: %s" % e.details) print("Exiting....") exit(1) try: advancedServo.openPhidget() advancedServo.waitForAttach(10000) advancedServo.setEngaged(0, True) except PhidgetException as e: print("Phidget Exception %i: %s" % (e.code, e.details)) try: advancedServo.closePhidget() except PhidgetException as e: print("Phidget Exception %i: %s" % (e.code, e.details))
from Phidgets.Devices.AdvancedServo import AdvancedServo from Phidgets.Devices.Servo import ServoTypes # get servo index argument servo_index = 0 if len(sys.argv) > 1 and int(sys.argv[1]) in range(8): servo_index = int(sys.argv[1]) print("starting servo index is %s\n" % str(servo_index)) # create advancedServo object try: advancedServo = AdvancedServo() except RuntimeError as e: print("Runtime Exception: %s" % e.details) print("Exiting....") exit(1) # stack to keep current values in currentList = [0, 0, 0, 0, 0, 0, 0, 0] velocityList = [0, 0, 0, 0, 0, 0, 0, 0] # display information def DisplayDeviceInfo(): print(
#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, CurrentChangeEventArgs, PositionChangeEventArgs, VelocityChangeEventArgs from Phidgets.Devices.AdvancedServo import AdvancedServo from Phidgets.Devices.Servo import ServoTypes import os #Create an advancedServo object try: advancedServo = AdvancedServo() except RuntimeError as e: print("Runtime Exception: %s" % e.details) print("Exiting....") exit(1) #stack to keep current values in currentList = [0,0,0,0,0,0,0,0] velocityList = [0,0,0,0,0,0,0,0] #Information Display Function def DisplayDeviceInfo(): print("|------------|----------------------------------|--------------|------------|") print("|- Attached -|- Type -|- Serial No. -|- Version -|") print("|------------|----------------------------------|--------------|------------|") print("|- %8s -|- %30s -|- %10d -|- %8d -|" % (advancedServo.isAttached(), advancedServo.getDeviceName(), advancedServo.getSerialNum(), advancedServo.getDeviceVersion()))
#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.Devices.AdvancedServo import AdvancedServo from Phidgets.Devices.Servo import ServoTypes #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....")
print 'You pressed Ctrl+C! Closing Phidgets.' try: servo.closePhidget() except PhidgetException, e: PhidgetError(e) print("Done.") sys.exit(0) def PhidgetError(e): print("Phidget Exception %i: %s, Exiting." % (e.code, e.details)) exit(1) #Create an servo object try: servo = AdvancedServo() except RuntimeError, e: PhidgetError(e) def DisplayDeviceInfo(): print("Servos Attached: %8s | Device Name: %30s" % (servo.isAttached(), servo.getDeviceName())) #Event Handler Callback Functions def ServoAttached(e): print("Servo %i Attached!" % (e.device.getSerialNum())) def ServoDetached(e): print("Servo %i Detached!" % (e.device.getSerialNum())) def ServoError(e):
from Phidgets.Devices.AdvancedServo import AdvancedServo from Phidgets.PhidgetException import PhidgetErrorCodes, PhidgetException from Phidgets.Events.Events import AttachEventArgs, DetachEventArgs, ErrorEventArgs, CurrentChangeEventArgs, PositionChangeEventArgs, VelocityChangeEventArgs from Phidgets.Devices.AdvancedServo import AdvancedServo from Phidgets.Devices.Servo import ServoTypes import time import sys import socket ###################### # globals stop = False # Boards servos = [AdvancedServo(), AdvancedServo()] # two boards for the cube serials = [392856, 392822] # Boards IDs actuators = [ 8, 4 ] #8 motors on the first board (top and vertical edges), and 4 motors for the other board # a socket for listening UDP messages and then update actuators address = ('localhost', 6006) sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) sock.bind(address) ###################### # funcs def engage(): """Engage the 8 actuators and set position to 0"""
def AttachAdvancedServo(databasepath, serialNumber): def onAttachHandler(event): logString = "AdvancedServo Attached " + str( event.device.getSerialNum()) DisplayAttachedDeviceInfo(event.device) def onDetachHandler(event): logString = "AdvancedServo Detached " + str( event.device.getSerialNum()) DisplayDetachedDeviceInfo(event.device) event.device.closePhidget() def onErrorHandler(event): logString = "AdvancedServo Error " + str( event.device.getSerialNum()) + ", Error: " + event.description print(logString) DisplayErrorDeviceInfo(event.device) def onServerConnectHandler(event): logString = "AdvancedServo Server Connect " + str( event.device.getSerialNum()) def onServerDisconnectHandler(event): logString = "AdvancedServo Server Disconnect " + str( event.device.getSerialNum()) def currentChangeHandler(event): logString = "AdvancedServo Current Changed" try: conn = sqlite3.connect(databasepath) conn.execute( "INSERT INTO ADVANCEDSERVO_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 positionChangeHandler(event): logString = "AdvancedServo Position Changed" #print(logString) try: conn = sqlite3.connect(databasepath) conn.execute( "INSERT INTO ADVANCEDSERVO_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 = "AdvancedServo Velocity Changed" #print(logString) try: conn = sqlite3.connect(databasepath) conn.execute( "INSERT INTO ADVANCEDSERVO_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 = AdvancedServo() p.setOnAttachHandler(onAttachHandler) p.setOnDetachHandler(onDetachHandler) p.setOnErrorhandler(onErrorHandler) p.setOnServerConnectHandler(onServerConnectHandler) p.setOnServerDisconnectHandler(onServerDisconnectHandler) p.setOnCurrentChangeHandler(currentChangeHandler) 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 threading # Multi threading from Phidgets.Devices.MotorControl import MotorControl # Motor control from Phidgets.Devices.AdvancedServo import AdvancedServo # Servo control from Phidgets.Devices.Servo import ServoTypes # Servo type setting import pygame # General Pygame imports from pygame import init as startPygame # Initialize imported Pygame modules from pygame import joystick, locals # Logitech attack 3 joystick support import tornado.web # Tornado web framework import tornado.ioloop # I/O event loop for non-blocking sockets import tornado.websocket # Bi directional messages from server (this) to client (webpage) # Motors and servo control boards m1 = MotorControl() m2 = MotorControl() m3 = MotorControl() s1 = AdvancedServo() # Websocket list for storing values wss = [] # Set motor acceleration def setAllMotorsAcceleration(v): m1.setAcceleration(0, v) m1.setAcceleration(1, v) m2.setAcceleration(0, v) m2.setAcceleration(1, v) m3.setAcceleration(0, v) # Tornado website application settings
class Controller(): """Implements Phidget Controller""" def __init__(self): try: self.advancedServo = AdvancedServo() self.advancedServo.openPhidget() print("Waiting to be attached") self.advancedServo.waitForAttach(10000) print("Initialize Servo") self.advancedServo.setVelocityLimit(0, 180.00) self.advancedServo.setServoType(0, ServoTypes.PHIDGET_SERVO_HITEC_HS322HD) print("Engage Servo") self.advancedServo.setEngaged(0, True) print("Ready") except RuntimeError as e: print("Runtime Exception: %s" % e.details) print("Exiting....") exit(1) def windMax(self): self.advancedServo.setPosition(0, self.advancedServo.getPositionMax(0)) def windMin(self): self.advancedServo.setPosition(0, self.advancedServo.getPositionMin(0)) sleep(3) def disengage(self): self.advancedServo.setEngaged(0, False) sleep(3) def close(self): self.advancedServo.closePhidget()
servos = nServos outputs = nOutputs return True from Phidgets.PhidgetException import * from Phidgets.Events.Events import * from Phidgets.Manager import Manager from Phidgets.Phidget import PhidgetLogLevel from Phidgets.Devices.MotorControl import MotorControl motorC = MotorControl() from Phidgets.Devices.AdvancedServo import AdvancedServo servoC = AdvancedServo() from Phidgets.Devices.InterfaceKit import InterfaceKit interC = InterfaceKit() def motorInpChange(e): inp = e.state print('[M] Motor Input: ' + str(inp)) def motorCurChange(e): cur = e.current print('[M] Motor Cur: ' + str(cur))
import os import numpy as np import visa import matplotlib.pyplot as plt from ctypes import * import sys #Phidget specific imports from Phidgets.PhidgetException import PhidgetErrorCodes, PhidgetException from Phidgets.Events.Events import AttachEventArgs, DetachEventArgs, ErrorEventArgs, CurrentChangeEventArgs, PositionChangeEventArgs, VelocityChangeEventArgs from Phidgets.Devices.AdvancedServo import AdvancedServo from Phidgets.Devices.Servo import ServoTypes """All the phidget stuff...""" try: advancedServo = AdvancedServo() except RuntimeError as e: print("Runtime Exception: %s" % e.details) print("Exiting....") exit(1) def DisplayDeviceInfo(): print "shutter attached" def Attached(e): attached = e.device print("Servo %i Attached!" % (attached.getSerialNum())) def Detached(e): detached = e.device print("Servo %i Detached!" % (detached.getSerialNum()))
from Phidgets.Events.Events import AttachEventArgs, DetachEventArgs, ErrorEventArgs, CurrentChangeEventArgs, PositionChangeEventArgs, VelocityChangeEventArgs from Phidgets.Devices.AdvancedServo import AdvancedServo from Phidgets.Devices.Servo import ServoTypes from multiprocessing import Process, Pipe from Phidgets.Phidget import Phidget from Phidgets.Events.Events import AccelerationChangeEventArgs, AttachEventArgs, DetachEventArgs, ErrorEventArgs from Phidgets.Devices.Accelerometer import Accelerometer #--------------------------------------------------------------------------------------------------------------------------------------------------------------- a_conn, m1_conn = Pipe() #--------------------------------------------------------------------------------------------------------------------------------------------------------------- #--------------------------------------------------------------------------------------------------------------------------------------------------------------- try: advancedServo = AdvancedServo() print ("M1 attached-----------------------------------------------------------------") except: print("Error!!!!!!!!!!!") def M1_Attached(e): attached = e.device print("Servo %i Attached!" % (attached.getSerialNum())) def M1_Detached(e): detached = e.device print("Servo %i Detached!" % (detached.getSerialNum()))
from time import sleep, clock from math import atan, atan2, sqrt, pi from msvcrt import getch, kbhit #Librerias de Phidgets from Phidgets.Phidget import Phidget from Phidgets.PhidgetException import PhidgetErrorCodes, PhidgetException from Phidgets.Devices.Spatial import Spatial, SpatialEventData, TimeSpan from Phidgets.Events.Events import SpatialDataEventArgs, AttachEventArgs, DetachEventArgs, ErrorEventArgs, CurrentChangeEventArgs, PositionChangeEventArgs, VelocityChangeEventArgs from Phidgets.Devices.AdvancedServo import AdvancedServo from Phidgets.Devices.Servo import ServoTypes from Phidgets.Phidget import PhidgetLogLevel #Crear un objeto AdvancedServo try: advancedServo = AdvancedServo() except RuntimeError as e: print("Runtime Exception: %s" % e.details) print("Exiting....") exit(1) #Crear un objeto Spatial try: spatial = Spatial() timeSpan = TimeSpan(0, 0) except RuntimeError as e: print("Runtime Exception: %s" % e.details) print("Exiting....") exit(1) #Valores de los mototres
class CameraStation(object): def __init__(self, serial_number, yservo, zservo): self.controller = AdvancedServo() self.serial_number = serial_number self.yservo = yservo self.zservo = zservo def initServo(self, index, vel): self.controller.setServoType(index, ServoTypes.PHIDGET_SERVO_HITEC_HS322HD) self.controller.setEngaged(index, False) maxAcceleration = self.controller.getAccelerationMax(index) maxVelocity = self.controller.getVelocityMax(index) self.controller.setAcceleration(index, maxAcceleration * 0.5) self.controller.setVelocityLimit(index, maxVelocity * vel / 100) self.controller.setEngaged(index, True) self.controller.setPosition(index, 90) sleep(0.5) print "Servo {} initialized.".format(index) def init(self, yvel, zvel): # Opening servo controller self.controller.openPhidget(self.serial_number) self.controller.waitForAttach(HARDWARE_CONNECTION_TIMEOUT) self.initServo(self.yservo, yvel) self.initServo(self.zservo, zvel) def setPosition(self, yangle, zangle): # Set new position for each servo self.controller.setPosition(self.yservo, yangle) self.controller.setPosition(self.zservo, zangle) def setMoveToPosition(self, yangle, zangle): # Calculate movement time for both servos dy = abs(yangle - self.controller.getPosition(self.yservo)) dz = abs(zangle - self.controller.getPosition(self.zservo)) ty = dy / self.controller.getVelocityLimit(self.yservo) tz = dz / self.controller.getVelocityLimit(self.zservo) # Set new position for each servo self.controller.setPosition(self.yservo, yangle) self.controller.setPosition(self.zservo, zangle) # Wait until motors sets to desire positions sleep(max(ty, tz)) def start(self): self.controller.setEngaged(self.yservo, True) self.controller.setEngaged(self.zservo, True) def stop(self): self.controller.setEngaged(self.yservo, False) self.controller.setEngaged(self.zservo, False) def __del__(self): self.stop() self.controller.closePhidget()
import numpy as np import hapticsstb from Phidgets.PhidgetException import PhidgetErrorCodes, PhidgetException from Phidgets.Devices.AdvancedServo import AdvancedServo def Error(e): try: source = e.device print("Phidget Error %i: %s" % (source.getSerialNum(), e.eCode, e.description)) except PhidgetException as e: print("Phidget Exception %i: %s" % (e.code, e.details)) # Setup Phidget servo = AdvancedServo() servo.setOnErrorhandler(Error) # Open Phidget servo.openPhidget() servo.waitForAttach(500) # Set Phidget servo parameters try: motor = 0 servo.setEngaged(0, True) servo_min = servo.getPositionMin(motor) + 5 servo_max = servo.getPositionMax(motor) - 5 servo_mid = (servo_max - servo_min)/2 servo.setAcceleration(motor, 500) # I just picked these to be smooth, feel free to change servo.setVelocityLimit(motor, 2000)
def __init__(self, serial_number, yservo, zservo): self.controller = AdvancedServo() self.serial_number = serial_number self.yservo = yservo self.zservo = zservo
def AttachAdvancedServo(databasepath, serialNumber): def onAttachHandler(event): logString = "AdvancedServo Attached " + str(event.device.getSerialNum()) DisplayAttachedDeviceInfo(event.device) def onDetachHandler(event): logString = "AdvancedServo Detached " + str(event.device.getSerialNum()) DisplayDetachedDeviceInfo(event.device) event.device.closePhidget() def onErrorHandler(event): logString = "AdvancedServo Error " + str(event.device.getSerialNum()) + ", Error: " + event.description print(logString) DisplayErrorDeviceInfo(event.device) def onServerConnectHandler(event): logString = "AdvancedServo Server Connect " + str(event.device.getSerialNum()) def onServerDisconnectHandler(event): logString = "AdvancedServo Server Disconnect " + str(event.device.getSerialNum()) def currentChangeHandler(event): logString = "AdvancedServo Current Changed" try: conn = sqlite3.connect(databasepath) conn.execute("INSERT INTO ADVANCEDSERVO_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 positionChangeHandler(event): logString = "AdvancedServo Position Changed" #print(logString) try: conn = sqlite3.connect(databasepath) conn.execute("INSERT INTO ADVANCEDSERVO_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 = "AdvancedServo Velocity Changed" #print(logString) try: conn = sqlite3.connect(databasepath) conn.execute("INSERT INTO ADVANCEDSERVO_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 = AdvancedServo() p.setOnAttachHandler(onAttachHandler) p.setOnDetachHandler(onDetachHandler) p.setOnErrorhandler(onErrorHandler) p.setOnServerConnectHandler(onServerConnectHandler) p.setOnServerDisconnectHandler(onServerDisconnectHandler) p.setOnCurrentChangeHandler(currentChangeHandler) 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)
class IOTools: def __init__(self, onRobot): self.onRobot = onRobot # Camera self._openCam = False # Motor Control self._snMot = -1 self._openMot = False self._attachedMot = False self._cur = [0, 0] # Servo self._snSer = -1 self._openSer = False self._attachedSer = False self._limits = [0, 0] # IF Kit self._snIF = -1 self._openIF = False self._attachedIF = False self._inp = [0, 0, 0, 0, 0, 0, 0, 0] self._sen = [0, 0, 0, 0, 0, 0, 0, 0] # LEDs self._fistTime = True self._status = [0, 0, 0] self._mod = [8, 1, 1] self._rep = [-1, 5, 6] self._val = [True, False, False] self._ofs = [0, 0, 0] self._updaterThread = threading.Thread(target=self.__updateLED) self._stop = False self._updaterThread.setDaemon(True) self._updaterThread.start() def destroy(self): # LEDs self._stop = True if self._attachedIF: self._interfaceKit.setOutputState(0, 0) self._interfaceKit.setOutputState(1, 0) self._interfaceKit.setOutputState(2, 0) # Servo self.servoDisengage() # Camera self._openCam = False if self._openCam: self._cap.release() def open(self): self.__openIF() self.__openMot() self.__openSer() self.__openCam() ###################### Camera ###################### def __openCam(self): if not os.path.exists('/dev/video0'): return False self._cap = cv2.VideoCapture() if not self._cap.open(-1): return False self._openCam = True def cameraGrab(self): if self._openCam: return self._cap.grab() else: return False def cameraRead(self): if self._openCam: (ret, img) = self._cap.retrieve() return img else: return False def cameraSetResolution(self, sz): if self._openCam: sz = sz.lower() if sz == 'low': self._cap.set(3, 160) self._cap.set(4, 120) if sz == 'medium': self._cap.set(3, 640) self._cap.set(4, 480) if sz == 'high': self._cap.set(3, 800) self._cap.set(4, 600) if sz == 'full': self._cap.set(3, 1280) self._cap.set(4, 720) def imshow(self, wnd, img): if not self.onRobot: if img.__class__ != numpy.ndarray: print "imshow - invalid image" return False else: cv2.imshow(wnd, img) cv2.waitKey(5) ####################### Servo ###################### def __openSer(self): try: self._advancedServo = AdvancedServo() except RuntimeError as e: print("Servo - Runtime Exception: %s" % e.details) return False try: self._advancedServo.setOnAttachHandler(self.__onAttachedSer) self._advancedServo.setOnDetachHandler(self.__onDetachedSer) self._advancedServo.setOnErrorhandler(self.__onErrorSer) except PhidgetException as e: print("Servo - Phidget Exception %i: %s" % (e.code, e.details)) return False try: self._advancedServo.openPhidget() except PhidgetException as e: print("Servo - Phidget Exception %i: %s" % (e.code, e.details)) return False self._openSer = True return True def __onAttachedSer(self, e): self._snSer = e.device.getSerialNum() self._advancedServo.setServoType( 0, ServoTypes.PHIDGET_SERVO_HITEC_HS322HD) self._advancedServo.setAcceleration( 0, self._advancedServo.getAccelerationMax(0)) self._advancedServo.setVelocityLimit( 0, self._advancedServo.getVelocityMax(0)) self._limits[0] = self._advancedServo.getPositionMin(0) self._limits[1] = self._advancedServo.getPositionMax(0) print("Servo %i Attached! Range: %f - %f" % (self._snSer, self._limits[0], self._limits[1])) self._attachedSer = True def __onDetachedSer(self, e): print("Servo %i Detached!" % (self._snSer)) self._snSer = -1 self._attachedSer = False def __onErrorSer(self, e): try: source = e.device print("Servo %i: Phidget Error %i: %s" % (source.getSerialNum(), e.eCode, e.description)) except PhidgetException as e: print("Servo - Phidget Exception %i: %s" % (e.code, e.details)) def __closeSer(self): if self._openSer == True: self.servoDisengage() self._advancedServo.closePhidget() def servoEngage(self): if self._attachedSer == True: self._advancedServo.setEngaged(0, True) def servoDisengage(self): if self._attachedSer == True: self._advancedServo.setEngaged(0, False) def servoSet(self, pos): if self._attachedSer == True: self._advancedServo.setPosition( 0, min(max(pos, self._limits[0]), self._limits[1])) ############### Motor Control ###################### def __openMot(self): try: self._motorControl = MotorControl() except RuntimeError as e: print("Motor Control - Runtime Exception: %s" % e.details) return False try: self._motorControl.setOnAttachHandler(self.__onAttachedMot) self._motorControl.setOnDetachHandler(self.__onDetachedMot) self._motorControl.setOnErrorhandler(self.__onErrorMot) self._motorControl.setOnCurrentChangeHandler( self.__onCurrentChangedMot) except PhidgetException as e: print("Motor Control - Phidget Exception %i: %s" % (e.code, e.details)) return False try: self._motorControl.openPhidget() except PhidgetException as e: print("Motor Control - Phidget Exception %i: %s" % (e.code, e.details)) return False self._openMot = True return True def __onAttachedMot(self, e): self._snMot = e.device.getSerialNum() print("Motor Control %i Attached!" % (self._snMot)) self._attachedMot = True def __onDetachedMot(self, e): print("Motor Control %i Detached!" % (self._snMot)) self._snMot = -1 self._attachedMot = False def __onErrorMot(self, e): try: source = e.device print("Motor Control %i: Phidget Error %i: %s" % (source.getSerialNum(), e.eCode, e.description)) except PhidgetException as e: print("Motor Control - Phidget Exception %i: %s" % (e.code, e.details)) def __onCurrentChangedMot(self, e): self._cur[e.index] = e.current def __closeMot(self): if self._openMot == True: self._motorControl.closePhidget() def setMotors(self, speed1=0.0, speed2=0.0, acceleration1=100.0, acceleration2=100.0): if self._openMot == True: self._motorControl.setAcceleration(0, acceleration1) self._motorControl.setVelocity(0, speed1) self._motorControl.setAcceleration(1, acceleration2) self._motorControl.setVelocity(1, speed2) ############### Interface Kit ###################### def __closeIF(self): if self._openIF == True: self._interfaceKit.closePhidget() def __openIF(self): try: self._interfaceKit = InterfaceKit() except RuntimeError as e: print("IF Kit - Runtime Exception: %s" % e.details) return False try: self._interfaceKit.setOnAttachHandler(self.__onAttachedIF) self._interfaceKit.setOnDetachHandler(self.__onDetachedIF) self._interfaceKit.setOnErrorhandler(self.__onErrorIF) self._interfaceKit.setOnInputChangeHandler(self.__onInputChangedIF) self._interfaceKit.setOnSensorChangeHandler( self.__onSensorChangedIF) except PhidgetException as e: print("IF Kit - Phidget Exception %i: %s" % (e.code, e.details)) return False try: self._interfaceKit.openPhidget() except PhidgetException as e: print("IF Kit - Phidget Exception %i: %s" % (e.code, e.details)) return False self._openIF = True return True def __onAttachedIF(self, e): self._snIF = e.device.getSerialNum() print("InterfaceKit %i Attached!" % (self._snIF)) self._attachedIF = True if self._fistTime: for i in range(0, 3): self._interfaceKit.setOutputState(0, 1) self._interfaceKit.setOutputState(1, 1) self._interfaceKit.setOutputState(2, 1) time.sleep(0.1) self._interfaceKit.setOutputState(0, 0) self._interfaceKit.setOutputState(1, 0) self._interfaceKit.setOutputState(2, 0) time.sleep(0.1) self._fistTime = False def __onDetachedIF(self, e): print("InterfaceKit %i Detached!" % (self._snIF)) self._snIF = -1 self._inp = [0, 0, 0, 0, 0, 0, 0, 0] self._sen = [0, 0, 0, 0, 0, 0, 0, 0] self._attachedIF = False def __onErrorIF(self, e): try: source = e.device print("InterfaceKit %i: Phidget Error %i: %s" % (source.getSerialNum(), e.eCode, e.description)) except PhidgetException as e: print("IF Kit - Phidget Exception %i: %s" % (e.code, e.details)) def __onSensorChangedIF(self, e): self._sen[e.index] = e.value def __onInputChangedIF(self, e): self._inp[e.index] = e.state def getSensors(self): return self._sen def getInputs(self): return self._inp ################ LEDs ####################### def __updateLED(self): t = 0 while self._stop == False: t = (t + 1) % 100 for i in range(0, 3): self._status[i] = ((t + self._ofs[i]) % self._mod[i] == 0) and self._val[i] and bool( self._rep[i]) self._rep[i] = self._rep[i] - int(self._rep[i] > 0 and self._status[i]) if self._attachedIF: self._interfaceKit.setOutputState(i, self._status[i]) time.sleep(0.15) def __setModeLED(self, i, mode, hz=2, cnt=1, ofs=0): if mode == 'on': self._rep[i] = -1 self._val[i] = True self._ofs[i] = 0 self._mod[i] = 1 if mode == 'off': self._rep[i] = -1 self._val[i] = False self._ofs[i] = 0 self._mod[i] = 1 if mode == 'flash': hz = min(max(hz, 1), 100) self._rep[i] = min(max(cnt, 1), 20) self._val[i] = True self._ofs[i] = min(max(ofs, 0), hz) self._mod[i] = hz def setStatus(self, mode, hz=2, cnt=1, ofs=0): self.__setModeLED(1, mode, hz, cnt, ofs) def setError(self, mode, hz=2, cnt=1, ofs=0): self.__setModeLED(2, mode, hz, cnt, ofs) def setSemaphor(self): self.__setModeLED(1, 'flash', 2, 6, 0) self.__setModeLED(2, 'flash', 2, 6, 1)
class IOTools: def __init__(self, onRobot): self.onRobot = onRobot # Camera self._openCam=False # Motor Control self._snMot=-1 self._openMot=False self._attachedMot=False self._cur = [0, 0] # Servo self._snSer=-1 self._openSer=False self._attachedSer=False self._limits = [0, 0] # IF Kit self._snIF=-1 self._openIF=False self._attachedIF=False self._inp = [0, 0, 0, 0, 0, 0, 0, 0] self._sen = [0, 0, 0, 0, 0, 0, 0, 0] # LEDs self._fistTime = True self._status = [0,0,0] self._mod = [8, 1, 1] self._rep = [-1, 5, 6] self._val = [True, False, False] self._ofs = [0, 0, 0] self._updaterThread = threading.Thread(target=self.__updateLED) self._stop = False self._updaterThread.setDaemon(True) self._updaterThread.start() def destroy(self): # LEDs self._stop = True if self._attachedIF: self._interfaceKit.setOutputState(0, 0) self._interfaceKit.setOutputState(1, 0) self._interfaceKit.setOutputState(2, 0) # Servo self.servoDisengage() # Camera self._openCam = False if self._openCam: self._cap.release() def open(self): self.__openIF() self.__openMot() self.__openSer() self.__openCam() ###################### Camera ###################### def __openCam(self): if not os.path.exists('/dev/video0'): return False self._cap = cv2.VideoCapture() if not self._cap.open(-1): return False self._openCam = True def cameraGrab(self): if self._openCam: return self._cap.grab() else: return False def cameraRead(self): if self._openCam: (ret, img)=self._cap.retrieve() return img else: return False def cameraSetResolution(self, sz): """ :rtype : object """ if self._openCam: sz=sz.lower() if sz=='low': self._cap.set(3,160) self._cap.set(4,120) if sz=='medium': self._cap.set(3,640) self._cap.set(4,480) if sz=='high': self._cap.set(3,800) self._cap.set(4,600) if sz=='full': self._cap.set(3,1280) self._cap.set(4,720) def imshow(self, wnd, img): if not self.onRobot: if img.__class__ != numpy.ndarray: print "imshow - invalid image" return False else: cv2.imshow(wnd,img) cv2.waitKey(5) ####################### Servo ###################### def __openSer(self): try: self._advancedServo = AdvancedServo() except RuntimeError as e: print("Servo - Runtime Exception: %s" % e.details) return False try: self._advancedServo.setOnAttachHandler(self.__onAttachedSer) self._advancedServo.setOnDetachHandler(self.__onDetachedSer) self._advancedServo.setOnErrorhandler(self.__onErrorSer) except PhidgetException as e: print("Servo - Phidget Exception %i: %s" % (e.code, e.details)) return False try: self._advancedServo.openPhidget() except PhidgetException as e: print("Servo - Phidget Exception %i: %s" % (e.code, e.details)) return False self._openSer=True return True def __onAttachedSer(self,e): self._snSer = e.device.getSerialNum() self._advancedServo.setServoType(0, ServoTypes.PHIDGET_SERVO_HITEC_HS322HD) self._advancedServo.setAcceleration(0, self._advancedServo.getAccelerationMax(0)) self._advancedServo.setVelocityLimit(0, self._advancedServo.getVelocityMax(0)) self._limits[0] = self._advancedServo.getPositionMin(0) self._limits[1] = self._advancedServo.getPositionMax(0) print("Servo %i Attached! Range: %f - %f" % (self._snSer, self._limits[0], self._limits[1])) self._attachedSer=True def __onDetachedSer(self,e ): print("Servo %i Detached!" % (self._snSer)) self._snSer = -1 self._attachedSer=False def __onErrorSer(self, e): try: source = e.device print("Servo %i: Phidget Error %i: %s" % (source.getSerialNum(), e.eCode, e.description)) except PhidgetException as e: print("Servo - Phidget Exception %i: %s" % (e.code, e.details)) def __closeSer(self): if self._openSer==True: self.servoDisengage() self._advancedServo.closePhidget() def servoEngage(self): if self._attachedSer==True: self._advancedServo.setEngaged(0, True) def servoDisengage(self): if self._attachedSer==True: self._advancedServo.setEngaged(0, False) def servoSet(self, pos): if self._attachedSer==True: self._advancedServo.setPosition(0, min(max(pos,self._limits[0]),self._limits[1])) ############### Motor Control ###################### def __openMot(self): try: self._motorControl = MotorControl() except RuntimeError as e: print("Motor Control - Runtime Exception: %s" % e.details) return False try: self._motorControl.setOnAttachHandler(self.__onAttachedMot) self._motorControl.setOnDetachHandler(self.__onDetachedMot) self._motorControl.setOnErrorhandler(self.__onErrorMot) self._motorControl.setOnCurrentChangeHandler(self.__onCurrentChangedMot) except PhidgetException as e: print("Motor Control - Phidget Exception %i: %s" % (e.code, e.details)) return False try: self._motorControl.openPhidget() except PhidgetException as e: print("Motor Control - Phidget Exception %i: %s" % (e.code, e.details)) return False self._openMot=True return True def __onAttachedMot(self,e): self._snMot = e.device.getSerialNum() print("Motor Control %i Attached!" % (self._snMot)) self._attachedMot=True def __onDetachedMot(self,e ): print("Motor Control %i Detached!" % (self._snMot)) self._snMot = -1 self._attachedMot=False def __onErrorMot(self, e): try: source = e.device print("Motor Control %i: Phidget Error %i: %s" % (source.getSerialNum(), e.eCode, e.description)) except PhidgetException as e: print("Motor Control - Phidget Exception %i: %s" % (e.code, e.details)) def __onCurrentChangedMot(self, e): self._cur[e.index] = e.current def __closeMot(self): if self._openMot==True: self._motorControl.closePhidget() def setMotors(self, speed1=0.0, speed2=0.0, acceleration1=100.0, acceleration2=100.0 ): if self._openMot==True: self._motorControl.setAcceleration(0, acceleration1) self._motorControl.setVelocity(0, speed1) self._motorControl.setAcceleration(1, acceleration2) self._motorControl.setVelocity(1, speed2) ############### Interface Kit ###################### def __closeIF(self): if self._openIF==True: self._interfaceKit.closePhidget() def __openIF(self): try: self._interfaceKit = InterfaceKit() except RuntimeError as e: print("IF Kit - Runtime Exception: %s" % e.details) return False try: self._interfaceKit.setOnAttachHandler(self.__onAttachedIF) self._interfaceKit.setOnDetachHandler(self.__onDetachedIF) self._interfaceKit.setOnErrorhandler(self.__onErrorIF) self._interfaceKit.setOnInputChangeHandler(self.__onInputChangedIF) self._interfaceKit.setOnSensorChangeHandler(self.__onSensorChangedIF) except PhidgetException as e: print("IF Kit - Phidget Exception %i: %s" % (e.code, e.details)) return False try: self._interfaceKit.openPhidget() except PhidgetException as e: print("IF Kit - Phidget Exception %i: %s" % (e.code, e.details)) return False self._openIF=True return True def __onAttachedIF(self,e): self._snIF = e.device.getSerialNum() print("InterfaceKit %i Attached!" % (self._snIF)) self._attachedIF=True if self._fistTime: for i in range(0,3): self._interfaceKit.setOutputState(0, 1) self._interfaceKit.setOutputState(1, 1) self._interfaceKit.setOutputState(2, 1) time.sleep(0.1) self._interfaceKit.setOutputState(0, 0) self._interfaceKit.setOutputState(1, 0) self._interfaceKit.setOutputState(2, 0) time.sleep(0.1) self._fistTime = False def __onDetachedIF(self,e ): print("InterfaceKit %i Detached!" % (self._snIF)) self._snIF = -1 self._inp = [0, 0, 0, 0, 0, 0, 0, 0] self._sen = [0, 0, 0, 0, 0, 0, 0, 0] self._attachedIF=False def __onErrorIF(self, e): try: source = e.device print("InterfaceKit %i: Phidget Error %i: %s" % (source.getSerialNum(), e.eCode, e.description)) except PhidgetException as e: print("IF Kit - Phidget Exception %i: %s" % (e.code, e.details)) def __onSensorChangedIF(self, e): self._sen[e.index] = e.value def __onInputChangedIF(self, e): self._inp[e.index] = e.state def getSensors(self): """ :rtype : object """ return self._sen; def getInputs(self): return self._inp; ################ LEDs ####################### def __updateLED(self): t=0 while self._stop==False: t=(t+1)%100 for i in range(0,3): self._status[i]=((t+self._ofs[i])%self._mod[i]==0) and self._val[i] and bool(self._rep[i]) self._rep[i]=self._rep[i]-int(self._rep[i]>0 and self._status[i]) if self._attachedIF: self._interfaceKit.setOutputState(i, self._status[i]) time.sleep(0.15) def __setModeLED(self, i, mode, hz=2, cnt=1, ofs=0): if mode=='on': self._rep[i]=-1 self._val[i]=True self._ofs[i]=0 self._mod[i]=1 if mode=='off': self._rep[i]=-1 self._val[i]=False self._ofs[i]=0 self._mod[i]=1 if mode=='flash': hz=min(max(hz,1),100) self._rep[i]=min(max(cnt,1),20) self._val[i]=True self._ofs[i]=min(max(ofs,0),hz) self._mod[i]=hz def setStatus(self, mode, hz=2, cnt=1, ofs=0): self.__setModeLED(1,mode, hz, cnt, ofs) def setError(self, mode, hz=2, cnt=1, ofs=0): self.__setModeLED(2,mode, hz, cnt, ofs) def setSemaphor(self): self.__setModeLED(1,'flash', 2, 6, 0) self.__setModeLED(2,'flash', 2, 6, 1)
""" #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, CurrentChangeEventArgs, PositionChangeEventArgs, VelocityChangeEventArgs from Phidgets.Devices.AdvancedServo import AdvancedServo from Phidgets.Devices.Servo import ServoTypes from Phidgets.Phidget import PhidgetLogLevel #Create an advancedServo object try: advancedServo = AdvancedServo() except RuntimeError as e: print("Runtime Exception: %s" % e.details) print("Exiting....") exit(1) #stack to keep current values in currentList = [0,0,0,0,0,0,0,0] velocityList = [0,0,0,0,0,0,0,0] #Information Display Function def DisplayDeviceInfo(): print("|------------|----------------------------------|--------------|------------|") print("|- Attached -|- Type -|- Serial No. -|- Version -|") print("|------------|----------------------------------|--------------|------------|") print("|- %8s -|- %30s -|- %10d -|- %8d -|" % (advancedServo.isAttached(), advancedServo.getDeviceName(), advancedServo.getSerialNum(), advancedServo.getDeviceVersion()))
""" #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, CurrentChangeEventArgs, PositionChangeEventArgs, VelocityChangeEventArgs from Phidgets.Devices.AdvancedServo import AdvancedServo from Phidgets.Devices.Servo import ServoTypes from Phidgets.Phidget import PhidgetLogLevel #Create an advancedServo object try: advancedServo = AdvancedServo() except RuntimeError as e: print("Runtime Exception: %s" % e.details) print("Exiting....") exit(1) #stack to keep current values in currentList = [0, 0, 0, 0, 0, 0, 0, 0] velocityList = [0, 0, 0, 0, 0, 0, 0, 0] #Information Display Function def DisplayDeviceInfo(): print( "|------------|----------------------------------|--------------|------------|" )
print("TextLCD %i: Phidget Error %i: %s" % (source.getSerialNum(), e.eCode, e.description)) except PhidgetException as e: print("Phidget Exception %i: %s" % (e.code, e.details)) try: interfaceKit_Relay = InterfaceKit() except RuntimeError as e: print("Runtime Exception: %s" % e.details) print("Exiting....") exit(1) #Create an advancedServo object try: advancedServo = AdvancedServo() except RuntimeError as e: print("Runtime Exception: %s" % e.details) print("Exiting....") exit(1) #stack to keep current values in currentList = [0, 0, 0, 0, 0, 0, 0, 0] velocityList = [0, 0, 0, 0, 0, 0, 0, 0] #Information Display Function def DisplayDeviceInfo(): print( "|------------|----------------------------------|--------------|------------|" )