inline void CommInterface::onInputLine(std::string line)
{
inputStream << line << "\n";
if (line == "go"||
line == "ready")
{
emit processTurn();
inputStream.str("");
std::string text = outputStream.str();
QByteArray buf(text.c_str(), text.size());
outputText(buf);
outputStream.clear();
outputStream.str("");
}
}
void Controller_StateMachine_v2::process() {
// Los datos de entrada a la maquina de estados se fijan desde fuera de manera asíncrona
// La state machine se ejecuta dentro de un controller mutex para que no haya corrupción de datos
// processMandatoryCode();
switch ( current_state ) {
case SM_stateNames::POSITION_CONTROL:
processPositionControl();
break;
case SM_stateNames::SPEED_CONTROL:
processSpeedControl();
break;
case SM_stateNames::STRAIGHT:
if ( justChangedState ) { // state "entry" functions here
cvg_int segment = checkpoint, N = trajectory.getLength();
segment = jesus_library::fmod( segment, N);
cvg_bool last_checkpoint = trajectory.calculate_straight( segment, r_p0, r_ur, r_ur2, rs_end, c_Rt);
if ( c_Rt < 0 ) {
if (last_checkpoint)
r_nextState = SM_stateNames::HOVER;
else
r_nextState = SM_stateNames::STRAIGHT;
} else {
if (last_checkpoint)
r_nextState = SM_stateNames::HOVER;
else
r_nextState = SM_stateNames::TURN;
}
justChangedState = false;
}
processStraight();
isStraightFinished();
updateTrueCheckpointValue();
break;
case SM_stateNames::TURN:
if ( justChangedState ) { // state "entry" functions here
cvg_int turn = pr_checkpoint, N = trajectory.getLength();
turn = jesus_library::fmod( turn, N);
trajectory.calculate_turn( turn, c_pinit, c_pend, c_pc, c_alim, c_u0);
c_vc = trajectory[turn].vc;
justChangedState = false;
}
processTurn();
isTurnFinished();
updateTrueCheckpointValue();
break;
case SM_stateNames::HOVER:
justChangedState = false; // no "entry" functions in this state
processHover();
isHoverFinished();
updateTrueCheckpointValue();
break;
}
#ifdef SM_STATEMACHINE_DEBUG_OLD
debug_counter++;
std::cout << "debug counter = " << debug_counter << "\n";
std::cout << "current position = [ " << xei <<", "<< yei <<", "<< zei << "]\n";
std::cout << "current state = " << getCurrentState() << "; checkpoint = "<< checkpoint << "; true_checkpoint = "<< true_checkpoint <<"\n";
std::cout << "position command = [ " << xrefo <<", "<< yrefo <<", "<< zrefo << "]\n";
std::cout << "speed command = [ " << vxfo <<", "<< vyfo <<", "<< vzfo << "]\n\n";
#endif // SM_STATEMACHINE_DEBUG
}
