def calibrateBaseline(scale):
# Compute the robot basline parameter using a range of wheel velocities.
# For each wheel velocity, the robot scale parameter can be computed by
# comparing the time elapsed and rotation completed to the input wheel
# velocities.
wheel_velocities_range = range(15, 60, 5)
delta_times = []
for wheel_vel in wheel_velocities_range:
print("Driving at {} ticks/s.".format(wheel_vel))
# Repeat the test until the correct time is found.
while True:
delta_time = input("Input the time to drive in seconds: ")
try:
delta_time = float(delta_time)
except ValueError:
print("Time must be a number.")
continue
# Spin the robot at the given speed for the given time
ppi.set_velocity(-wheel_vel, wheel_vel, delta_time)
uInput = input("Did the robot spin 360deg?[y/N]")
if uInput == 'y':
delta_times.append(delta_time)
print("Recording that the robot spun 360deg in {:.2f} seconds at wheel speed {}.\n".format(delta_time, wheel_vel))
break
# Once finished driving, compute the basline parameter by averaging
num = len(wheel_velocities_range)
def calibrateWheelRadius():
# Compute the robot scale parameter using a range of wheel velocities.
# For each wheel velocity, the robot scale parameter can be computed
# by comparing the time and distance driven to the input wheel velocities.
wheel_velocities_range = range(15, 55, 5)
delta_times = []
for wheel_vel in wheel_velocities_range:
print("Driving at {} ticks/s.".format(wheel_vel))
# Repeat the test until the correct time is found.
while True:
delta_time = input("Input the time to drive in seconds: ")
try:
delta_time = float(delta_time)
except ValueError:
print("Time must be a number.")
continue
# Drive the robot at the given speed for the given time
ppi.set_velocity(wheel_vel, wheel_vel, delta_time)
uInput = input("Did the robot travel 1m?[y/N]")
if uInput == 'y':
delta_times.append(delta_time)
print(
"Recording that the robot drove 1m in {:.2f} seconds at wheel speed {}.\n"
.format(delta_time, wheel_vel))
break
# Once finished driving, compute the scale parameter by averaging
num = len(wheel_velocities_range)
scale = 0
for delta_time, wheel_vel in zip(delta_times, wheel_velocities_range):
scale += 1 / num * (1 / (wheel_vel * delta_time))
print("The scale parameter is estimated as {:.2f} m/ticks.".format(scale))
return scale
def preprocess_img(img, transform):
img = transform(img)
# add dimension for batch
img = img.unsqueeze(0)
return img
transform = transforms.Compose([
transforms.ToTensor(),
transforms.Normalize((0.5, 0.5, 0.5), (0.5, 0.5, 0.5))
])
net = load_net("steerNet.pt")
#~~~~~~~~~~~~ SET UP ROBOT ~~~~~~~~~~~~~~
ppi.set_velocity(0, 0)
try:
angle = 0
while True:
# read image from camera
image = ppi.get_image()
# resize to fit network
image = cv2.resize(image, (84, 84))
# preprocess
input_img = preprocess_img(image, transform)
# get steering prediction
steer = net(input_img).data.numpy()
print(steer)
