FSE100: Lego EV3 Car with Claw Pick-up/Autonomous Directional Handling Project

ev3-2025-3-5.m

% Global Variables
speedFwd = 70;          % Forward speed
speedBwd = -70;         % Backward speed
speedTurn = 40;         % Turning speed
sensorPort = 4;         % Color sensor port
ultrasonicPort = 2;     % Ultrasonic sensor port (right side)
thresholdDistance = 20; % Ideal distance from the wall
maxTurnTime = 1.0;      % Max turn duration to avoid circles

global key
InitKeyboard();

% Mode flags
manualControl = true;      % Manual mode enabled by default
autoMove = false;          % Auto mode off initially

% Memory flags
redDetectedOnce = false;   % Prevents repeated stops at the same red zone
turnAttempts = 0;          % Limit excessive turning

% Initialize sensors
brick.SetColorMode(sensorPort, 2); % Set to Color Code Mode

while 1
    pause(0.1);
    
    % --- Color Detection ---
    color = brick.ColorCode(sensorPort);
    
    % 🛑 Stop for Red once and ignore further detections in the same area
    if color == 5 
        if ~redDetectedOnce  % Stop only if red is detected for the first time
            fprintf("🛑 Red detected! Pausing for 1 second...\n");
            brick.StopMotor('AD');
            pause(1)
            fprintf("✅ Resuming movement... Ignoring further red in this area\n");
            
            redDetectedOnce = true;   % Mark red as detected
            brick.MoveMotor('AD', speedFwd); % Resume movement
        else
            % Ignore repeated red detection
            fprintf("🚫 Red ignored (already detected once)\n");
        end
    else
        redDetectedOnce = false;  % Reset flag when no red is detected
    end
    
    % 🔵 Blue color detection → 2 max-volume beeps
    if color == 2
        fprintf("🔵 Blue detected! Switching to Manual Mode\n");

        % Beep 2 times with max volume
        for i = 1:2
            brick.playTone(100, 500, 300);  % Max volume (100), 500 Hz, 300 ms
            pause(0.3);
        end

        manualControl = true;
        autoMove = false;
        brick.StopMotor('AD');
    end

    % 🟢 Green color detection → 3 max-volume beeps
    if color == 3
        fprintf("🟢 Green detected! Beeping 3 times...\n");

        % Beep 3 times with max volume
        for i = 1:3
            brick.playTone(100, 700, 300);  % Max volume (100), 700 Hz, 300 ms
            pause(0.3);
        end
    end
    
    % --- Right-Side Ultrasonic Sensor for Maze Navigation ---
    distance = brick.UltrasonicDist(ultrasonicPort);
    
    if autoMove
        % --- Wall Following Logic ---
        if distance < thresholdDistance - 5
            % Too close → Slight left correction
            fprintf("⬅️ Too close! Adjusting left...\n");
            brick.MoveMotor('A', speedTurn);
            brick.StopMotor('D');
            pause(0.4);  % Gentle correction
            brick.MoveMotor('AD', speedFwd);  % Move forward after correction
            pause(0.5);
        
        elseif distance > thresholdDistance + 5
            % Too far → Slight right c
            % orrection
            fprintf("➡️ Too far! Adjusting right...\n");
            brick.MoveMotor('D', speedTurn);
            brick.StopMotor('A');
            pause(0.4);  % Gentle correction
            brick.MoveMotor('AD', speedFwd);  % Move forward after correction
            pause(0.5);

        else
            % 🚫 No wall detected → Turn **left** at 90 degrees
            fprintf("🚫 No wall detected! Turning LEFT 90 degrees...\n");

            % Prevent infinite circles by limiting turn attempts
            if turnAttempts < 3
                % Back up slightly before turning
                brick.MoveMotor('AD', speedBwd);
                pause(0.8);
                brick.StopMotor('AD');

                % Execute precise 90° left turn
                brick.MoveMotor('A', speedTurn);  
                pause(1.0);  % Adjusted turn time for precise 90°
                brick.StopMotor('AD');

                % Resume moving forward
                brick.MoveMotor('AD', speedFwd);
                
                turnAttempts = turnAttempts + 1;
            else
                % Stop turning after too many attempts
                fprintf("⚠️ Too many turns! Resetting...\n");
                turnAttempts = 0;
                brick.MoveMotor('AD', speedFwd);
                pause(1.0);
            end
        end
    end
    
    % --- Keyboard Controls ---
    if manualControl
        switch key
            case 'uparrow'   % Move Forward
                brick.MoveMotor('AD', speedFwd);
                autoMove = false;
            case 'downarrow'  % Move Backward
                brick.MoveMotor('AD', speedBwd);
                autoMove = false;
            case 'leftarrow'  % Turn Left
                brick.MoveMotor('A', speedTurn);
                brick.StopMotor('D');
                autoMove = false;
            case 'rightarrow' % Turn Right
                brick.MoveMotor('D', speedTurn);
                brick.StopMotor('A');
                autoMove = false;
            case 'backspace'  % Stop All Movement
                brick.StopMotor('AD');
                autoMove = false;
            
            % --- Mode Switching ---
            case 'a'          % Enable Auto Mode
                fprintf("▶ Switching to Autonomous Mode\n");
                autoMove = true;
                manualControl = false;

            case 'm'          % Switch to Manual Mode
                fprintf("▶ Switching to Manual Mode\n");
                manualControl = true;
                autoMove = false;
            
            case 'n'          % Stop all motors
                fprintf("⛔ Stopping all motors\n");
                brick.StopMotor('AD');
                autoMove = false;
        end
    end

    % --- Quit Program ---
    if key == 'q'
        fprintf("🚪 Exiting program...\n");
        brick.StopAllMotors();
        break;
    end
end

CloseKeyboard();