def run(self):
rate = rospy.Rate(10)
try:
while not rospy.is_shutdown():
rate.sleep()
except rospy.ROSInterruptException:
motors.forceStop()
pass
print("Motor 1 reverse")
for s in test_reverse_speeds:
motors.motor1.setSpeed(s)
raiseIfFault()
time.sleep(0.002)
# Disable the drivers for half a second.
motors.disable()
time.sleep(0.5)
motors.enable()
print("Motor 2 forward")
for s in test_forward_speeds:
motors.motor2.setSpeed(s)
raiseIfFault()
time.sleep(0.002)
print("Motor 2 reverse")
for s in test_reverse_speeds:
motors.motor2.setSpeed(s)
raiseIfFault()
time.sleep(0.002)
except DriverFault as e:
print("Driver %s fault!" % e.driver_num)
finally:
# Stop the motors, even if there is an exception
# or the user presses Ctrl+C to kill the process.
motors.forceStop()