class MotorCtl(object):
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 shutdown(self):
# Always stop the robot when shutting down the node.
#Stop the motor
self.mc.setVelocity(self.motorstate_d['motor'],0)
rospy.loginfo("Stopping the Node...")
rospy.sleep(1)
def procMotorCtl(self,data):
rospy.loginfo("Got command for motor=%s command=%f",self.motorName,data.data[1])
self.motorCmd(data.data[0],data.data[1])
def motorCmd(self,motor,velocity):
self.mc.setAcceleration(motor, 50.00)
self.mc.setVelocity(motor, velocity)
self.motorstate_d['motor']=motor
self.motorstate_d['velocity']=velocity
if velocity >0:
self.motorstate_d['state']=1
else:
self.motorstate_d['state']=0
self.motorstate_d['ticks']=self.mc.getEncoderPosition(0)
rospy.loginfo("encoder ticks=%d",self.motorstate_d['ticks'])
print (self.motorstate_d.values())
self.motordata.data=self.motorstate_d.values()
self.rsp.publish(self.motordata)
def runner(self):
motorCtl = rospy.Subscriber('motor_ctl',Int32MultiArray,self.procMotorCtl)
rospy.spin()
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
print("Exiting....")
exit(1)
print("Opening phidget object....")
try:
motorControl.openPhidget()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
print("Waiting for attach....")
try:
motorControl.waitForAttach(10000)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
try:
motorControl.closePhidget()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
print("Exiting....")
exit(1)
else:
displayDeviceInfo()
#Control the motor a bit.
print("Will now simulate motor operation....")
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()
motorControlR.openRemote('odroid',serial=298856)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
if not motorControlL.isAttachedToServer():
sleep(2)
print('motorControlL attached to server status: %s' % motorControlL.isAttachedToServer() )
print('motorControlR attached to server status: %s' % motorControlR.isAttachedToServer() )
print("Waiting for attach....")
try:
motorControlL.waitForAttach(10000)
motorControlR.waitForAttach(10000)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
try:
motorControlL.closePhidget()
motorControlR.closePhidget()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
print("Exiting....")
exit(1)
else:
displayDeviceInfo()
print("Exiting....")
exit(1)
print("Opening phidget object....")
try:
motorControl.openPhidget()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
print("Waiting for attach....")
try:
motorControl.waitForAttach(10000)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
try:
motorControl.closePhidget()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
print("Exiting....")
exit(1)
else:
displayDeviceInfo()
#Control the motor a bit.
print("Will now simulate motor operation....")
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
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
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)
try:
mcL.waitForAttach(10000)
mcR.waitForAttach(10000)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
try:
mcL.closePhidget()
mcR.closePhidget()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
print("Exiting....")
exit(1)
print("mcL attached to WebService: %s" % mcL.isAttachedToServer())
print("mcR attached to WebService: %s" % mcR.isAttachedToServer())
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
"""
rospy.init_node(
'PhidgetMotor',
log_level = rospy.DEBUG
)
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)
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:
rospy.logerr("Unable to register the handlers")
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("PhidgetMotor", MotorCommand ,move)
phidgetMotorService = rospy.Service('PhidgetMotor',Move, move)
if phidget1065 == True:
posdataPub = rospy.Publisher("position_data", PosMsg)
rospy.spin()
class MotorCtl(object):
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 shutdown(self):
# Always stop the robot when shutting down the node.
#Stop the motor
self.mc.setVelocity(self.motorstate_d['motor'], 0)
rospy.loginfo("Stopping the Node...")
rospy.sleep(1)
def procMotorCtl(self, data):
rospy.loginfo("Got command for motor=%s command=%f", self.motorName,
data.data[1])
self.motorCmd(data.data[0], data.data[1])
def motorCmd(self, motor, velocity):
self.mc.setAcceleration(motor, 50.00)
self.mc.setVelocity(motor, velocity)
self.motorstate_d['motor'] = motor
self.motorstate_d['velocity'] = velocity
if velocity > 0:
self.motorstate_d['state'] = 1
else:
self.motorstate_d['state'] = 0
self.motorstate_d['ticks'] = self.mc.getEncoderPosition(0)
rospy.loginfo("encoder ticks=%d", self.motorstate_d['ticks'])
print(self.motorstate_d.values())
self.motordata.data = self.motorstate_d.values()
self.rsp.publish(self.motordata)
def runner(self):
motorCtl = rospy.Subscriber('motor_ctl', Int32MultiArray,
self.procMotorCtl)
rospy.spin()
print("Goodbye")
exit(1)
# Initialize pygame
startPygame()
joystick.init()
# Initialize motors and servo
try:
print("Initializing motors and servo...")
m1.openPhidget(serial=392573)
m2.openPhidget(serial=393150)
m3.openPhidget(serial=398770)
s1.openPhidget(serial=99590)
m1.waitForAttach(10000)
m2.waitForAttach(10000)
m3.waitForAttach(10000)
s1.waitForAttach(10000)
print("Done\n")
except Exception as e:
print str(e)
exit(1)
# Setup motors and servos
print("Setting up motors and servo...")
s1.setServoType(0, ServoTypes.PHIDGET_SERVO_HITEC_HS322HD)
s1.setEngaged(0, True)
s1.setAcceleration(0, s1.getAccelerationMax(0))
s1.setVelocityLimit(0, s1.getVelocityMax(0))
s1.setPosition(0, s1.getPositionMin(0))
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))
exit(1)
"""
Step 3 - Detect when a device is attached
"""
try:
# method has units of milliseconds, so wait for 5 seconds
motor.waitForAttach(5000)
encoder.waitForAttach(5000)
torque_sensor.waitForAttach(5000)
except PhidgetException as error:
print ('Device Error %i: %s' % (error.code, error.details))
try:
motor.closePhidget()
encoder.closePhidget()
torque_sensor.closePhidget()
except PhidgetException as error:
print ('Phidget Error %i: %s' % (error.code, error.details))
exit(1)
exit(1)
"""
Step 4 - Main Control Program
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
if not motorControlL.isAttachedToServer():
sleep(2)
print('motorControlL attached to server status: %s' %
motorControlL.isAttachedToServer())
print('motorControlR attached to server status: %s' %
motorControlR.isAttachedToServer())
print("Waiting for attach....")
try:
motorControlL.waitForAttach(10000)
motorControlR.waitForAttach(10000)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
try:
motorControlL.closePhidget()
motorControlR.closePhidget()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
print("Exiting....")
exit(1)
else:
displayDeviceInfo()
