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(
"|------------|----------------------------------|--------------|------------|"
)
