Browse Source

Start Autonomous via socket; redirect signals to uart

master
Tim Zeuner 1 year ago
parent
commit
59ce30a828
3 changed files with 48 additions and 0 deletions
  1. 44
    0
      lfr_state_machine.cpp
  2. 2
    0
      lfr_state_machine.h
  3. 2
    0
      lfr_states.cpp

+ 44
- 0
lfr_state_machine.cpp View File

std::cout << "no switch" << std::endl; std::cout << "no switch" << std::endl;
} }


}

void LFR_StateMachine::enterAutonomous()
{
//cv::Mat img;
autonomousMode.addListener([&](LFR_Result result)
{
std::unique_lock<std::mutex> lock(mutex);
if (!result.rawImage.empty())
{
/*
img = result.rawImage;
//Resize to minimize latency using raspi over ssh/adapt to 3.5in screen
cv::resize(result.rawImage, img, cv::Size(480, 320));

//Calculate frame rate
now = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch());
unsigned int deltaMs = static_cast<unsigned int>((now-last).count());
double delta = static_cast<double>(deltaMs) / 1000.0;
double frameRate = 1.0 / static_cast<double>(delta);*/
double frameRate = -1.0;

if (result.validLane)
{
std::cout << "Frame rate: " << frameRate << " angle: " << result.data.angle
<< " motor 1: " << result.motorSignals[0] << " motor 2: " << result.motorSignals[1]
<< " motor 3: " << result.motorSignals[2] << " motor 4: " << result.motorSignals[3] <<std::endl;
uartCommunicator.sendTelegram(result.motorSignals[0], result.motorSignals[1], result.motorSignals[2], result.motorSignals[3]);
}
else
{
std::cout << "No lane found." << std::endl;;
}
//last = now;
}
}, &mutex);

autonomousMode.startLoop();
}

void LFR_StateMachine::exitAutonomous()
{
autonomousMode.removeListener(&mutex);
autonomousMode.endLoop();
} }

+ 2
- 0
lfr_state_machine.h View File

void parseString(string s); void parseString(string s);


public: public:
void enterAutonomous();
void exitAutonomous();
LFR_StateMachine(); LFR_StateMachine();
inline LFR_IState* getCurrentState() const {return currentState;} inline LFR_IState* getCurrentState() const {return currentState;}
void setState(LFR_IState& newState); void setState(LFR_IState& newState);

+ 2
- 0
lfr_states.cpp View File

void State::Autonomous::enter(LFR_StateMachine* stateMachine) void State::Autonomous::enter(LFR_StateMachine* stateMachine)
{ {
std::cout << "enter autonomous mode" << std::endl; std::cout << "enter autonomous mode" << std::endl;
stateMachine->enterAutonomous();
} }


void State::Autonomous::exit(LFR_StateMachine* stateMachine) void State::Autonomous::exit(LFR_StateMachine* stateMachine)
{ {
std::cout << "exit autonomous mode" << std::endl; std::cout << "exit autonomous mode" << std::endl;
stateMachine->exitAutonomous();
} }


LFR_IState& State::Autonomous::getInstance() LFR_IState& State::Autonomous::getInstance()

Loading…
Cancel
Save