def __init__(self, motornum, motorname): self.sernum = motornum self.motorName = motorname self.mc = MotorControl() try: self.mc.openPhidget(self.sernum) self.mc.waitForAttach(10000) except PhidgetException as e: print("Phidget Exception %i: %s" % (e.code, e.details)) print("Exiting....") exit(1) self.motorstate_d = { 'motor': 0, 'serial': self.sernum, 'state': 0, 'velocity': 0, 'ticks': 0 } self.motordata = Int32MultiArray() self.motordata.layout.dim = [MultiArrayDimension('motor_state', 4, 1)] self.motordata.data = self.motorstate_d.values() self.rsp = rospy.Publisher('phidget_motor_state', Int32MultiArray, queue_size=10) rospy.init_node('aux_motor_ctl', anonymous=True) rospy.on_shutdown(self.shutdown) robotrate = 10 r = rospy.Rate(robotrate)
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 setupMoveService(): """Initialize the DriveMotors service Establish a connection with the Phidget HC Motor Control and then with the ROS Master as the service DriveMotors """ global motorControl, minAcceleration, maxAcceleration try: motorControl = MotorControl() except: rospy.logerr("Unable to connect to Phidget HC Motor Control") return try: motorControl.setOnAttachHandler(mcAttached) motorControl.setOnDetachHandler(mcDetached) motorControl.setOnErrorhandler(mcError) motorControl.setOnCurrentChangeHandler(mcCurrentChanged) motorControl.setOnInputChangeHandler(mcInputChanged) motorControl.setOnVelocityChangeHandler(mcVelocityChanged) except: rospy.logerr("Unable to register the handlers") return try: motorControl.openPhidget() except PhidgetException as e: rospy.logerr("Fail to openPhidget() %i: %s", e.code, e.details) return try: motorControl.waitForAttach(10000) except PhidgetException as e: rospy.logerr("Fail to attach to Phidget %i: %s", e.code, e.details) return if motorControl.isAttached(): rospy.loginfo("Device: %s, Serial: %d, Version: %d", motorControl.getDeviceName(), motorControl.getSerialNum(), motorControl.getDeviceVersion()) minAcceleration = motorControl.getAccelerationMin(leftWheels) maxAcceleration = motorControl.getAccelerationMax(leftWheels) rospy.init_node('DriveMotors', log_level=rospy.DEBUG) phidgetMotorService = rospy.Service('DriveMotors', Move, move) rospy.spin()
def _BuildPhidgetsMotorController(serial_number): #Create an motorcontrol object try: motorControl = MotorControl() motorControl.setOnErrorhandler(motorControlError) motorControl.openPhidget(serial_number) motorControl.waitForAttach(10000) print("Serial number: %i" % motorControl.getSerialNum()) return motorControl except RuntimeError as e: motorControl.closePhidget() print("Runtime Exception: %s" % e.details) return None
def setupMoveService(): """Initialize the PhidgetMotor service Establish a connection with the Phidget HC Motor Control and then with the ROS Master as the service PhidgetMotor """ node_name = 'PhidgetMotor' rospy.init_node(node_name, log_level=rospy.DEBUG) serial_no = rospy.get_param("~serial_no", 0) if not serial_no == 0: rospy.loginfo("Using motor controller with serial number %d", serial_no) else: rospy.loginfo( "No serial number specified. This is fine for systems with one controller, but may behave unpredictably for systems with multiple motor controllers" ) global motorControl, motorControlRight, minAcceleration, maxAcceleration, timer, motors_inverted, phidget1065, rightWheels, posdataPub timer = 0 try: motorControl = MotorControl() except: rospy.logerr("Unable to connect to Phidget HC Motor Control") return try: motorControl.setOnAttachHandler(mcAttached) motorControl.setOnDetachHandler(mcDetached) motorControl.setOnErrorhandler(mcError) motorControl.setOnCurrentChangeHandler(mcCurrentChanged) motorControl.setOnInputChangeHandler(mcInputChanged) motorControl.setOnVelocityChangeHandler(mcVelocityChanged) if serial_no != 0: motorControl.openPhidget(serial_no) else: motorControl.openPhidget() #attach the board motorControl.waitForAttach(10000) """use the function getMotorCount() to know how many motors the Phidget board can take if the result is more than 1, we have a 1064 board and we take care of both motors with one motorControl variable. The corobot_phidgetIK handles the Phidget encoder board that is separated of the phidget 1064. if the result is equal to 1, we have two phidget 1065 boards. The one with serial number that is the lowest will be the left will, the other the right weel. The encoder has to be handled in this file as it is part of the 1065 board. """ if motorControl.getMotorCount() == 1: phidget1065 = True rightWheels = 0 motorControlRight.setOnAttachHandler(mcAttached) motorControlRight.setOnDetachHandler(mcDetached) motorControlRight.setOnErrorhandler(mcError) motorControlRight.setOnCurrentChangeHandler(mcCurrentChanged) motorControlRight.setOnInputChangeHandler(mcInputChanged) motorControlRight.setOnVelocityChangeHandler(mcVelocityChanged) if motorControl.getSerialNum() > motorControlRight.getSerialNum(): """ As a rule, we need the serial number of the left board to be lower than for the right board. This is for consistancy for all the robots """ motorControlTemp = motorControl motorControl = motorControlRight motorControlRight = motorControlTemp #Set up the encoders handler motorControl.setOnPositionUpdateHandler(leftEncoderUpdated) motorControlRight.setOnPositionUpdateHandler(rightEncoderUpdated) #attach the board motorControlRight.waitForAttach(10000) except PhidgetException as e: rospy.logerr("Unable to register the handlers: %i: %s", e.code, e.details) return except AttributeError as e: rospy.logerr("Unable to register the handlers: %s", e) return except: rospy.logerr("Unable to register the handlers: %s", sys.exc_info()[0]) return if motorControl.isAttached(): rospy.loginfo("Device: %s, Serial: %d, Version: %d", motorControl.getDeviceName(), motorControl.getSerialNum(), motorControl.getDeviceVersion()) if phidget1065 == True: if motorControlRight.isAttached(): rospy.loginfo("Device: %s, Serial: %d, Version: %d", motorControlRight.getDeviceName(), motorControlRight.getSerialNum(), motorControlRight.getDeviceVersion()) minAcceleration = motorControl.getAccelerationMin(leftWheels) maxAcceleration = motorControl.getAccelerationMax(leftWheels) motors_inverted = rospy.get_param('~motors_inverted', False) phidgetMotorTopic = rospy.Subscriber(node_name, MotorCommand, move) phidgetMotorService = rospy.Service(node_name, Move, move) if phidget1065 == True: posdataPub = rospy.Publisher("position_data", PosMsg) rospy.spin()
__version__ = '2.1.8' __date__ = 'May 17 2010' #Basic imports from ctypes import * import sys #Phidget specific imports from Phidgets.PhidgetException import PhidgetErrorCodes, PhidgetException from Phidgets.Events.Events import AttachEventArgs, DetachEventArgs, ErrorEventArgs, CurrentChangeEventArgs, InputChangeEventArgs, VelocityChangeEventArgs from Phidgets.Devices.MotorControl import MotorControl #import methods for sleeping thread from time import sleep #Create an motorcontrol object try: motorControl = MotorControl() except RuntimeError as e: print("Runtime Exception: %s" % e.details) print("Exiting....") exit(1) #Information Display Function def displayDeviceInfo(): print( "|------------|----------------------------------|--------------|------------|" ) print( "|- Attached -|- Type -|- Serial No. -|- Version -|" ) print(
def setupMoveService(): """Initialize the PhidgetLinear service Establish a connection with the Phidget 1065 Motor Control and then with the ROS Master as the service PhidgetLinear """ rospy.init_node('PhidgetLinear', log_level=rospy.DEBUG) serial_no = rospy.get_param("~serial_no", 0) if not serial_no == 0: rospy.loginfo("Using motor controller with serial number %d", serial_no) else: rospy.loginfo( "No serial number specified. This is fine for systems with one controller, but may behave unpredictably for systems with multiple motor controllers" ) global motorControl, minAcceleration, maxAcceleration, timer, invert_speed, posdataPub timer = 0 try: motorControl = MotorControl() except: rospy.logerr("Unable to connect to Phidget Motor Control") return try: motorControl.setOnAttachHandler(mcAttached) motorControl.setOnDetachHandler(mcDetached) motorControl.setOnErrorhandler(mcError) motorControl.setOnCurrentChangeHandler(mcCurrentChanged) motorControl.setOnInputChangeHandler(mcInputChanged) motorControl.setOnVelocityChangeHandler(mcVelocityChanged) motorControl.setOnSensorUpdateHandler(mcSensorUpdated) if serial_no != 0: motorControl.openPhidget(serial_no) else: motorControl.openPhidget() #attach the board motorControl.waitForAttach(10000) except PhidgetException as e: rospy.logerr("Unable to register the handlers: %i: %s", e.code, e.details) return except AttributeError as e: rospy.logerr("Unable to register the handlers: %s", e) return except: rospy.logerr("Unable to register the handlers: %s", sys.exc_info()[0]) return if motorControl.isAttached(): rospy.loginfo("Device: %s, Serial: %d, Version: %d", motorControl.getDeviceName(), motorControl.getSerialNum(), motorControl.getDeviceVersion()) # ensure the motor controller attempts to brake the linear # when velocity is cut to 0 motorControl.setBraking(linear, 100) # print motorControl.getBraking(linear) minAcceleration = motorControl.getAccelerationMin(linear) maxAcceleration = motorControl.getAccelerationMax(linear) invert_speed = rospy.get_param('~invert_speed', False) maxPos = rospy.get_param('~max_pos', 1014) minPos = rospy.get_param('~min_pos', 10) phidgetMotorTopic = rospy.Subscriber("PhidgetLinear", LinearCommand, move) phidgetMotorService = rospy.Service('PhidgetLinear', Move, move) posdataPub = rospy.Publisher("position", UInt32, latch=True) rospy.spin()
def initMotorAndEncoderBoards(): global motorControl, motorControlRight, rightWheels, phidget1065, encoders, leftEncoderPosition, rightEncoderPosition, motors_inverted, encoders_inverted try: motorControl = MotorControl() except: rospy.logerr("Unable to connect to Phidget HC Motor Control") return try: motorControl.setOnAttachHandler(mcAttached) motorControl.setOnErrorhandler(mcError) motorControl.setOnVelocityChangeHandler(mcVelocityChanged) motorControl.openPhidget() #attach the board motorControl.waitForAttach(10000) """use the function getMotorCount() to know how many motors the Phidget board can take if the result is more than 1, we have a 1064 board and we take care of both motors with one motorControl variable. We need to handle the Phidget encoder board that is separated from the phidget 1064. if the result is equal to 1, we have two phidget 1065 boards. The one with serial number that is the lowest will be the left will, the other the right weel. The encoder has to be handled in this file as it is part of the 1065 board. """ if motorControl.getMotorCount() == 1: phidget1065 = True rightWheels = 0 motorControlRight = MotorControl() motorControlRight.setOnAttachHandler(mcAttached) motorControlRight.setOnErrorhandler(mcError) motorControlRight.setOnVelocityChangeHandler(mcVelocityChanged) if motorControl.getSerialNum() > motorControlRight.getSerialNum(): """ As a rule, we need the serial number of the left board to be lower than for the right board. This is for consistancy for all the robots """ motorControlTemp = motorControl motorControl = motorControlRight motorControlRight = motorControlTemp #Set up the encoders handler motorControl.setOnPositionUpdateHandler(leftEncoderUpdated) motorControlRight.setOnPositionUpdateHandler(rightEncoderUpdated) #attach the board motorControlRight.openPhidget() motorControlRight.waitForAttach(10000) # we have a motor controller board that control 2 motors but doesn't get any encoder input, so we need to initialize the encoder board. else: encoders = Encoder() encoders.setOnPositionChangeHandler(encoderBoardPositionChange) encoders.openPhidget() encoders.waitForAttach(10000) # some robots have the left and right encoders switched by mistake if(motors_inverted or encoders_inverted): leftEncoderPosition = 1; rightEncoderPosition = 0; encoders.setEnabled(leftEncoderPosition, True) encoders.setEnabled(rightEncoderPosition, True) except PhidgetException as e: motorsError = 1 encodersError = 1 rospy.logerr("Unable to initialize the motors and encoders board: %i: %s", e.code, e.details) return except: motorsError = 1 encodersError = 1 rospy.logerr("Unable to register the motors and encoders board") return if motorControl.isAttached(): rospy.loginfo("Device: %s, Serial: %d, Version: %d",motorControl.getDeviceName(),motorControl.getSerialNum(),motorControl.getDeviceVersion()) if phidget1065 == True: if motorControlRight.isAttached(): rospy.loginfo("Device: %s, Serial: %d, Version: %d",motorControlRight.getDeviceName(),motorControlRight.getSerialNum(),motorControlRight.getDeviceVersion()) else: rospy.loginfo("Device: %s, Serial: %d, Version: %d",encoders.getDeviceName(),encoders.getSerialNum(),encoders.getDeviceVersion()) if stop_when_obstacle: timer = Timer(1.0, checkEncoders) timer.start() return
__author__ = 'Cameron Rodda' __version__ = '0.0.1' __date__ = '21 August 2013' import commands import zmq import time from ctypes import * import sys from Phidgets.PhidgetException import PhidgetErrorCodes, PhidgetException from Phidgets.Events.Events import AttachEventArgs, DetachEventArgs, ErrorEventArgs, CurrentChangeEventArgs, InputChangeEventArgs, VelocityChangeEventArgs from Phidgets.Devices.MotorControl import MotorControl #Create a motor control object try: mcL = MotorControl() # Left Motor mcR = MotorControl() # Right Motor except RuntimeError as e: print("Runtime Exception: %s" % e.details) print("Exiting....") exit(1) try: #mcL.openPhidget(serial=298857) #mcR.openPhidget(serial=298856) mcL.openRemote('odroid', serial=298857) mcR.openRemote('odroid', serial=298856) except PhidgetException as e: print("Phidget Exception %i: %s" % (e.code, e.details)) print("Exiting....") exit(1)
def AttachMotorControl(databasepath, serialNumber): def onAttachHandler(event): logString = "MotorControl Attached " + str(event.device.getSerialNum()) #print(logString) DisplayAttachedDeviceInfo(event.device) def onDetachHandler(event): logString = "MotorControl Detached " + str(event.device.getSerialNum()) #print(logString) DisplayDetachedDeviceInfo(event.device) event.device.closePhidget() def onErrorHandler(event): logString = "MotorControl Error " + str(event.device.getSerialNum()) + ", Error: " + event.description print(logString) DisplayErrorDeviceInfo(event.device) def onServerConnectHandler(event): logString = "MotorControl Server Connect " + str(event.device.getSerialNum()) #print(logString) def onServerDisconnectHandler(event): logString = "MotorControl Server Disconnect " + str(event.device.getSerialNum()) #print(logString) def backEMFUpdateHandler(event): logString = "MotorControl BackEMF Update" #print(logString) try: conn = sqlite3.connect(databasepath) #conn.execute("INSERT INTO MOTORCONTROL_BACKEMFUPDATE VALUES(NULL, DateTime('now'), ?, ?, ?)", #(event.device.getSerialNum(), event.index, event.voltage)) conn.commit() conn.close() except sqlite3.Error as e: print "An error occurred:", e.args[0] def currentChangeHandler(event): logString = "MotorControl Current Change" #print(logString) try: conn = sqlite3.connect(databasepath) #conn.execute("INSERT INTO MOTORCONTROL_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 currentUpdateHandler(event): logString = "MotorControl Current Update" #print(logString) try: conn = sqlite3.connect(databasepath) #conn.execute("INSERT INTO MOTORCONTROL_CURRENTUPDATE VALUES(NULL, DateTime('now'), ?, ?, ?)", #(event.device.getSerialNum(), event.index, event.current)) conn.commit() conn.close() except sqlite3.Error as e: print "An error occurred:", e.args[0] def inputChangeHandler(event): logString = "MotorControl Input Change" #print(logString) try: conn = sqlite3.connect(databasepath) #conn.execute("INSERT INTO MOTORCONTROL_INPUTCHANGE VALUES(NULL, DateTime('now'), ?, ?, ?)", #(event.device.getSerialNum(), event.index, event.state)) conn.commit() conn.close() except sqlite3.Error as e: print "An error occurred:", e.args[0] def positionChangeHandler(event): logString = "MotorControl Position Change" #print(logString) try: conn = sqlite3.connect(databasepath) #conn.execute("INSERT INTO MOTORCONTROL_POSITIONCHANGE 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 positionUpdateHandler(event): logString = "MotorControl Position Update" #print(logString) try: conn = sqlite3.connect(databasepath) #conn.execute("INSERT INTO MOTORCONTROL_POSITIONUPDATE 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 sensorUpdateHandler(event): logString = "MotorControl Sensor Update" #print(logString) try: conn = sqlite3.connect(databasepath) #conn.execute("INSERT INTO MOTORCONTROL_SENSORUPDATE VALUES(NULL, DateTime('now'), ?, ?, ?)", #(event.device.getSerialNum(), event.index, event.value)) conn.commit() conn.close() except sqlite3.Error as e: print "An error occurred:", e.args[0] def velocityChangeHandler(event): logString = "MotorControl Velocity Change" #print(logString) try: conn = sqlite3.connect(databasepath) conn.execute("INSERT INTO MOTORCONTROL_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 = MotorControl() p.setOnAttachHandler(onAttachHandler) p.setOnDetachHandler(onDetachHandler) p.setOnErrorhandler(onErrorHandler) p.setOnServerConnectHandler(onServerConnectHandler) p.setOnServerDisconnectHandler(onServerDisconnectHandler) p.setOnBackEMFUpdateHandler (backEMFUpdateHandler) p.setOnCurrentChangeHandler (currentChangeHandler) p.setOnCurrentUpdateHandler (currentUpdateHandler) p.setOnInputChangeHandler (inputChangeHandler) p.setOnPositionChangeHandler(positionChangeHandler) p.setOnPositionUpdateHandler(positionUpdateHandler) p.setOnSensorUpdateHandler (sensorUpdateHandler) p.setOnVelocityChangeHandler(velocityChangeHandler) p.openPhidget(serialNumber) except PhidgetException as e: print("Phidget Exception %i: %s" % (e.code, e.details)) print("Exiting...") exit(1)
#!/usr/bin/env python from time import sleep # Wait between commands 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)
interC.setOutputState(i, nOutputs[i]) motors = nMotors 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
import time import csv # Device specific imports from Phidgets.PhidgetException import PhidgetErrorCodes, PhidgetException from Phidgets.Events.Events import * from Phidgets.Manager import Manager from Phidgets.Devices.MotorControl import MotorControl from Phidgets.Devices.Encoder import Encoder from Phidgets.Phidget import PhidgetLogLevel """ Step 1 - Create device objects(s) [I'm skipping the event handlers & their callbacks for simplicity] """ try: motor = MotorControl() encoder = Encoder() torque_sensor = AnalogInput() except RuntimeError as error: print ('Python Runtime Exception relating to motor object: %s' % error.details) exit(1) """ Step 2 - Open device(s) using the object created above """ try: motor.openPhidget() encoder.openPhidget() torque_sensor.openPhidget() except PhidgetException as error: print ('Device error %i, %s' % (error.code, error.details))
import sys #Phidget specific imports from Phidgets.PhidgetException import PhidgetErrorCodes, PhidgetException from Phidgets.Events.Events import AttachEventArgs, DetachEventArgs, ErrorEventArgs, CurrentChangeEventArgs, InputChangeEventArgs, VelocityChangeEventArgs from Phidgets.Devices.MotorControl import MotorControl #import methods for sleeping thread from time import sleep import zmq import logging from datetime import datetime #Create an motorcontrol object try: motorControlL = MotorControl() # Left Motor motorControlR = MotorControl() # Right Motor except RuntimeError as e: print("Runtime Exception: %s" % e.details) print("Exiting....") exit(1) #Information Display Function def displayDeviceInfo(): print( "|------------|------------------------------------|--------------|------------|" ) print( "|- Attached -|- Type -|- Serial No. -|- Version -|" )