def motor_run(motor_chn, speed):
#pismart.motor_switch(1)
# motor库中有速度正负和限值的处理。
if motor_chn == "A":
MotorA = Motor(Motor.MotorA)
MotorA.speed = int(speed)
elif motor_chn == "B":
MotorB = Motor(Motor.MotorB)
MotorB.speed = int(speed)
elif motor_chn == "both":
MotorA = Motor(Motor.MotorA)
MotorA.speed = int(speed)
MotorB = Motor(Motor.MotorB)
MotorB.speed = int(speed)
msg = "[PiSmart] Set Motor %s at speed %d" % (motor_chn, speed)
print(msg)
# i = i + 1
global time
time = rospy.Time.now()
r = rospy.Rate(100)
while not rospy.is_shutdown():
if (rospy.Time.now() - time).to_sec() > 0.5:
motorA.speed = 0
motorB.speed = 0
r.sleep()
if __name__ == '__main__':
try:
nodeName = 'pismart0'
print nodeName
rospy.init_node(nodeName, anonymous=False)
rospy.Subscriber(nodeName+'_motor_speed', Vector3, motorCallback)
rospy.Subscriber(nodeName+'_led', Int32, ledCallback)
print "start listening"
t = threading.Thread(target=imuThread)
t.start()
listener()
t.join()
except KeyboardInterrupt:
motorA.speed = 0
motorB.speed = 0
motorA.stop()
motorB.stop()
p.motor_switch(0)