def getAvgSpeed(motor_id,speed): # Stop all motors and reset encoder counts w.kill() w.ResetEncoders(motor_id) # Move forward w.Forward(motor_id, speed) # Let the motor ramp up and get stable time.sleep(1) # how many samples should I get? #N = 10 # how many seconds to sample over? Tmax = 5 Ts = 0.100 # 100 ms, sample rate # Based on that, how many samples will I be getting? N = int(math.floor(Tmax/Ts)) samples = [] while len(samples)<N: samples.append(s(addr)[1]) time.sleep(Ts) avg = sum(samples)/N # Stop motors w.kill() return (avg,N)
def getAvgSpeed(motor_id, speed):
# Stop all motors and reset encoder counts
w.kill()
w.ResetEncoders(motor_id)
# Move forward
w.Forward(motor_id, speed)
# Let the motor ramp up and get stable
time.sleep(1)
# how many samples should I get?
#N = 10
# how many seconds to sample over?
Tmax = 5
Ts = 0.100 # 100 ms, sample rate
# Based on that, how many samples will I be getting?
N = int(math.floor(Tmax / Ts))
samples = []
while len(samples) < N:
samples.append(s(addr)[1])
time.sleep(Ts)
avg = sum(samples) / N
# Stop motors
w.kill()
return (avg, N)
def kill():
"""Kill
Call this only if you don't care about how the bot stops
"""
w.kill()
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 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 kill():
"""Kill
Call this only if you don't care about how the bot stops
"""
w.kill()
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()