def main():
rospy.init_node('motion', anonymous=False)
if rospy.get_param('/simulation_mode', False):
print "[motion] Bye!"
return
# Get this robot's motor's QPPS from rosparam
m1 = rospy.get_param('M1QPPS', None)
m2 = rospy.get_param('M2QPPS', None)
m3 = rospy.get_param('M3QPPS', None)
# Does this robot use v3 or v5 RoboClaws?
use_rcv3 = rospy.get_param('use_rcv3', True)
# init wheelbase
wheelbase.init(m1qpps=m1, m2qpps=m2, m3qpps=m3, use_rcv3=use_rcv3)
# Register a shutdown hook to kill motion
rospy.on_shutdown(_shutdown_hook)
rospy.Subscriber('vel_cmds', Twist, _handle_velocity_command)
pub = rospy.Publisher('encoder_estimates', EncoderEstimates, queue_size=10)
# Services
rospy.Service('battery', RoboClawRPC, _get_battery_voltage)
rospy.Service('kick', Trigger, _kick)
# So that we know the robot's theta
rospy.Subscriber('robot_state', RobotState, _handle_theta)
rospy.spin()
return
rate = rospy.Rate(10) # 100hz
while not rospy.is_shutdown():
(vx, vy, w, s1, s2, s3) = motion.get_velocities(theta=_theta)
estimate = EncoderEstimates()
estimate.world_velocities.vx = vx
estimate.world_velocities.vy = vy
estimate.world_velocities.w = w
estimate.pulses.s1 = s1
estimate.pulses.s2 = s2
estimate.pulses.s3 = s3
pub.publish(estimate)
rate.sleep()
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
def test_square_calibration(velocity=0.6,
sleep_time=1.5,
M1QPPS=None,
M2QPPS=None,
M3QPPS=None):
global wheelbase_configured
# Make sure everything is init'd
if not wheelbase_configured:
w.init()
wheelbase_configured = True
_m1qpps = M1QPPS if M1QPPS is not None else qppsM1
_m2qpps = M2QPPS if M2QPPS is not None else qppsM2
_m3qpps = M3QPPS if M3QPPS is not None else qppsM3
if _m1qpps is not None and _m2qpps is not None and _m3qpps is not None:
w.SetVelocityPID(w.M1, kp, ki, kd, _m1qpps)
w.SetVelocityPID(w.M2, kp, ki, kd, _m2qpps)
w.SetVelocityPID(w.M3, kp, ki, kd, _m3qpps)
# Right
motion.drive(velocity, 0, 0)
time.sleep(sleep_time)
motion.stop()
time.sleep(sleep_time)
# Up
motion.drive(0, velocity, 0)
time.sleep(sleep_time)
motion.stop()
time.sleep(sleep_time)
# Left
motion.drive(-velocity, 0, 0)
time.sleep(sleep_time)
motion.stop()
time.sleep(sleep_time)
# Down
motion.drive(0, -velocity, 0)
time.sleep(sleep_time)
motion.stop()
def test_square_calibration(velocity=0.6, sleep_time=1.5, M1QPPS=None, M2QPPS=None, M3QPPS=None):
global wheelbase_configured
# Make sure everything is init'd
if not wheelbase_configured:
w.init()
wheelbase_configured = True
_m1qpps = M1QPPS if M1QPPS is not None else qppsM1
_m2qpps = M2QPPS if M2QPPS is not None else qppsM2
_m3qpps = M3QPPS if M3QPPS is not None else qppsM3
if _m1qpps is not None and _m2qpps is not None and _m3qpps is not None:
w.SetVelocityPID(w.M1, kp, ki, kd, _m1qpps)
w.SetVelocityPID(w.M2, kp, ki, kd, _m2qpps)
w.SetVelocityPID(w.M3, kp, ki, kd, _m3qpps)
# Right
motion.drive(velocity, 0, 0)
time.sleep(sleep_time)
motion.stop()
time.sleep(sleep_time)
# Up
motion.drive(0, velocity, 0)
time.sleep(sleep_time)
motion.stop()
time.sleep(sleep_time)
# Left
motion.drive(-velocity, 0, 0)
time.sleep(sleep_time)
motion.stop()
time.sleep(sleep_time)
# Down
motion.drive(0, -velocity, 0)
time.sleep(sleep_time)
motion.stop()
def main():
global _motion_timer
_motion_timer = RepeatedTimer(_motion_timer_period, _handle_motion_timer)
_motion_timer.start()
global _odom_timer
_odom_timer = RepeatedTimer(_odom_timer_period, _handle_odom_timer)
_odom_timer.start()
global _ctrl_timer
_ctrl_timer = RepeatedTimer(_ctrl_timer_period, _handle_ctrl_timer)
_ctrl_timer.start()
use_rcv3 = os.environ['USE_RCV3'] == 'true'
w.init(use_rcv3=use_rcv3)
# TODO: Okay, so Controller.init(None) will automagically load in some PID
# values for each of the x, y, theta position controllers. However, since
# `teleop` is run in real life on different robots, really it should read
# the correct YAML robot config file and load in the PID constants and
# pass them into Controller.init(gains) as in the controller_node.py.
# Two ways to do this: (1) do like os.environ['USE_RCV3'], where the odroid_setup.bash
# script loads up the PID constants as environment variables (this seems messy).
# (2) find a python YAML reading library and just read the file directly from here
# based on which robot this is (you can use os.environ['ROBOT'])
Controller.init()
print 'Started.'
global _velocities, _set_speed, _smooth, _odom_on, _previous_action, _ctrl_on
while(1):
action = get_action()
if action == 'UP':
_set_speed = True
_velocities = (0, _vy, 0)
elif action == 'DOWN':
_set_speed = True
_velocities = (0, -_vy, 0)
elif action == 'RIGHT':
_set_speed = True
_velocities = (_vx, 0, 0)
elif action == 'LEFT':
_set_speed = True
_velocities = (-_vx, 0, 0)
elif action == 'SPIN_CW':
_set_speed = True
_velocities = (0, 0, _w)
elif action == 'SPIN_CCW':
_set_speed = True
_velocities = (0, 0, -_w)
elif action == 'SET_HOME':
Odometry.init()
elif action == 'GO_HOME':
_motion_timer.stop()
motion.stop()
time.sleep(1)
_go_home()
time.sleep(1)
_set_speed = True
_velocities = (0, 0, 0)
_motion_timer.start()
elif action == 'GO_TO_POINT':
_toggle_timers(False)
motion.stop()
time.sleep(1)
_ask_for_point()
time.sleep(1)
_ctrl_on = True
_odom_on = True
_toggle_timers(True)
elif action == 'TOGGLE_CNTRL':
_ctrl_on = not _ctrl_on
print("Controller: {}".format(_ctrl_on))
elif action == 'TOGGLE_SMOOTH':
_smooth = not _smooth
print("Smooth: {}".format(_smooth))
elif action == 'TOGGLE_ODOM':
_odom_on = not _odom_on
print("Odom: {}".format(_odom_on))
elif action == 'BATTERY':
print("\n\r*** Battery: {} ***\n\r".format(get_battery()))
elif action == 'BREAKPOINT':
_toggle_timers(False)
import ipdb; ipdb.set_trace()
_toggle_timers(True)
elif action == 'CALIBRATION_MODE':
_toggle_timers(False)
_deal_with_calibration()
time.sleep(1)
_toggle_timers(True)
elif action == 'DIE':
_toggle_timers(False)
motion.stop()
w.kill()
return sys.exit(0)
else:
_set_speed = True
_velocities = (0, 0, 0)
# handle stopping before changing directions
if _action_requires_stop(action):
_set_speed = False
motion.stop()
time.sleep(0.4)
_set_speed = True
_previous_action = action
print "{}\r".format(_velocities)
def init():
w.init(set_PID=False)
def calibrate(speed=48, sleep_time=2, set_PID=True):
speedM1Forward = 0
speedM1Backward = 0
speedM2Forward = 0
speedM2Backward = 0
speedM3Forward = 0
speedM3Backward = 0
# Should I use rcv3 or rcv5?
use_rcv3 = os.environ['USE_RCV3'] == 'true'
# Initialize wheelbase with PID
w.init(use_rcv3=use_rcv3, set_PID=False)
global wheelbase_configured
wheelbase_configured = True
w.kill()
_print_stats()
speedM1_f = 60
speedM1_b = 50
speedM2_f = 55
speedM2_b = 55
speedM3_f = 55
speedM3_b = 55
# Forward (at 0 degrees)
w.Backward(w.M1, speedM1_b) # M1 backward sample 1
w.Forward(w.M3, speedM3_f) # M3 forward sample 1
time.sleep(sleep_time)
speedM1Backward = speedM1Backward + _read(w.M1)
speedM3Forward = speedM3Forward + _read(w.M3)
w.kill()
time.sleep(1)
# Backward (at 180 degrees)
w.Forward(w.M1, speedM1_f) # M1 forward sample 1
w.Backward(w.M3, speedM3_b) # M3 backward sample 1
time.sleep(sleep_time)
speedM1Forward = speedM1Forward + _read(w.M1)
speedM3Backward = speedM3Backward + _read(w.M3)
w.kill()
time.sleep(1)
# Right back (at -120 degrees)
w.Backward(w.M3, speedM3_b) # M3 backward sample 2
w.Forward(w.M2, speedM2_f) # M2 forward sample 1
time.sleep(sleep_time)
speedM3Backward = speedM3Backward + _read(w.M3)
# speedM3Backward = speedM3Backward/2
speedM2Forward = speedM2Forward + _read(w.M2)
w.kill()
time.sleep(1)
# Right forward (at 120 degrees)
w.Forward(w.M3, speedM3_f) # M3 forward sample 2
w.Backward(w.M2, speedM2_b) # M2 backward sample 1
time.sleep(sleep_time)
speedM3Forward = speedM3Forward + _read(w.M3)
# speedM3Forward = speedM3Forward/2
speedM2Backward = speedM2Backward + _read(w.M2)
w.kill()
time.sleep(1)
# Left Back (at 120 degrees)
w.Forward(w.M1, speedM1_f) # M1 forward sample 2
w.Backward(w.M2, speedM2_b) # M2 backward sample 2
time.sleep(sleep_time)
speedM1Forward = speedM1Forward + _read(w.M1)
# speedM1Forward = speedM1Forward/2
speedM2Backward = speedM2Backward + _read(w.M2)
# speedM2Backward = speedM2Backward/2
w.kill()
time.sleep(1)
# Left Forward (at -120 degrees)
w.Backward(w.M1, speedM1_b) # M1 backward sample 2
w.Forward(w.M2, speedM2_f) # M2 forward sample 2
time.sleep(sleep_time)
speedM1Backward = speedM1Backward + _read(w.M1)
# speedM1Backward = speedM1Backward/2
speedM2Forward = speedM2Forward + _read(w.M2)
# speedM2Forward = speedM2Forward/2
w.kill()
speedM1Forward = (speedM1Forward * MAX_PWM) / speedM1_f
speedM1Backward = (speedM1Backward * MAX_PWM) / speedM1_b
speedM2Forward = (speedM2Forward * MAX_PWM) / speedM2_f
speedM2Backward = (speedM2Backward * MAX_PWM) / speedM2_b
speedM3Forward = (speedM3Forward * MAX_PWM) / speedM3_f
speedM3Backward = (speedM3Backward * MAX_PWM) / speedM3_b
global qppsM1
global qppsM2
global qppsM3
qppsM1 = (speedM1Forward + abs(speedM1Backward)) / 2
qppsM2 = (speedM2Forward + abs(speedM2Backward)) / 2
qppsM3 = (speedM3Forward + abs(speedM3Backward)) / 2
print "M1QPPS={},M2QPPS={},M3QPPS={}".format(qppsM1, qppsM2, qppsM3)
if set_PID:
w.SetVelocityPID(w.M1, kp, ki, kd, qppsM1)
w.SetVelocityPID(w.M2, kp, ki, kd, qppsM2)
w.SetVelocityPID(w.M3, kp, ki, kd, qppsM3)
_print_stats()
def test_calibration(velocity=0.6,
sleep_time=1.5,
M1QPPS=None,
M2QPPS=None,
M3QPPS=None):
global wheelbase_configured
# Make sure everything is init'd
if not wheelbase_configured:
w.init()
wheelbase_configured = True
_m1qpps = M1QPPS if M1QPPS is not None else qppsM1
_m2qpps = M2QPPS if M2QPPS is not None else qppsM2
_m3qpps = M3QPPS if M3QPPS is not None else qppsM3
if _m1qpps is not None and _m2qpps is not None and _m3qpps is not None:
w.UpdateVelocityPID(w.M1, q=_m1qpps)
w.UpdateVelocityPID(w.M2, q=_m2qpps)
w.UpdateVelocityPID(w.M3, q=_m3qpps)
_print_stats()
print("velocity: {}\r".format(velocity))
# Forward
motion.drive(velocity, 0, 0)
time.sleep(sleep_time)
motion.stop()
time.sleep(sleep_time)
# Backward
motion.drive(-velocity, 0, 0)
time.sleep(sleep_time)
motion.stop()
time.sleep(sleep_time)
# Left back
motion.drive(velocity * np.cos(2 * np.pi / 3),
velocity * np.sin(2 * np.pi / 3), 0)
time.sleep(sleep_time)
motion.stop()
time.sleep(sleep_time)
# Right forward
motion.drive(-np.cos(2 * np.pi / 3) * velocity,
-np.sin(2 * np.pi / 3) * velocity, 0)
time.sleep(sleep_time)
motion.stop()
time.sleep(sleep_time)
# Right back
motion.drive(
np.cos(-2 * np.pi / 3) * velocity,
np.sin(-2 * np.pi / 3) * velocity, 0)
time.sleep(sleep_time)
motion.stop()
time.sleep(sleep_time)
# Right Forward
motion.drive(-np.cos(-2 * np.pi / 3) * velocity,
-np.sin(-2 * np.pi / 3) * velocity, 0)
time.sleep(sleep_time)
motion.stop()
def calibrate(speed=48, sleep_time=2, set_PID=True):
speedM1Forward = 0
speedM1Backward = 0
speedM2Forward = 0
speedM2Backward = 0
speedM3Forward = 0
speedM3Backward = 0
# Should I use rcv3 or rcv5?
use_rcv3 = os.environ["USE_RCV3"] == "true"
# Initialize wheelbase with PID
w.init(use_rcv3=use_rcv3, set_PID=False)
global wheelbase_configured
wheelbase_configured = True
w.kill()
_print_stats()
speedM1_f = 60
speedM1_b = 50
speedM2_f = 55
speedM2_b = 55
speedM3_f = 55
speedM3_b = 55
# Forward (at 0 degrees)
w.Backward(w.M1, speedM1_b) # M1 backward sample 1
w.Forward(w.M3, speedM3_f) # M3 forward sample 1
time.sleep(sleep_time)
speedM1Backward = speedM1Backward + _read(w.M1)
speedM3Forward = speedM3Forward + _read(w.M3)
w.kill()
time.sleep(1)
# Backward (at 180 degrees)
w.Forward(w.M1, speedM1_f) # M1 forward sample 1
w.Backward(w.M3, speedM3_b) # M3 backward sample 1
time.sleep(sleep_time)
speedM1Forward = speedM1Forward + _read(w.M1)
speedM3Backward = speedM3Backward + _read(w.M3)
w.kill()
time.sleep(1)
# Right back (at -120 degrees)
w.Backward(w.M3, speedM3_b) # M3 backward sample 2
w.Forward(w.M2, speedM2_f) # M2 forward sample 1
time.sleep(sleep_time)
speedM3Backward = speedM3Backward + _read(w.M3)
# speedM3Backward = speedM3Backward/2
speedM2Forward = speedM2Forward + _read(w.M2)
w.kill()
time.sleep(1)
# Right forward (at 120 degrees)
w.Forward(w.M3, speedM3_f) # M3 forward sample 2
w.Backward(w.M2, speedM2_b) # M2 backward sample 1
time.sleep(sleep_time)
speedM3Forward = speedM3Forward + _read(w.M3)
# speedM3Forward = speedM3Forward/2
speedM2Backward = speedM2Backward + _read(w.M2)
w.kill()
time.sleep(1)
# Left Back (at 120 degrees)
w.Forward(w.M1, speedM1_f) # M1 forward sample 2
w.Backward(w.M2, speedM2_b) # M2 backward sample 2
time.sleep(sleep_time)
speedM1Forward = speedM1Forward + _read(w.M1)
# speedM1Forward = speedM1Forward/2
speedM2Backward = speedM2Backward + _read(w.M2)
# speedM2Backward = speedM2Backward/2
w.kill()
time.sleep(1)
# Left Forward (at -120 degrees)
w.Backward(w.M1, speedM1_b) # M1 backward sample 2
w.Forward(w.M2, speedM2_f) # M2 forward sample 2
time.sleep(sleep_time)
speedM1Backward = speedM1Backward + _read(w.M1)
# speedM1Backward = speedM1Backward/2
speedM2Forward = speedM2Forward + _read(w.M2)
# speedM2Forward = speedM2Forward/2
w.kill()
speedM1Forward = (speedM1Forward * MAX_PWM) / speedM1_f
speedM1Backward = (speedM1Backward * MAX_PWM) / speedM1_b
speedM2Forward = (speedM2Forward * MAX_PWM) / speedM2_f
speedM2Backward = (speedM2Backward * MAX_PWM) / speedM2_b
speedM3Forward = (speedM3Forward * MAX_PWM) / speedM3_f
speedM3Backward = (speedM3Backward * MAX_PWM) / speedM3_b
global qppsM1
global qppsM2
global qppsM3
qppsM1 = (speedM1Forward + abs(speedM1Backward)) / 2
qppsM2 = (speedM2Forward + abs(speedM2Backward)) / 2
qppsM3 = (speedM3Forward + abs(speedM3Backward)) / 2
print "M1QPPS={},M2QPPS={},M3QPPS={}".format(qppsM1, qppsM2, qppsM3)
if set_PID:
w.SetVelocityPID(w.M1, kp, ki, kd, qppsM1)
w.SetVelocityPID(w.M2, kp, ki, kd, qppsM2)
w.SetVelocityPID(w.M3, kp, ki, kd, qppsM3)
_print_stats()
def test_calibration(velocity=0.6, sleep_time=1.5, M1QPPS=None, M2QPPS=None, M3QPPS=None):
global wheelbase_configured
# Make sure everything is init'd
if not wheelbase_configured:
w.init()
wheelbase_configured = True
_m1qpps = M1QPPS if M1QPPS is not None else qppsM1
_m2qpps = M2QPPS if M2QPPS is not None else qppsM2
_m3qpps = M3QPPS if M3QPPS is not None else qppsM3
if _m1qpps is not None and _m2qpps is not None and _m3qpps is not None:
w.UpdateVelocityPID(w.M1, q=_m1qpps)
w.UpdateVelocityPID(w.M2, q=_m2qpps)
w.UpdateVelocityPID(w.M3, q=_m3qpps)
_print_stats()
print ("velocity: {}\r".format(velocity))
# Forward
motion.drive(velocity, 0, 0)
time.sleep(sleep_time)
motion.stop()
time.sleep(sleep_time)
# Backward
motion.drive(-velocity, 0, 0)
time.sleep(sleep_time)
motion.stop()
time.sleep(sleep_time)
# Left back
motion.drive(velocity * np.cos(2 * np.pi / 3), velocity * np.sin(2 * np.pi / 3), 0)
time.sleep(sleep_time)
motion.stop()
time.sleep(sleep_time)
# Right forward
motion.drive(-np.cos(2 * np.pi / 3) * velocity, -np.sin(2 * np.pi / 3) * velocity, 0)
time.sleep(sleep_time)
motion.stop()
time.sleep(sleep_time)
# Right back
motion.drive(np.cos(-2 * np.pi / 3) * velocity, np.sin(-2 * np.pi / 3) * velocity, 0)
time.sleep(sleep_time)
motion.stop()
time.sleep(sleep_time)
# Right Forward
motion.drive(-np.cos(-2 * np.pi / 3) * velocity, -np.sin(-2 * np.pi / 3) * velocity, 0)
time.sleep(sleep_time)
motion.stop()
def init():
w.init(set_PID=False)