def drive_to_target(self, spd=None):
"""Drive forward self.drive_dist toward target,
updating self.posn incrementally along the way."""
if not spd:
spd = CARSPEED
# instantiate PID steering
target = int(car.heading)
pid = PID(target)
car.reset_odometer()
print(f"Odometer offset = {car.ODOMETER_OFFSET}")
print(f"First odo reading after reset = {car.odometer}")
prev_dist = 0
waypoints = []
self.log.addline(f"Distance to target: {self.drive_dist:.1f} cm.")
dist_to_go = self.drive_dist
print(f"Distance to go: {dist_to_go}")
wx, wy = self.posn
while dist_to_go > self.COAST_DIST and self.y_min < wy < self.y_max:
car.go(spd, FWD, spin=pid.trim())
# Update self.posn incrementally
curr_dist = car.odometer
incr_dist = curr_dist - prev_dist
prev_dist = curr_dist
hdg = -car.heading
dx, dy = geo.p2r(incr_dist, hdg)
wx, wy = self.posn
self.posn = (wx + dx, wy + dy)
waypoints.append(self.posn)
odo = car.odometer
dist_to_go = self.drive_dist - odo
print(f"Odometer: {odo}")
car.stop_wheels()
time.sleep(0.5)
curr_dist = car.odometer
incr_dist = curr_dist - prev_dist
dx, dy = geo.p2r(incr_dist, hdg)
wx, wy = self.posn
self.posn = (wx + dx, wy + dy)
waypoints.append(self.posn)
self.log.addline(f"Distance driven: {car.odometer} cm.")
# Just plot every third waypoint
waypoints = [
waypoint for idx, waypoint in enumerate(waypoints) if not idx % 3
]
self.waypoints.extend(waypoints)
self.log.addline(f"Heading after drive: {car.heading} deg.")
self.log.addline(f"Ending Coords: {integerize(self.posn)}")
def auto_detect_open_sector(self):
""" Under development...
First find average dist value (not == -3).
Then look for sectors at radius = 1.5 * average.
Put target at mid-anlge at radius/2.
Convert to (x, y) coords and save as self.target
so that map can access it.
"""
# Find average (non-zero) dist value
rvals = [
point.get('dist') for point in self.points
if point.get('dist') != 3
]
avgdist = sum(rvals) / len(rvals)
# make radius somewhat larger
radius = avgdist * 1.5
print(f"radius: {int(radius)}")
sectors = self.open_sectors(radius)
print(sectors)
# Find first sector of reaonable width
for sector in sectors:
angle0, angle1 = sector
if (angle0 - angle1) > 12:
target_angle = (angle0 + angle1) / 2
target_coords = geo.p2r(radius / 2, target_angle)
self.target = target_coords
break
def next_target_point(self):
"""Find next target point by analyzing scan data for openings."""
# Find average (non-zero) dist value
rvals = [point.get('dist')
for point in self.points
if point.get('dist') != -VLEG]
avgdist = int(sum(rvals)/len(rvals))
dist = avgdist
points = [point.get('xy') for point in self.points
if point.get('dist') != -VLEG]
mid_angles = pathfwd.best_paths(points, dist)
# convert last angle in list to xy coords in car coord sys
theta = mid_angles[-1] - 90
target_pnt = geo.p2r(dist, theta)
return target_pnt
def go(self, speed, angle, spin=0):
"""Drive at speed (int) in relative direction angle (degrees)
while simultaneoulsy spinning CCW at rate = spin (int).
Return distances: [front_dist, left_dist, right_dist]"""
# convert from polar coordinates to omni_car's 'natural' coords
# where one coaxial pair of wheels drives u and the other drives v
u, v = geo.p2r(speed, angle - 45)
# motor numbers
m1, m2, m3, m4 = (1, 2, 3, 4)
# combine motor speed with spin
m1spd = int(spin + u)
m2spd = int(spin - u)
m3spd = int(spin - v)
m4spd = int(spin + v)
# friction keeps motors from running smoothly at speeds below ~50
if 0 < m1spd < 50:
m1spd = 50
if 0 < m2spd < 50:
m2spd = 50
if 0 < m3spd < 50:
m3spd = 50
if 0 < m4spd < 50:
m4spd = 50
if -50 < m1spd < 0:
m1spd = -50
if -50 < m2spd < 0:
m2spd = -50
if -50 < m3spd < 0:
m3spd = -50
if -50 < m4spd < 0:
m4spd = -50
distances = self._xfer_data((1, m1spd, m2spd, m3spd, m4spd, 0))
return distances
def R_xform(pnt, angle):
"""Transform point by rotation angle (degrees) CW about the origin."""
r, theta = geo.r2p(pnt)
newpnt = geo.p2r(r, theta + angle)
return newpnt