Pulling for project

examples/dynamic_2d_map.py

@@ -0,0 +1,68 @@
+import math
+import matplotlib.pyplot as plt
+from rplidar import RPLidar
+
+# LIDAR setup
+PORT_NAME = '/dev/ttyUSB0'
+lidar = RPLidar(PORT_NAME)
+
+# Parameters for dynamic mapping
+DMAX = 4000  # Maximum distance in mm to include points
+POINT_SIZE = 1  # Size of points in the plot
+
+# Functions
+def polar_to_cartesian(angle_deg, distance_mm):
+    """Convert polar coordinates to Cartesian coordinates."""
+    angle_rad = math.radians(angle_deg)
+    x = distance_mm * math.cos(angle_rad)
+    y = distance_mm * math.sin(angle_rad)
+    return x, y
+
+def update_map(lidar, ax, scatter):
+    """Fetch data from LIDAR and update the map dynamically."""
+    points = []
+    for scan in lidar.iter_scans():
+        for _, angle, distance in scan:
+            if distance > 0 and distance <= DMAX:  # Filter out invalid or out-of-range points
+                x, y = polar_to_cartesian(angle, distance)
+                points.append((x, y))
+
+        # Update scatter plot
+        if points:
+            x_coords = [p[0] for p in points]
+            y_coords = [p[1] for p in points]
+            scatter.set_offsets(list(zip(x_coords, y_coords)))
+            plt.pause(0.01)  # Adjust for smoother animation
+
+        points.clear()  # Clear points for the next frame
+
+# Main execution
+def main():
+    try:
+        print("Starting LIDAR...")
+        lidar.connect()
+
+        # Set up the plot
+        plt.ion()
+        fig, ax = plt.subplots(figsize=(10, 10))
+        ax.set_title("Dynamic 2D Mapping")
+        ax.set_xlim(-DMAX, DMAX)
+        ax.set_ylim(-DMAX, DMAX)
+        ax.set_xlabel("X (mm)")
+        ax.set_ylabel("Y (mm)")
+        ax.grid(True)
+        scatter = ax.scatter([], [], s=POINT_SIZE)
+
+        # Dynamic mapping
+        update_map(lidar, ax, scatter)
+
+    except KeyboardInterrupt:
+        print("Stopping LIDAR...")
+
+    finally:
+        lidar.stop()
+        lidar.disconnect()
+        print("LIDAR stopped.")
+
+if __name__ == "__main__":
+    main()

examples/dynamic_mapping_motion.py

@@ -0,0 +1,144 @@
+import math
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
+from rplidar import RPLidar
+import serial
+import time
+import rplidar
+import threading
+
+# LIDAR and IMU setup
+LIDAR_PORT = '/dev/ttyUSB0'
+IMU_PORT = '/dev/ttyACM0'
+IMU_BAUDRATE = 9600
+
+lidar = RPLidar(LIDAR_PORT)
+imu_serial = serial.Serial(IMU_PORT, IMU_BAUDRATE, timeout=1)
+
+# Parameters
+DMAX = 4000  # Maximum distance in mm to include points
+POINT_SIZE = 1  # Size of points in the plot
+
+# Global map for 3D points
+global_map = []
+
+# Functions
+def polar_to_cartesian(angle_deg, distance_mm):
+    """Convert polar coordinates to Cartesian coordinates."""
+    angle_rad = math.radians(angle_deg)
+    x = distance_mm * math.cos(angle_rad)
+    y = distance_mm * math.sin(angle_rad)
+    return x, y
+
+def get_tilt_angle():
+    """Fetch tilt angle (pitch) from IMU."""
+    try:
+        line = imu_serial.readline().decode('utf-8').strip()
+        if line.startswith("Accel:"):  # Clean up prefix if present
+            line = line.replace("Accel: ", "").strip()
+        if line.count(",") == 2:  # Ensure correct format
+            ax, ay, gz = map(float, line.split(","))
+        else:
+            return 0.0  # Default to no tilt on malformed data
+        # Use gyroscope Z-axis (gz) for tilt angle (pitch)
+        time.sleep(0.05)  # Adjust delay as needed
+        return gz  # In degrees or radians as needed
+    except Exception as e:
+        print(f"Error reading IMU data: {e}")
+        return 0.0
+
+def transform_to_3d(local_points, tilt_angle):
+    """Transform 2D LIDAR points to 3D based on tilt angle."""
+    global_points = []
+    tilt_rad = math.radians(tilt_angle)
+    for x, y in local_points:
+        z = y * math.sin(tilt_rad)  # Project y-axis distance to z-axis
+        y_proj = y * math.cos(tilt_rad)  # Adjust y-axis for tilt
+        global_points.append((x, y_proj, z))
+    return global_points
+
+def update_map(lidar, ax, scatter):
+    global global_map
+    for scan in lidar.iter_scans():
+        start_time = time.time()
+
+        # LIDAR data collection
+        local_points = []
+        for _, angle, distance in scan:
+            if distance > 0 and distance <= DMAX:  # Filter out invalid or out-of-range points
+                x, y = polar_to_cartesian(angle, distance)
+                local_points.append((x, y))
+
+        # IMU data collection
+        tilt_angle = get_tilt_angle()
+
+        # Transform to 3D points
+        global_points = transform_to_3d(local_points, tilt_angle)
+        global_map.extend(global_points)
+
+        # Update scatter plot
+        if global_map:
+            x_coords = [p[0] for p in global_map]
+            y_coords = [p[1] for p in global_map]
+            z_coords = [p[2] for p in global_map]
+            scatter._offsets3d = (x_coords, y_coords, z_coords)
+
+        # Maintain consistent timing
+        elapsed = time.time() - start_time
+        time.sleep(max(0, 0.1 - elapsed))  # Adjust interval as needed
+        plt.pause(0.01)  # Adjust for smoother animation
+
+# Main execution
+def main():
+    try:
+        print("Starting LIDAR and IMU...")
+
+        # Clear LIDAR buffer
+        lidar.stop()
+        lidar.disconnect()
+        lidar.connect()
+        lidar.clean_input()
+
+        lidar._serial.reset_input_buffer()  # Clear any old data
+        imu_serial.reset_input_buffer()
+
+
+        # Clear IMU buffer
+        imu_serial.reset_input_buffer()  # Clear IMU input buffer
+        imu_serial.reset_output_buffer()  # Clear IMU output buffer
+
+        # Start LIDAR motor
+        lidar.start_motor()
+        lidar._set_pwm(60)
+        lidar.clean_input
+        print(lidar.motor_speed)# Ensure the motor is running
+  # Reduce from default 660 RPM to 200 RPM
+
+        # Set up the plot
+        plt.ion()
+        fig = plt.figure(figsize=(10, 10))
+        ax = fig.add_subplot(111, projection='3d')
+        ax.set_title("3D Mapping with Tilted LIDAR")
+        ax.set_xlim(-DMAX, DMAX)
+        ax.set_ylim(-DMAX, DMAX)
+        ax.set_zlim(-DMAX, DMAX)
+        ax.set_xlabel("X (mm)")
+        ax.set_ylabel("Y (mm)")
+        ax.set_zlabel("Z (mm)")
+        scatter = ax.scatter([], [], [], s=POINT_SIZE)
+
+        # Begin dynamic mapping
+        update_map(lidar, ax, scatter)
+
+    except KeyboardInterrupt:
+        print("Stopping LIDAR and IMU...")
+
+    finally:
+        lidar.stop()
+        lidar.disconnect()
+        imu_serial.close()
+        print("LIDAR and IMU stopped")
+
+
+if __name__ == "__main__":
+    main()

examples/generate_2d_map.py

@@ -0,0 +1,52 @@
+import math
+import matplotlib.pyplot as plt
+
+# File containing raw data (output from record_measurements.py)
+DATA_FILE = "output.txt"
+
+def polar_to_cartesian(angle_deg, distance_mm):
+    """Convert polar coordinates to Cartesian coordinates."""
+    angle_rad = math.radians(angle_deg)
+    x = distance_mm * math.cos(angle_rad)
+    y = distance_mm * math.sin(angle_rad)
+    return x, y
+
+def load_data(file_path):
+    """Load raw LIDAR data from the file."""
+    points = []
+    with open(file_path, "r") as f:
+        for line in f:
+            parts = line.split()
+            if len(parts) != 4:
+                continue  # Skip invalid lines
+
+            _, quality, angle, distance = parts
+            quality = int(quality)
+            angle = float(angle)
+            distance = float(distance)
+
+            # Filter out invalid or low-quality data
+            if distance > 0 and quality > 0:
+                x, y = polar_to_cartesian(angle, distance)
+                points.append((x, y))
+    return points
+
+def plot_map(points):
+    """Plot the 2D map using the processed points."""
+    x_coords = [p[0] for p in points]
+    y_coords = [p[1] for p in points]
+    plt.figure(figsize=(10, 10))
+    plt.scatter(x_coords, y_coords, s=1)
+    plt.title("2D Map from LIDAR Data")
+    plt.xlabel("X (mm)")
+    plt.ylabel("Y (mm)")
+    plt.axis("equal")
+    plt.grid(True)
+    plt.show()
+
+if __name__ == "__main__":
+    # Load and process LIDAR data
+    points = load_data(DATA_FILE)
+
+    # Plot the 2D map
+    plot_map(points)

examples/read_imu_serial.py

@@ -0,0 +1,29 @@
+import serial
+
+def read_imu_data(port='/dev/ttyACM0', baudrate=9600):
+    """Reads IMU data from Arduino via Serial."""
+    ser = serial.Serial(port, baudrate, timeout=1)
+    try:
+        while True:
+            line = ser.readline().decode('utf-8').strip()  # Read and clean data
+            print(f"Raw data: {line}")  # Debug: Print raw data
+            if line.startswith("Accel:"):  # Handle lines with 'Accel:' prefix
+                line = line.replace("Accel: ", "").strip()  # Remove the prefix
+
+            if line.count(",") == 2:  # Ensure correct format (two commas)
+                try:
+                    ax, ay, gz = map(float, line.split(","))
+                    print(f"AccelX: {ax}, AccelY: {ay}, GyroZ: {gz}")
+                except ValueError:
+                    print("Malformed data, skipping...")
+            else:
+                print("Incomplete or malformed data, skipping...")
+    except KeyboardInterrupt:
+        print("Stopping IMU read...")
+    finally:
+        ser.close()
+
+
+
+if __name__ == "__main__":
+    read_imu_data()

examples/record_measurments.py

@@ -15,7 +15,7 @@ def run(path):
     outfile = open(path, 'w')
     try:
         print('Recording measurments... Press Crl+C to stop.')
-        for measurment in lidar.iter_measurments():
+        for measurment in lidar.iter_measures():
             line = '\t'.join(str(v) for v in measurment)
             outfile.write(line + '\n')
     except KeyboardInterrupt:

rplidar.py

@@ -55,7 +55,7 @@
 
 # Constants & Command to start A2 motor
 MAX_MOTOR_PWM = 1023
-DEFAULT_MOTOR_PWM = 660
+DEFAULT_MOTOR_PWM = 60
 SET_PWM_BYTE = b'\xF0'
 
 _HEALTH_STATUSES = {
@@ -157,6 +157,12 @@ def disconnect(self):
     def _set_pwm(self, pwm):
         payload = struct.pack("<H", pwm)
         self._send_payload_cmd(SET_PWM_BYTE, payload)
+
+    def set_motor_speed(self, pwm):
+        '''Sets the motor speed using PWM (0 to 1023).'''
+        if not (0 <= pwm <= MAX_MOTOR_PWM):
+            raise ValueError(f"PWM value must be between 0 and {MAX_MOTOR_PWM}.")
+        self._set_pwm(pwm)
 
     @property
     def motor_speed(self):
@@ -355,7 +361,7 @@ def reset(self):
         time.sleep(2)
         self.clean_input()
 
-    def iter_measures(self, scan_type='normal', max_buf_meas=3000):
+    def iter_measures(self, scan_type='normal', max_buf_meas=10000):
         '''Iterate over measures. Note that consumer must be fast enough,
         otherwise data will be accumulated inside buffer and consumer will get
         data with increasing lag.