Alex Pirciu / Mbed 2 deprecated BFMC

a

Dependencies:   mbed mbed-rtos

src/CommandInterpreter/commandinterpreter.cpp@1:ceee5a608e7c, 2019-03-28 (annotated)

Committer:
alexpirciu
Date:
Thu Mar 28 07:44:42 2019 +0000
Revision:
1:ceee5a608e7c
assa

Who changed what in which revision?

UserRevisionLine numberNew contents of line
alexpirciu 1:ceee5a608e7c 1 /**
alexpirciu 1:ceee5a608e7c 2 ******************************************************************************
alexpirciu 1:ceee5a608e7c 3 * @file CommandInterpreter.cpp
alexpirciu 1:ceee5a608e7c 4 * @author RBRO/PJ-IU
alexpirciu 1:ceee5a608e7c 5 * @version V1.0.0
alexpirciu 1:ceee5a608e7c 6 * @date day-month-2017
alexpirciu 1:ceee5a608e7c 7 * @brief This file contains the class definition for the command interpreter
alexpirciu 1:ceee5a608e7c 8 * functionality.
alexpirciu 1:ceee5a608e7c 9 ******************************************************************************
alexpirciu 1:ceee5a608e7c 10 */
alexpirciu 1:ceee5a608e7c 11
alexpirciu 1:ceee5a608e7c 12 #include <CommandInterpreter/commandinterpreter.hpp>
alexpirciu 1:ceee5a608e7c 13
alexpirciu 1:ceee5a608e7c 14 /** \brief CCommandInterpreter Class constructor
alexpirciu 1:ceee5a608e7c 15 *
alexpirciu 1:ceee5a608e7c 16 * Constructor method
alexpirciu 1:ceee5a608e7c 17 *
alexpirciu 1:ceee5a608e7c 18 * \param[in] f_car reference to MOVE object
alexpirciu 1:ceee5a608e7c 19 */
alexpirciu 1:ceee5a608e7c 20 CCommandInterpreter::CCommandInterpreter(Move& f_car)
alexpirciu 1:ceee5a608e7c 21 : m_buffer()
alexpirciu 1:ceee5a608e7c 22 , m_car(f_car)
alexpirciu 1:ceee5a608e7c 23 , m_commandValue()
alexpirciu 1:ceee5a608e7c 24 {
alexpirciu 1:ceee5a608e7c 25 }
alexpirciu 1:ceee5a608e7c 26
alexpirciu 1:ceee5a608e7c 27 /** \brief Interpret character
alexpirciu 1:ceee5a608e7c 28 *
alexpirciu 1:ceee5a608e7c 29 * \param[in] f_c character value
alexpirciu 1:ceee5a608e7c 30 */
alexpirciu 1:ceee5a608e7c 31 void CCommandInterpreter::interpretChar(unsigned char f_c)
alexpirciu 1:ceee5a608e7c 32 {
alexpirciu 1:ceee5a608e7c 33 if ((f_c == 'A') || (f_c == 'S'))
alexpirciu 1:ceee5a608e7c 34 {
alexpirciu 1:ceee5a608e7c 35 // led1 = 1;
alexpirciu 1:ceee5a608e7c 36 reset();
alexpirciu 1:ceee5a608e7c 37 m_buffer.push(f_c);
alexpirciu 1:ceee5a608e7c 38 m_commandID = f_c;
alexpirciu 1:ceee5a608e7c 39 // char s[100];
alexpirciu 1:ceee5a608e7c 40 // unsigned int l = sprintf(s,"storing command starting with %c, %c stored",f_c, m_buffer.peek());
alexpirciu 1:ceee5a608e7c 41 // g_rpiWriteBuffer.push(s,l);
alexpirciu 1:ceee5a608e7c 42 }
alexpirciu 1:ceee5a608e7c 43 else
alexpirciu 1:ceee5a608e7c 44 {
alexpirciu 1:ceee5a608e7c 45 m_buffer.push(f_c);
alexpirciu 1:ceee5a608e7c 46 // char s[100];
alexpirciu 1:ceee5a608e7c 47 // unsigned int l = sprintf(s,"peeked %c, command_ID is %c, size is %d",m_buffer.peek(),m_commandID,m_buffer.getSize());
alexpirciu 1:ceee5a608e7c 48 // g_rpiWriteBuffer.push(s,l);
alexpirciu 1:ceee5a608e7c 49 if (m_buffer.getSize() == 5)
alexpirciu 1:ceee5a608e7c 50 {
alexpirciu 1:ceee5a608e7c 51 // char s[100];
alexpirciu 1:ceee5a608e7c 52 // unsigned int l = sprintf(s,"interpreting command starting with %c",m_commandID);
alexpirciu 1:ceee5a608e7c 53 // g_rpiWriteBuffer.push(s,l);
alexpirciu 1:ceee5a608e7c 54 intepretCommand();
alexpirciu 1:ceee5a608e7c 55 reset();
alexpirciu 1:ceee5a608e7c 56 }
alexpirciu 1:ceee5a608e7c 57 }
alexpirciu 1:ceee5a608e7c 58 }
alexpirciu 1:ceee5a608e7c 59
alexpirciu 1:ceee5a608e7c 60 /** \brief Execute command
alexpirciu 1:ceee5a608e7c 61 *
alexpirciu 1:ceee5a608e7c 62 */
alexpirciu 1:ceee5a608e7c 63 void CCommandInterpreter::executeCommand()
alexpirciu 1:ceee5a608e7c 64 {
alexpirciu 1:ceee5a608e7c 65 m_car.Steer(m_angleValue);
alexpirciu 1:ceee5a608e7c 66 m_car.Speed(m_speedValue);
alexpirciu 1:ceee5a608e7c 67 // reset();
alexpirciu 1:ceee5a608e7c 68 }
alexpirciu 1:ceee5a608e7c 69
alexpirciu 1:ceee5a608e7c 70 /** \brief Reset
alexpirciu 1:ceee5a608e7c 71 *
alexpirciu 1:ceee5a608e7c 72 *
alexpirciu 1:ceee5a608e7c 73 *
alexpirciu 1:ceee5a608e7c 74 */
alexpirciu 1:ceee5a608e7c 75 void CCommandInterpreter::reset()
alexpirciu 1:ceee5a608e7c 76 {
alexpirciu 1:ceee5a608e7c 77 m_commandID = 0;
alexpirciu 1:ceee5a608e7c 78 m_buffer.empty();
alexpirciu 1:ceee5a608e7c 79 }
alexpirciu 1:ceee5a608e7c 80
alexpirciu 1:ceee5a608e7c 81 /** \brief Interpret command
alexpirciu 1:ceee5a608e7c 82 *
alexpirciu 1:ceee5a608e7c 83 *
alexpirciu 1:ceee5a608e7c 84 *
alexpirciu 1:ceee5a608e7c 85 */
alexpirciu 1:ceee5a608e7c 86 void CCommandInterpreter::intepretCommand()
alexpirciu 1:ceee5a608e7c 87 {
alexpirciu 1:ceee5a608e7c 88 unsigned char test_char = m_buffer.pop();
alexpirciu 1:ceee5a608e7c 89 if (test_char != m_commandID)
alexpirciu 1:ceee5a608e7c 90 {
alexpirciu 1:ceee5a608e7c 91 reset();
alexpirciu 1:ceee5a608e7c 92 // led1 = 0;
alexpirciu 1:ceee5a608e7c 93 // char s[100];
alexpirciu 1:ceee5a608e7c 94 // unsigned int l = sprintf(s,"interpretation failed at ID. Expected: %c, received: %c",m_commandID,test_char);
alexpirciu 1:ceee5a608e7c 95 // g_rpiWriteBuffer.push(s,l);
alexpirciu 1:ceee5a608e7c 96 }
alexpirciu 1:ceee5a608e7c 97 else
alexpirciu 1:ceee5a608e7c 98 {
alexpirciu 1:ceee5a608e7c 99 float l_sign = 0;
alexpirciu 1:ceee5a608e7c 100 if (m_buffer.peek() == '0')
alexpirciu 1:ceee5a608e7c 101 {
alexpirciu 1:ceee5a608e7c 102 l_sign = 1;
alexpirciu 1:ceee5a608e7c 103 }
alexpirciu 1:ceee5a608e7c 104 else if (m_buffer.peek() == '1')
alexpirciu 1:ceee5a608e7c 105 {
alexpirciu 1:ceee5a608e7c 106 l_sign = -1;
alexpirciu 1:ceee5a608e7c 107 }
alexpirciu 1:ceee5a608e7c 108 else
alexpirciu 1:ceee5a608e7c 109 {
alexpirciu 1:ceee5a608e7c 110 reset();
alexpirciu 1:ceee5a608e7c 111 // led1 = 0;
alexpirciu 1:ceee5a608e7c 112 // char s[100];
alexpirciu 1:ceee5a608e7c 113 // unsigned int l = sprintf(s,"interpretation failed a sign");
alexpirciu 1:ceee5a608e7c 114 // g_rpiWriteBuffer.push(s,l);
alexpirciu 1:ceee5a608e7c 115 return;
alexpirciu 1:ceee5a608e7c 116 }
alexpirciu 1:ceee5a608e7c 117 m_buffer.pop();
alexpirciu 1:ceee5a608e7c 118 if ((m_buffer.peek() < '0') || (m_buffer.peek() > '9'))
alexpirciu 1:ceee5a608e7c 119 {
alexpirciu 1:ceee5a608e7c 120 reset();
alexpirciu 1:ceee5a608e7c 121 // led1 = 0;
alexpirciu 1:ceee5a608e7c 122 // char s[100];
alexpirciu 1:ceee5a608e7c 123 // unsigned int l = sprintf(s,"interpretation failed at val 1");
alexpirciu 1:ceee5a608e7c 124 // g_rpiWriteBuffer.push(s,l);
alexpirciu 1:ceee5a608e7c 125 return;
alexpirciu 1:ceee5a608e7c 126 }
alexpirciu 1:ceee5a608e7c 127 else
alexpirciu 1:ceee5a608e7c 128 {
alexpirciu 1:ceee5a608e7c 129 m_commandValue = m_buffer.pop() - '0';
alexpirciu 1:ceee5a608e7c 130 }
alexpirciu 1:ceee5a608e7c 131 if ((m_buffer.peek() < '0') || (m_buffer.peek() > '9'))
alexpirciu 1:ceee5a608e7c 132 {
alexpirciu 1:ceee5a608e7c 133 reset();
alexpirciu 1:ceee5a608e7c 134 // led1 = 0;
alexpirciu 1:ceee5a608e7c 135 // char s[100];
alexpirciu 1:ceee5a608e7c 136 // unsigned int l = sprintf(s,"interpretation failed at val 2, value is %c",m_buffer.peek());
alexpirciu 1:ceee5a608e7c 137 // g_rpiWriteBuffer.push(s,l);
alexpirciu 1:ceee5a608e7c 138 return;
alexpirciu 1:ceee5a608e7c 139 }
alexpirciu 1:ceee5a608e7c 140 else
alexpirciu 1:ceee5a608e7c 141 {
alexpirciu 1:ceee5a608e7c 142 m_commandValue *= 10;
alexpirciu 1:ceee5a608e7c 143 m_commandValue += m_buffer.pop() - '0';
alexpirciu 1:ceee5a608e7c 144 m_commandValue *= l_sign;
alexpirciu 1:ceee5a608e7c 145 }
alexpirciu 1:ceee5a608e7c 146 if (m_buffer.pop() != ';')
alexpirciu 1:ceee5a608e7c 147 {
alexpirciu 1:ceee5a608e7c 148 reset();
alexpirciu 1:ceee5a608e7c 149 // led1 = 0;
alexpirciu 1:ceee5a608e7c 150 // char s[100];
alexpirciu 1:ceee5a608e7c 151 // unsigned int l = sprintf(s,"interpretation failed at terminator");
alexpirciu 1:ceee5a608e7c 152 // g_rpiWriteBuffer.push(s,l);
alexpirciu 1:ceee5a608e7c 153 return;
alexpirciu 1:ceee5a608e7c 154 }
alexpirciu 1:ceee5a608e7c 155 else
alexpirciu 1:ceee5a608e7c 156 {
alexpirciu 1:ceee5a608e7c 157 if(m_commandID == 'A')
alexpirciu 1:ceee5a608e7c 158 {
alexpirciu 1:ceee5a608e7c 159 m_angleValue = m_commandValue;
alexpirciu 1:ceee5a608e7c 160 // char s[100];
alexpirciu 1:ceee5a608e7c 161 // unsigned int l = sprintf(s,"set angle to %f\n", m_angleValue);
alexpirciu 1:ceee5a608e7c 162 // g_rpiWriteBuffer.push(s,l);
alexpirciu 1:ceee5a608e7c 163 // led1 = 1;
alexpirciu 1:ceee5a608e7c 164 reset();
alexpirciu 1:ceee5a608e7c 165 return;
alexpirciu 1:ceee5a608e7c 166 }
alexpirciu 1:ceee5a608e7c 167 if(m_commandID == 'S')
alexpirciu 1:ceee5a608e7c 168 {
alexpirciu 1:ceee5a608e7c 169 m_speedValue = m_commandValue;
alexpirciu 1:ceee5a608e7c 170 // char s[100];
alexpirciu 1:ceee5a608e7c 171 // unsigned int l = sprintf(s,"set speed to %f\n", m_speedValue);
alexpirciu 1:ceee5a608e7c 172 // g_rpiWriteBuffer.push(s,l);
alexpirciu 1:ceee5a608e7c 173 // led1 = 1;
alexpirciu 1:ceee5a608e7c 174 reset();
alexpirciu 1:ceee5a608e7c 175 return;
alexpirciu 1:ceee5a608e7c 176 }
alexpirciu 1:ceee5a608e7c 177 reset();
alexpirciu 1:ceee5a608e7c 178 }
alexpirciu 1:ceee5a608e7c 179 }
alexpirciu 1:ceee5a608e7c 180 }