Alex Pirciu
/
BFMC
a
Diff: src/CommandInterpreter/commandinterpreter.cpp
- Revision:
- 1:ceee5a608e7c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/src/CommandInterpreter/commandinterpreter.cpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,180 @@ +/** + ****************************************************************************** + * @file CommandInterpreter.cpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-2017 + * @brief This file contains the class definition for the command interpreter + * functionality. + ****************************************************************************** + */ + +#include <CommandInterpreter/commandinterpreter.hpp> + +/** \brief CCommandInterpreter Class constructor + * + * Constructor method + * + * \param[in] f_car reference to MOVE object + */ +CCommandInterpreter::CCommandInterpreter(Move& f_car) + : m_buffer() + , m_car(f_car) + , m_commandValue() +{ +} + +/** \brief Interpret character + * + * \param[in] f_c character value + */ +void CCommandInterpreter::interpretChar(unsigned char f_c) +{ + if ((f_c == 'A') || (f_c == 'S')) + { +// led1 = 1; + reset(); + m_buffer.push(f_c); + m_commandID = f_c; +// char s[100]; +// unsigned int l = sprintf(s,"storing command starting with %c, %c stored",f_c, m_buffer.peek()); +// g_rpiWriteBuffer.push(s,l); + } + else + { + m_buffer.push(f_c); +// char s[100]; +// unsigned int l = sprintf(s,"peeked %c, command_ID is %c, size is %d",m_buffer.peek(),m_commandID,m_buffer.getSize()); +// g_rpiWriteBuffer.push(s,l); + if (m_buffer.getSize() == 5) + { +// char s[100]; +// unsigned int l = sprintf(s,"interpreting command starting with %c",m_commandID); +// g_rpiWriteBuffer.push(s,l); + intepretCommand(); + reset(); + } + } +} + +/** \brief Execute command + * + */ +void CCommandInterpreter::executeCommand() +{ + m_car.Steer(m_angleValue); + m_car.Speed(m_speedValue); +// reset(); +} + +/** \brief Reset + * + * + * + */ +void CCommandInterpreter::reset() +{ + m_commandID = 0; + m_buffer.empty(); +} + +/** \brief Interpret command + * + * + * + */ +void CCommandInterpreter::intepretCommand() +{ + unsigned char test_char = m_buffer.pop(); + if (test_char != m_commandID) + { + reset(); +// led1 = 0; +// char s[100]; +// unsigned int l = sprintf(s,"interpretation failed at ID. Expected: %c, received: %c",m_commandID,test_char); +// g_rpiWriteBuffer.push(s,l); + } + else + { + float l_sign = 0; + if (m_buffer.peek() == '0') + { + l_sign = 1; + } + else if (m_buffer.peek() == '1') + { + l_sign = -1; + } + else + { + reset(); +// led1 = 0; +// char s[100]; +// unsigned int l = sprintf(s,"interpretation failed a sign"); +// g_rpiWriteBuffer.push(s,l); + return; + } + m_buffer.pop(); + if ((m_buffer.peek() < '0') || (m_buffer.peek() > '9')) + { + reset(); +// led1 = 0; +// char s[100]; +// unsigned int l = sprintf(s,"interpretation failed at val 1"); +// g_rpiWriteBuffer.push(s,l); + return; + } + else + { + m_commandValue = m_buffer.pop() - '0'; + } + if ((m_buffer.peek() < '0') || (m_buffer.peek() > '9')) + { + reset(); +// led1 = 0; +// char s[100]; +// unsigned int l = sprintf(s,"interpretation failed at val 2, value is %c",m_buffer.peek()); +// g_rpiWriteBuffer.push(s,l); + return; + } + else + { + m_commandValue *= 10; + m_commandValue += m_buffer.pop() - '0'; + m_commandValue *= l_sign; + } + if (m_buffer.pop() != ';') + { + reset(); +// led1 = 0; +// char s[100]; +// unsigned int l = sprintf(s,"interpretation failed at terminator"); +// g_rpiWriteBuffer.push(s,l); + return; + } + else + { + if(m_commandID == 'A') + { + m_angleValue = m_commandValue; +// char s[100]; +// unsigned int l = sprintf(s,"set angle to %f\n", m_angleValue); +// g_rpiWriteBuffer.push(s,l); +// led1 = 1; + reset(); + return; + } + if(m_commandID == 'S') + { + m_speedValue = m_commandValue; +// char s[100]; +// unsigned int l = sprintf(s,"set speed to %f\n", m_speedValue); +// g_rpiWriteBuffer.push(s,l); +// led1 = 1; + reset(); + return; + } + reset(); + } + } +}