...

Author: CursedPrograms

scripts/line_follower.py

@@ -1,4 +1,4 @@
- import RPi.GPIO as GPIO
+from gpiozero import LineSensor
 import time
 
 # Define pin numbers
@@ -31,3 +31,102 @@
 
 finally:
     GPIO.cleanup()
+
+class LineFollower:
+    def __init__(self, left_pin=17, right_pin=27, motors=None):
+        self.left_sensor  = LineSensor(left_pin)
+        self.right_sensor = LineSensor(right_pin)
+        self.motors = motors
+
+        # Speed tuning
+        self.base_speed = 0.45
+        self.turn_speed = 0.35
+        self.max_speed  = 0.6
+
+        # State
+        self.last_direction = "forward"
+        self.lost_count = 0
+
+        # Recovery system (like your obstacle avoider)
+        self.recovery_mode = False
+        self.recovery_direction = None
+        self.recovery_start = 0
+        self.recovery_time = 1.5
+
+    def read_sensors(self):
+        return self.left_sensor.value, self.right_sensor.value
+
+    def _start_recovery(self):
+        self.recovery_mode = True
+        self.recovery_start = time.time()
+
+        # Commit to last known direction
+        if self.last_direction == "left":
+            self.recovery_direction = "left"
+        elif self.last_direction == "right":
+            self.recovery_direction = "right"
+        else:
+            self.recovery_direction = "left"
+
+        print(f"🔄 LINE LOST — recovering {self.recovery_direction.upper()}")
+
+    def _handle_recovery(self):
+        if time.time() - self.recovery_start > self.recovery_time:
+            self.recovery_mode = False
+            self.lost_count = 0
+            print("✅ Line reacquire timeout")
+            return False
+
+        if self.recovery_direction == "left":
+            self.motors.turn_left(self.turn_speed)
+        else:
+            self.motors.turn_right(self.turn_speed)
+
+        return True
+
+    def follow_line(self):
+        if self.motors is None:
+            return False
+
+        left, right = self.read_sensors()
+
+        # 🧠 RECOVERY MODE
+        if self.recovery_mode:
+            return self._handle_recovery()
+
+        # 🟢 ON TRACK
+        if left and right:
+            self.motors.forward(self.max_speed)
+            self.last_direction = "forward"
+            self.lost_count = 0
+            return False
+
+        # ↩️ DRIFT RIGHT → correct LEFT
+        elif not left and right:
+            self.motors.turn_left(self.turn_speed)
+            self.last_direction = "left"
+            self.lost_count = 0
+            return True
+
+        # ↪️ DRIFT LEFT → correct RIGHT
+        elif left and not right:
+            self.motors.turn_right(self.turn_speed)
+            self.last_direction = "right"
+            self.lost_count = 0
+            return True
+
+        # ❌ LINE LOST
+        else:
+            self.lost_count += 1
+
+            if self.lost_count > 5:
+                self._start_recovery()
+
+            self.motors.stop()
+            return True
+
+    def cleanup(self):
+        if self.motors:
+            self.motors.stop()
+        self.left_sensor.close()
+        self.right_sensor.close()

line_test.py scripts/line_test.pyline_test.py renamed to scripts/line_test.py

@@ -1,13 +1,28 @@
 # server.py
+# KIDA Web Control Server (Improved)
 
-from flask import Flask, jsonify, request
+from flask import Flask, jsonify, request, Response
+import threading
+import queue
+import cv2
+from picamera2 import Picamera2
 
 app = Flask(__name__)
+command_queue = queue.Queue()
+
+# --- Camera Setup ---
+picam2 = Picamera2()
+picam2.configure(picam2.create_preview_configuration())
+picam2.start()
 
 # --- Routes ---
 @app.route('/')
 def home():
-    return "Welcome to KIDA's Flask Server!"
+    return """
+    <h1>KIDA Control</h1>
+    <p>Status: Online</p>
+    <img src="/video_feed" width="480">
+    """
 
 @app.route('/status')
 def status():
@@ -19,12 +34,45 @@ def status():
 
 @app.route('/command', methods=['POST'])
 def receive_command():
-    data = request.get_json()
-    command = data.get('command')
-    # Process the command here (e.g., send to robot controller)
-    print(f"📡 Received command: {command}")
-    return jsonify({'received': command, 'status': 'executed'})
+    try:
+        data = request.get_json()
+        command = data.get('command')
+
+        print(f"📡 Received command: {command}")
+        command_queue.put(command)
+
+        return jsonify({'received': command, 'status': 'queued'})
+    except Exception as e:
+        return jsonify({'error': str(e)}), 400
+
+# --- Video Streaming ---
+def generate_frames():
+    while True:
+        frame = picam2.capture_array()
+        frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
+
+        _, buffer = cv2.imencode('.jpg', frame)
+        frame_bytes = buffer.tobytes()
+
+        yield (b'--frame\r\n'
+               b'Content-Type: image/jpeg\r\n\r\n' + frame_bytes + b'\r\n')
+
+@app.route('/video_feed')
+def video_feed():
+    return Response(generate_frames(),
+                    mimetype='multipart/x-mixed-replace; boundary=frame')
+
+# --- Command Processor Thread ---
+def command_worker():
+    while True:
+        cmd = command_queue.get()
+        print(f"🤖 Executing: {cmd}")
+
+        # Hook into your motor system here
+        # Example:
+        # if cmd == "forward": motors.forward()
 
 # --- Start Server ---
 if __name__ == '__main__':
-    app.run(host='0.0.0.0', port=5000, debug=True)
+    threading.Thread(target=command_worker, daemon=True).start()
+    app.run(host='0.0.0.0', port=5000, debug=False)