273 lines
No EOL
11 KiB
Python
273 lines
No EOL
11 KiB
Python
import time
|
|
from logger import get_logger
|
|
|
|
# Constants for state management
|
|
STATE_SEARCHING = 0
|
|
STATE_ALIGNING = 1
|
|
STATE_FOLLOWING = 2
|
|
STATE_RIGHT_TURNING = 3
|
|
STATE_LEFT_TURNING = 4
|
|
|
|
|
|
# alignment parameters
|
|
DESIRED_DISTANCE_FROM_WALL = 250 # mm (200 seems to work very well)
|
|
SIDE_ALIGNMENT_TOLERANCE = 20 # mm (for side distance correction)
|
|
ALIGNMENT_ERROR_THRESHOLD = 50 # mm (for angle correction)
|
|
STRAFE_SPEED = 30 # mm/s
|
|
CORRECTION_AMOUNT = 2 # direction only, gets clamped anyway
|
|
|
|
# Front obstacle detection
|
|
FRONT_OBSTACLE_THRESHOLD = 400 # mm
|
|
CORRECTION_TIME = 1.0 # seconds (alignment timing)
|
|
FORWARD_SPEED = 30 # mm/s
|
|
|
|
# Right turn params
|
|
RIGHT_TURN_THRESHOLD = 500 # mm
|
|
RIGHT_TURN_SPEED = 30 # mm/s
|
|
TURN_RESET_DISTANCE = 500 # mm
|
|
|
|
logger = get_logger("wall_follower")
|
|
|
|
# 💫 Rotation helper using dumb timing
|
|
def rotate_degrees(chassis, degrees):
|
|
DEG_PER_SEC = 90 # confirmed from tuning test
|
|
duration = abs(degrees) / DEG_PER_SEC
|
|
direction = 1 if degrees > 0 else -1
|
|
logger.info(f"🔁 Rotating {degrees}° ({'CW' if direction > 0 else 'CCW'}) for {duration:.2f}s")
|
|
chassis.set_velocity(0, 0, direction)
|
|
time.sleep(duration)
|
|
chassis.reset_motors()
|
|
|
|
def sanitize_reading(val):
|
|
return val if isinstance(val, (int, float)) and val > 0 else 0
|
|
|
|
class WallFollower:
|
|
def __init__(self, chassis):
|
|
self.chassis = chassis
|
|
self.state = STATE_SEARCHING
|
|
self.last_alignment_time = time.time()
|
|
self.realign_interval = 5.0 # seconds between realignment checks
|
|
self._turning_phase_done = False # Used to track turn phases
|
|
self.last_correction_time = 0
|
|
self.alignment_cooldown = 0.3 # seconds
|
|
|
|
def stop(self):
|
|
logger.info("🛑 WallFollower stopping motors.")
|
|
self.chassis.reset_motors()
|
|
|
|
def step(self, scan):
|
|
front = sanitize_reading(scan[0]) #
|
|
side = sanitize_reading(scan[90]) #
|
|
diag_front = sanitize_reading(scan[45]) #
|
|
diag_back = sanitize_reading(scan[135]) #
|
|
|
|
|
|
|
|
if self.state == STATE_SEARCHING:
|
|
self._searching(side)
|
|
elif self.state == STATE_ALIGNING:
|
|
self._aligning(side, diag_front, diag_back)
|
|
elif self.state == STATE_FOLLOWING:
|
|
self._following(front, diag_front, diag_back, side)
|
|
elif self.state == STATE_RIGHT_TURNING:
|
|
self._right_turning(diag_front, side)
|
|
elif self.state == STATE_LEFT_TURNING:
|
|
self._left_turning()
|
|
|
|
|
|
def _searching(self, side_distance):
|
|
logger.info("🔍 SEARCHING for wall...")
|
|
|
|
# Immediately transition to ALIGNING state for development/testing
|
|
logger.info("✅ Skipping search — switching to ALIGNING mode")
|
|
self.state = STATE_ALIGNING
|
|
self.last_alignment_time = time.time()
|
|
|
|
def _aligning(self, side_distance, dist_45, dist_135):
|
|
|
|
logger.info("📐 ALIGNING to wall...")
|
|
|
|
# Safety check
|
|
if side_distance == 0 or dist_45 == 0 or dist_135 == 0:
|
|
logger.warning("⚠️ Invalid LIDAR reading(s). Holding position.")
|
|
#self.chassis.reset_motors()
|
|
return
|
|
|
|
# Calculate errors
|
|
angle_error = dist_45 - dist_135
|
|
distance_error = side_distance - DESIRED_DISTANCE_FROM_WALL
|
|
|
|
# 🔄 Step 1: Correct angle first
|
|
if abs(angle_error) > ALIGNMENT_ERROR_THRESHOLD:
|
|
logger.info(f"🔄 Alignment error: {angle_error:.1f} mm")
|
|
|
|
P_GAIN = 0.1
|
|
correction_degrees = P_GAIN * angle_error
|
|
correction_degrees = max(min(correction_degrees, 2), -2)
|
|
|
|
logger.debug(f"🎯 Rotating by {correction_degrees:.1f}° for alignment")
|
|
rotate_degrees(self.chassis, correction_degrees)
|
|
time.sleep(0.1)
|
|
return
|
|
|
|
# ↔️ Step 2: Then fix distance to wall
|
|
if abs(distance_error) > SIDE_ALIGNMENT_TOLERANCE:
|
|
if distance_error > 0:
|
|
logger.debug("➡️ Strafe Right to correct wall distance")
|
|
logger.debug(f"📏 Distance: {side_distance:.1f} mm")
|
|
logger.debug(f"📏 Distance error: {distance_error:.1f} mm")
|
|
self.chassis.set_velocity(STRAFE_SPEED, 0, 0)
|
|
else:
|
|
logger.debug("⬅️ Strafe Left to correct wall distance")
|
|
self.chassis.set_velocity(-STRAFE_SPEED, 0, 0)
|
|
time.sleep(0.3)
|
|
self.chassis.reset_motors()
|
|
time.sleep(0.2)
|
|
return
|
|
|
|
# ✅ If both are good
|
|
logger.info("✅ Aligned successfully — switching to FOLLOWING mode")
|
|
self.state = STATE_FOLLOWING
|
|
self.last_alignment_time = time.time()
|
|
|
|
|
|
# ✅ If both are good
|
|
logger.info("✅ Aligned successfully — switching to FOLLOWING mode")
|
|
self.state = STATE_FOLLOWING
|
|
self.last_alignment_time = time.time()
|
|
|
|
def _following(self, front_distance, dist_45, dist_135, side):
|
|
|
|
time.sleep(1.0) # Allow LIDAR to settle
|
|
|
|
logger.info("🚧 FOLLOWING state: driving forward...")
|
|
|
|
# 🧭 Check for possible left-hand turn
|
|
if 0 < front_distance < FRONT_OBSTACLE_THRESHOLD:
|
|
logger.info("🧭 Potential left turn detected — entering LEFT_TURNING state")
|
|
logger.debug(f"📏 Front distance: {front_distance:.1f} mm")
|
|
self.state = STATE_LEFT_TURNING
|
|
return
|
|
|
|
# 🔁 Timed angle + distance correction
|
|
if time.time() - self.last_alignment_time > CORRECTION_TIME:
|
|
if dist_45 > 0 and dist_135 > 0 and side > 0:
|
|
angle_error = dist_45 - dist_135
|
|
distance_error = side - DESIRED_DISTANCE_FROM_WALL
|
|
|
|
logger.info(f"🛠️ Periodic corrections | Angle error: {angle_error:.1f} mm | Distance error: {distance_error:.1f} mm")
|
|
logger.debug(f"📏 dist_45 = {dist_45} | dist_135 = {dist_135} | side = {side}")
|
|
|
|
# 📐 Angle correction (rotation)
|
|
if abs(angle_error) > ALIGNMENT_ERROR_THRESHOLD:
|
|
P_GAIN = 0.05
|
|
correction_degrees = max(min(P_GAIN * angle_error, 5), -5)
|
|
logger.debug(f"🎯 FOLLOWING correction: rotating by {correction_degrees:.2f}°")
|
|
rotate_degrees(self.chassis, correction_degrees)
|
|
|
|
# ➡️ Distance correction: strafe right if too far from wall
|
|
if distance_error > SIDE_ALIGNMENT_TOLERANCE:
|
|
logger.debug("📏 FOLLOWING correction: strafing right to re-approach wall")
|
|
self.chassis.set_velocity(STRAFE_SPEED, 0, 0)
|
|
time.sleep(0.2)
|
|
self.chassis.reset_motors()
|
|
|
|
self.last_alignment_time = time.time()
|
|
|
|
# 🧭 Check for possible right-hand turn
|
|
if dist_45 > RIGHT_TURN_THRESHOLD and front_distance > FRONT_OBSTACLE_THRESHOLD:
|
|
logger.info("🧭 Potential right turn detected — entering RIGHT_TURNING state")
|
|
self.state = STATE_RIGHT_TURNING
|
|
self.turn_stage = "checking_forward"
|
|
return
|
|
|
|
#if 0 < front_distance < FRONT_OBSTACLE_THRESHOLD:
|
|
# logger.warning("🛑 Obstacle detected ahead! Stopping.")
|
|
# self.chassis.reset_motors()
|
|
# return
|
|
|
|
# 🚗 Continue forward motion
|
|
logger.debug("🚗 Continuing forward after (or without) correction.")
|
|
self.chassis.set_velocity(FORWARD_SPEED, 90, 0)
|
|
|
|
|
|
|
|
def _right_turning(self, dist_45, dist_90):
|
|
|
|
time.sleep(1.0) # Allow LIDAR to settle
|
|
"""
|
|
State 4: RIGHT_TURNING
|
|
Handles logic for detecting and confirming a 90-degree right-hand turn.
|
|
The robot first reverses until the 45° distance returns to normal,
|
|
then moves forward slowly until both 45° and 90° are large,
|
|
confirming a valid turn. It stops just past the corner.
|
|
"""
|
|
logger.info("↪️ RIGHT_TURNING state: verifying turn...")
|
|
"""
|
|
if self.turn_stage == "backing_up":
|
|
logger.debug("🔙 Backing up slowly to detect return of wall...")
|
|
self.chassis.set_velocity(30, 270, 0) # reverse slowly
|
|
|
|
if 0 < dist_45 < TURN_RESET_DISTANCE:
|
|
logger.info("🧱 Wall detected again at 45° — switching to forward check")
|
|
self.turn_stage = "checking_forward"
|
|
time.sleep(0.5)
|
|
"""
|
|
|
|
"""
|
|
if self.turn_stage == "checking_forward":
|
|
logger.debug("🚶♂️ Moving forward slowly to confirm turn...")
|
|
self.chassis.set_velocity(RIGHT_TURN_SPEED, 90, 0)
|
|
|
|
logger.debug(f"📈 dist_45 = {dist_45} | dist_90 = {dist_90}")
|
|
#if dist_45 > RIGHT_TURN_THRESHOLD and dist_90 > RIGHT_TURN_THRESHOLD:
|
|
if dist_45 > RIGHT_TURN_THRESHOLD:
|
|
logger.info("✅ Verified right-hand turn — initiating reposition sequence")
|
|
"""
|
|
|
|
# Step 1: Drive forward to clear the corner
|
|
self.chassis.set_velocity(RIGHT_TURN_SPEED, 90, 0)
|
|
time.sleep(0.25)
|
|
self.chassis.reset_motors()
|
|
|
|
# Step 2: Rotate 90° to face next wall
|
|
rotate_degrees(self.chassis, 70)
|
|
|
|
# Step 3: Drive forward into new alignment zone
|
|
self.chassis.set_velocity(RIGHT_TURN_SPEED, 90, 0)
|
|
time.sleep(3.0)
|
|
self.chassis.reset_motors()
|
|
|
|
# After repositioning
|
|
time.sleep(2.0) # Let LIDAR settle
|
|
|
|
# Step 4: Switch to ALIGNING
|
|
logger.info("📐 Reposition complete — switching to ALIGNING state")
|
|
self.state = STATE_ALIGNING # was previously set to STATE_ALIGNING
|
|
self.last_alignment_time = time.time()
|
|
|
|
def _left_turning(self):
|
|
time.sleep(1.0) # Allow LIDAR to settle
|
|
"""
|
|
STATE_LEFT_TURNING — reverses, rotates left, then aligns.
|
|
"""
|
|
logger.info("↩️ LEFT_TURNING: initiating left turn sequence")
|
|
|
|
# Step 1: Backup to clear corner
|
|
logger.debug("🔙 Reversing slightly before turning")
|
|
self.chassis.set_velocity(RIGHT_TURN_SPEED, 270, 0)
|
|
time.sleep(1.0)
|
|
self.chassis.reset_motors()
|
|
|
|
# Step 2: Rotate left (CCW) with extra compensation
|
|
logger.info("🔁 Rotating -100° (CCW) to face new wall")
|
|
rotate_degrees(self.chassis, -95)
|
|
|
|
# Step 3: Pause to let LIDAR stabilize
|
|
logger.debug("⏸️ Pausing briefly after turn...")
|
|
time.sleep(2.5)
|
|
|
|
# Step 4: Switch to ALIGNING
|
|
logger.info("📐 Left turn complete — switching to ALIGNING")
|
|
self.state = STATE_FOLLOWING # was previously set to STATE_ALIGNING
|
|
self.last_alignment_time = time.time() |