Line-Following-Robot/lfr_state_machine.cpp

#include "lfr_state_machine.h"

LFR_StateMachine::LFR_StateMachine(LFR_IState& startState):    
    autonomousMode(videoHeight, videoWidth, thresholdBinary, gaussKernelSize, [&](std::exception const &ex)
    {
        std::unique_lock<std::mutex> lock(mutex);
        std::cerr<<"camera exception:"<<ex.what()<<std::endl;
        return false;
    }),
    uartCommunicator(),
    socket([&](std::exception const &ex)
    {
        std::unique_lock<std::mutex> lock(mutex);
        std::cerr<<"socket exception:"<<ex.what()<<std::endl;
        return false;
    })
{
    // Connect String parser to socket
    socket.addListener([&](LFR_Socket::LFR_Telegram telegram)
    {
        std::unique_lock<std::mutex> lock(mutex);
        std::cout << telegram;
    }, &mutex);
    socket.startLoop();
    
    currentState = &startState;
    currentState->enter(this);

    //Start the permanent loop
    char input;
    std::cout << "press q to quit" << std::endl;
    std::cin >> input;
    std::cout << "binned" << std::endl;
    while (input != 'q')
    {
        std::cin >> input;
        std::cout << "binned" << std::endl;
    }
    std::cout << "Exiting central" << std::endl;
}