1+ from gpiozero import DistanceSensor
2+ import time
3+
4+ class ObstacleAvoidance :
5+ def __init__ (self , trigger_pin = 27 , echo_pin = 22 , motors = None , threshold = 0.5 ):
6+ # Faster response (no smoothing delay)
7+ self .sensor = DistanceSensor (
8+ echo = echo_pin ,
9+ trigger = trigger_pin ,
10+ max_distance = 2.0 ,
11+ queue_len = 1
12+ )
13+ self .motors = motors
14+ self .threshold = threshold
15+ self .turn_left_next = True
16+
17+ # More realistic speeds for a Pi 3 robot
18+ self .min_speed = 0.3
19+ self .max_speed = 0.6
20+
21+ # Stuck detection
22+ self .obstacle_count = 0
23+ self .last_obstacle_time = 0
24+ self .stuck_threshold = 3 # Number of obstacles in quick succession
25+ self .stuck_time_window = 2.0 # Within 2 seconds
26+ self .committed_direction = None # Will be 'left' or 'right'
27+ self .commitment_timeout = 5.0 # Stay committed for 5 seconds
28+ self .commitment_start_time = 0
29+
30+ def get_distance (self ):
31+ try :
32+ return self .sensor .distance
33+ except Exception :
34+ return None
35+
36+ def _map_speed (self , distance ):
37+ if distance >= self .threshold :
38+ return self .max_speed
39+ if distance <= 0 :
40+ return self .min_speed
41+ ratio = distance / self .threshold
42+ speed = self .min_speed + (self .max_speed - self .min_speed ) * ratio
43+ return max (self .min_speed , min (self .max_speed , speed ))
44+
45+ def _detect_stuck (self ):
46+ """Check if robot is repeatedly hitting obstacles (stuck pattern)"""
47+ current_time = time .time ()
48+
49+ # Check if we're still in the time window
50+ if current_time - self .last_obstacle_time < self .stuck_time_window :
51+ self .obstacle_count += 1
52+ else :
53+ # Reset counter if too much time has passed
54+ self .obstacle_count = 1
55+
56+ self .last_obstacle_time = current_time
57+
58+ # If we hit stuck threshold, commit to a direction
59+ if self .obstacle_count >= self .stuck_threshold :
60+ if self .committed_direction is None :
61+ # Choose direction based on current preference
62+ self .committed_direction = 'left' if self .turn_left_next else 'right'
63+ self .commitment_start_time = current_time
64+ print (f"🔄 STUCK DETECTED! Committing to turn { self .committed_direction .upper ()} )
65+ return True
66+ return False
67+
68+ def _should_stay_committed (self ):
69+ """Check if we should still follow committed direction"""
70+ if self .committed_direction is None :
71+ return False
72+
73+ # Release commitment after timeout
74+ if time .time () - self .commitment_start_time > self .commitment_timeout :
75+ print (f"✅ Commitment timeout - resuming normal operation" )
76+ self .committed_direction = None
77+ self .obstacle_count = 0
78+ return False
79+
80+ return True
81+
82+ def check_and_avoid (self ):
83+ if self .motors is None :
84+ return False
85+
86+ distance = self .get_distance ()
87+ if distance is None :
88+ return False
89+
90+ # 🚨 OBSTACLE DETECTED
91+ if distance < self .threshold :
92+ # Check if we're stuck
93+ is_stuck = self ._detect_stuck ()
94+
95+ self .motors .stop ()
96+ time .sleep (0.05 )
97+
98+ # Reverse briefly
99+ self .motors .backward (self .min_speed )
100+ start_time = time .time ()
101+ while time .time () - start_time < 0.3 :
102+ pass
103+ self .motors .stop ()
104+ time .sleep (0.05 )
105+
106+ # Decide turn direction
107+ if self ._should_stay_committed ():
108+ # Stuck mode: always turn the same way
109+ turn_direction = self .committed_direction
110+ # Longer turn when stuck
111+ turn_duration = 0.7
112+ else :
113+ # Normal mode: alternate
114+ turn_direction = 'left' if self .turn_left_next else 'right'
115+ turn_duration = 0.55
116+ self .turn_left_next = not self .turn_left_next
117+
118+ # Execute turn
119+ if turn_direction == 'left' :
120+ self .motors .turn_left (self .min_speed )
121+ else :
122+ self .motors .turn_right (self .min_speed )
123+
124+ start_time = time .time ()
125+ while time .time () - start_time < turn_duration :
126+ pass
127+
128+ self .motors .stop ()
129+ return True
130+
131+ # ✅ PATH CLEAR
132+ else :
133+ # Reset stuck detection if we have clear path
134+ if distance > self .threshold * 1.5 : # Good clearance
135+ if self .obstacle_count > 0 :
136+ self .obstacle_count = max (0 , self .obstacle_count - 1 )
137+
138+ speed = self ._map_speed (distance )
139+ self .motors .forward (speed )
140+ return False
141+
142+ def cleanup (self ):
143+ if self .motors :
144+ self .motors .stop ()
145+ self .sensor .close ()
0 commit comments