Train

function runTrain()
    delete(timerfindall);
    clear;
    close all;
    delete(instrfindall);
    clc;

    simulator = input('Run the train on a simulator? (0/1)\nsimulator = ');
    dSpeed = input('What is the speed limit in the departure phase?\ndSpeed = ');
    aSpeed = input('What is the speed limit in the approach phase?\naSpeed = ');
    runTime = input('For how many seconds should the train run? (0 for forever)\nrunTime = ');

    if(simulator > 0)
        train = arduino_sim('COM3');
    else
        train = arduino('COM3');
    end

    % Load audio file
    [soundY, soundFs] = audioread('train.wav');

    % Initialize LEDs; 6 / 7 is red / green on approach. 8 / 9 is red /
    % green on departure. 14 and 15 are the crossing LEDs.
    train.pinMode(6, 'output');
    train.pinMode(7, 'output');
    train.pinMode(8, 'output');
    train.pinMode(9, 'output');
    train.pinMode(14, 'output');
    train.pinMode(15, 'output');

    % Set up the crossing gate and default it to raised.
    train.servoAttach(1);
    train.servoWrite(1, 90);

    % Initialize the variable which tracks which LED on the crossing gate
    % is currently on.
    global flashState;
    flashState = 1;

    % Found using ratio 3 / stopDelay = aSpeed / 170, where the 3 is the
    % time it took to stop at a speed of 170.
    stopDelay = 510 / aSpeed;

    % Starts the train 2.5 seconds after it stops
    startTimer = timer('StartDelay', stopDelay + 2.5, 'Name', 'startTimer');
    startTimer.TimerFcn = {@startTrain, train, aSpeed};

    % Stop the train at the calculated stop delay.
    stopTimer = timer('StartDelay', stopDelay, 'Name', 'stopTimer');
    stopTimer.TimerFcn = {@stopTrain, train};

    % Alternate the crossing lights every .5 seconds for as many times
    % until the period adds up to about the time it takes the train to
    % stop. When the timer finishes, both lights are turned off.
    flashTimer = timer('Period', .5, 'TasksToExecute', ceil((stopDelay / .5)) + 1, 'ExecutionMode', 'fixedRate', 'Name', 'flashTimer');
    flashTimer.TimerFcn = {@flashLights, train};
    flashTimer.StopFcn = {@turnOffLights, train};

    % Raise the gate when the train stops.
    raiseGateTimer = timer('StartDelay', stopDelay, 'Name', 'raiseGateTimer');
    raiseGateTimer.TimerFcn = {@raiseGate, train};

    % Lower the gate .5 seconds after the train hits approach.
    lowerGateTimer = timer('StartDelay', .5, 'Name', 'lowerGateTimer');
    lowerGateTimer.TimerFcn = {@lowerGate, train};

    % Start the train.
    train.motorRun(1, 'forward');
    train.motorSpeed(1, dSpeed);

    % Makes sure the train is stopped when the program exits.
    cleanupFcn = onCleanup(@() cleanup(train));

    % The program will 'officially' start when the train first hits the
    % approach sensor, so assume it's not initially in approach.
    inApproach = 0;

    % Run until the time reaches the specify length, or always run if 0 was
    % specified.
    curTime = tic;
    while toc(curTime) <= runTime || runTime == 0
        % Read the sensor's values
        approach = train.analogRead(3);
        departure = train.analogRead(2);

        % Knowing these values will help debug if neccessary.
        clc
        disp('In approach?')
        disp(inApproach)
        disp('Approach:')
        disp(approach)
        disp('Departue:')
        disp(departure)

        % If the approach sensor goes high and the train is currently not
        % in approach.
        if approach > 300
            if ~inApproach
                % Turn the green departure light on and the red approach
                % light on.
                train.digitalWrite(6, 1);
                train.digitalWrite(7, 0);
                train.digitalWrite(8, 0);
                train.digitalWrite(9, 1);

                % Tell the program the train's in approach now.
                inApproach = 1;

                % Set the train to the approach speed.
                train.motorSpeed(1, aSpeed);

                % All timers start at once and are timed relative to when
                % the train should stop. Some timers will execute
                % simultaneously.
                start(flashTimer);
                start(raiseGateTimer);
                start(lowerGateTimer);
                start(stopTimer);
                start(startTimer);

                % Play the train sound. The timers are asynchronous, so
                % this executes immediatally after the timers are started.
                sound(soundY, soundFs);
            end
        % If the departure sensor goes high and the train is in approach.
        elseif departure > 300
            if inApproach
                % Turn the green approach light on and the red departure
                % light on.
                train.digitalWrite(6, 0);
                train.digitalWrite(7, 1);
                train.digitalWrite(8, 1);
                train.digitalWrite(9, 0);

                % We're no longer in the approach phase.
                inApproach = 0;

                % Just in case the sensor triggers when it's not supposed
                % to, cancel the timers to prevent unexpected behavior.
                stop(timerfindall);

                % Return to departure speed.
                train.motorSpeed(1, dSpeed);
            end
        end
    end
end

function flashLights(~, ~, train)
    % Each time this fcn runs, flashState alternates its value, so we can
    % alternate the lights by checking its value.
    global flashState;
    if flashState == 1
        train.digitalWrite(14, 1);
        train.digitalWrite(15, 0);
    else
        train.digitalWrite(14, 0);
        train.digitalWrite(15, 1);
    end
    flashState = ~flashState;
end

function turnOffLights(~, ~, train)
    train.digitalWrite(14, 0);
    train.digitalWrite(15, 0);
end

function stopTrain(~, ~, train)
    train.motorRun(1, 'release');
end

function raiseGate(~, ~, train)
    train.servoWrite(1, 90);
end

function lowerGate(~, ~, train)
    train.servoWrite(1, 180)
end

function startTrain(~, ~, train, aSpeed)
    train.motorRun(1, 'forward');
    train.motorSpeed(1, aSpeed);
end

function cleanup(train)
    train.motorRun(1, 'release');
    % Cancel the timers to make sure the train doesn't start back up after
    % the program stops.
    stop(timerfindall);
end