commandinterpreter.cpp

00001 /**
00002   ******************************************************************************
00003   * @file    CommandInterpreter.cpp
00004   * @author  RBRO/PJ-IU
00005   * @version V1.0.0
00006   * @date    day-month-2017
00007   * @brief   This file contains the class definition for the command interpreter
00008   *          functionality.
00009   ******************************************************************************
00010  */
00011 
00012 #include <CommandInterpreter/commandinterpreter.hpp>
00013 
00014 /** \brief  CCommandInterpreter Class constructor
00015  *
00016  *  Constructor method
00017  *
00018  *  \param[in] f_car     reference to MOVE object
00019  */
00020 CCommandInterpreter::CCommandInterpreter(Move& f_car) 
00021     : m_buffer()
00022     , m_car(f_car)
00023     , m_commandValue()
00024 {
00025 }
00026 
00027 /** \brief  Interpret character
00028  *
00029  *  \param[in] f_c     character value
00030  */
00031 void CCommandInterpreter::interpretChar(unsigned char f_c)
00032 {
00033     if ((f_c == 'A') || (f_c == 'S'))
00034     {
00035 //            led1 = 1;
00036         reset();
00037         m_buffer.push(f_c);
00038         m_commandID = f_c;
00039 //            char s[100];
00040 //            unsigned int  l = sprintf(s,"storing command starting with %c, %c stored",f_c, m_buffer.peek());
00041 //            g_rpiWriteBuffer.push(s,l);
00042     }
00043     else
00044     {
00045         m_buffer.push(f_c);
00046 //            char s[100];
00047 //            unsigned int  l = sprintf(s,"peeked %c, command_ID is %c, size is %d",m_buffer.peek(),m_commandID,m_buffer.getSize());
00048 //            g_rpiWriteBuffer.push(s,l);
00049         if (m_buffer.getSize() == 5)
00050         {
00051 //                char s[100];
00052 //                unsigned int  l = sprintf(s,"interpreting command starting with %c",m_commandID);
00053 //                g_rpiWriteBuffer.push(s,l);
00054             intepretCommand();
00055             reset();
00056         }
00057     }
00058 }
00059 
00060 /** \brief  Execute command
00061  *
00062  */
00063 void CCommandInterpreter::executeCommand()
00064 {
00065     m_car.Steer(m_angleValue);
00066     m_car.Speed(m_speedValue);
00067 //        reset();
00068 }
00069 
00070 /** \brief  Reset
00071  *
00072  *  
00073  *  
00074  */
00075 void CCommandInterpreter::reset()
00076 {
00077     m_commandID = 0;
00078     m_buffer.empty();
00079 }
00080 
00081 /** \brief  Interpret command
00082  *
00083  *  
00084  *  
00085  */
00086 void CCommandInterpreter::intepretCommand()
00087 {
00088     unsigned char test_char = m_buffer.pop();
00089     if (test_char != m_commandID)
00090     {
00091         reset();
00092 //            led1 = 0;
00093 //            char s[100];
00094 //            unsigned int  l = sprintf(s,"interpretation failed at ID. Expected: %c, received: %c",m_commandID,test_char);
00095 //            g_rpiWriteBuffer.push(s,l);
00096     }
00097     else
00098     {
00099         float l_sign = 0;
00100         if (m_buffer.peek() == '0')
00101         {
00102             l_sign = 1;
00103         }
00104         else if (m_buffer.peek() == '1')
00105         {
00106             l_sign = -1;
00107         }
00108         else
00109         {
00110             reset();
00111 //                led1 = 0;
00112 //                char s[100];
00113 //                unsigned int  l = sprintf(s,"interpretation failed a sign");
00114 //                g_rpiWriteBuffer.push(s,l);
00115             return;
00116         }
00117         m_buffer.pop();
00118         if ((m_buffer.peek() < '0') || (m_buffer.peek() > '9'))
00119         {
00120             reset();
00121 //                led1 = 0;
00122 //                char s[100];
00123 //                unsigned int  l = sprintf(s,"interpretation failed at val 1");
00124 //                g_rpiWriteBuffer.push(s,l);
00125             return;
00126         }
00127         else
00128         {
00129             m_commandValue = m_buffer.pop() - '0';
00130         }
00131         if ((m_buffer.peek() < '0') || (m_buffer.peek() > '9'))
00132         {
00133             reset();
00134 //                led1 = 0;
00135 //                char s[100];
00136 //                unsigned int  l = sprintf(s,"interpretation failed at val 2, value is %c",m_buffer.peek());
00137 //                g_rpiWriteBuffer.push(s,l);
00138             return;
00139         }
00140         else
00141         {
00142             m_commandValue *= 10;
00143             m_commandValue += m_buffer.pop() - '0';
00144             m_commandValue *= l_sign;
00145         }
00146         if (m_buffer.pop() != ';')
00147         {
00148             reset();
00149 //                led1 = 0;
00150 //                char s[100];
00151 //                unsigned int  l = sprintf(s,"interpretation failed at terminator");
00152 //                g_rpiWriteBuffer.push(s,l);
00153             return;
00154         }      
00155         else
00156         {
00157             if(m_commandID == 'A')
00158             {
00159                 m_angleValue = m_commandValue;
00160 //                    char s[100];
00161 //                    unsigned int  l = sprintf(s,"set angle to %f\n", m_angleValue);
00162 //                    g_rpiWriteBuffer.push(s,l);
00163 //                    led1 = 1;
00164                 reset();
00165                 return;
00166             }
00167             if(m_commandID == 'S')
00168             {
00169                 m_speedValue = m_commandValue;
00170 //                    char s[100];
00171 //                    unsigned int  l = sprintf(s,"set speed to %f\n", m_speedValue);
00172 //                    g_rpiWriteBuffer.push(s,l);
00173 //                    led1 = 1;
00174                 reset();
00175                 return;
00176             }
00177             reset();
00178         }      
00179     }
00180 }