Arduino Alvik Maze Navigation - From Wall Following to SLAM

from arduino_alvik import ArduinoAlvik
import time

# Initialize Alvik
alvik = ArduinoAlvik()
alvik.begin()

# Distance thresholds (in cm)
WALL_DISTANCE = 15  # Desired distance from wall
TOO_CLOSE = 10      # Too close to wall
TOO_FAR = 20        # Too far from wall
FRONT_OBSTACLE = 20 # Obstacle ahead

def wall_follow_left():
    """
    Follow the left wall using simple rules
    """
    while True:
        # Read distance sensors
        left = alvik.get_distance_left()
        front = alvik.get_distance_center()
        right = alvik.get_distance_right()

        # Decision logic
        if front < FRONT_OBSTACLE:
            # Wall ahead - turn right
            alvik.rotate(90)

        elif left > TOO_FAR:
            # No wall on left - turn left to find it
            alvik.rotate(-90)
            alvik.drive(10, 0)  # Move forward a bit

        elif left < TOO_CLOSE:
            # Too close to left wall - drift right
            alvik.drive(10, 30)

        else:
            # Perfect distance - go straight
            alvik.drive(10, 0)

        time.sleep(0.1)

# Run the wall following algorithm
try:
    wall_follow_left()
except KeyboardInterrupt:
    alvik.stop()
    print("Stopped by user")

def enhanced_wall_follow():
    """
    Improved wall following with proportional control
    """
    BASE_SPEED = 15
    KP = 2.0  # Proportional gain

    while True:
        left = alvik.get_distance_left()
        front = alvik.get_distance_center()

        # Check for front obstacle
        if front < FRONT_OBSTACLE:
            alvik.stop()
            alvik.rotate(90)
            continue

        # Proportional control for smooth following
        error = WALL_DISTANCE - left
        turn_rate = error * KP

        # Constrain turn rate
        turn_rate = max(-45, min(45, turn_rate))

        # Drive with correction
        alvik.drive(BASE_SPEED, turn_rate)

        time.sleep(0.05)

class MazeNavigator:
    def __init__(self):
        self.alvik = ArduinoAlvik()
        self.alvik.begin()
        self.state = "FOLLOW_WALL"

    def run(self):
        while True:
            if self.state == "FOLLOW_WALL":
                self.follow_wall()
            elif self.state == "TURN_LEFT":
                self.turn_left()
            elif self.state == "TURN_RIGHT":
                self.turn_right()
            elif self.state == "MOVE_FORWARD":
                self.move_forward()

    def follow_wall(self):
        left = self.alvik.get_distance_left()
        front = self.alvik.get_distance_center()

        if front < FRONT_OBSTACLE:
            self.state = "TURN_RIGHT"
        elif left > TOO_FAR:
            self.state = "TURN_LEFT"
        else:
            self.state = "MOVE_FORWARD"

    def turn_left(self):
        self.alvik.rotate(-90)
        self.state = "FOLLOW_WALL"

    def turn_right(self):
        self.alvik.rotate(90)
        self.state = "FOLLOW_WALL"

    def move_forward(self):
        self.alvik.drive(10, 0)
        time.sleep(0.1)
        self.state = "FOLLOW_WALL"

from arduino_alvik import ArduinoAlvik
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist

class AlvikROS2Node(Node):
    def __init__(self):
        super().__init__('alvik_node')

        # Initialize Alvik
        self.alvik = ArduinoAlvik()
        self.alvik.begin()

        # Create publishers
        self.scan_pub = self.create_publisher(
            LaserScan,
            'scan',
            10
        )

        # Create subscribers
        self.cmd_vel_sub = self.create_subscription(
            Twist,
            'cmd_vel',
            self.cmd_vel_callback,
            10
        )

        # Timer for sensor publishing
        self.timer = self.create_timer(0.1, self.publish_sensors)

    def publish_sensors(self):
        """Publish sensor data as LaserScan"""
        scan = LaserScan()
        scan.header.stamp = self.get_clock().now().to_msg()
        scan.header.frame_id = 'laser'

        # Get distance readings
        distances = [
            self.alvik.get_distance_left(),
            self.alvik.get_distance_center(),
            self.alvik.get_distance_right()
        ]

        scan.ranges = distances
        scan.angle_min = -0.785  # -45 degrees
        scan.angle_max = 0.785   # 45 degrees
        scan.angle_increment = 0.785  # 45 degrees
        scan.range_min = 0.05
        scan.range_max = 3.5

        self.scan_pub.publish(scan)

    def cmd_vel_callback(self, msg):
        """Handle movement commands from ROS2"""
        linear = msg.linear.x * 100  # Convert to cm/s
        angular = msg.angular.z * 57.3  # Convert to degrees/s

        self.alvik.drive(linear, angular)

def main():
    rclpy.init()
    node = AlvikROS2Node()

    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.alvik.stop()
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        # SLAM Toolbox
        Node(
            package='slam_toolbox',
            executable='async_slam_toolbox_node',
            name='slam_toolbox',
            output='screen',
            parameters=[
                {'use_sim_time': False},
                {'base_frame': 'base_link'},
                {'odom_frame': 'odom'},
                {'map_frame': 'map'}
            ]
        ),

        # RViz for visualization
        Node(
            package='rviz2',
            executable='rviz2',
            name='rviz2',
            arguments=['-d', '/path/to/alvik_slam.rviz']
        )
    ])