commander.cpp

00001 //
00002 // commander.cpp
00003 // Created by Chandan Siyag on 14/11/2015.
00004 #include <iostream>
00005 #include <string>
00006 #include "commander.h"
00007 
00008 using namespace std;
00009 
00010 const int kCommandValueBufferSize = 80;
00011 const int kObjectBufferSize = 20;
00012 
00013 /// All objects that can recieve commands
00014 string CommandObjectValue [6] = { "mbed", "pc", "colour_sensor", "servos", "port", "break_beam" };
00015 /// All object(fist column) and their possible commands(respective row).
00016 string CommandObjectCommandsValue [6][kMaxCommandCount] = {
00017     {"mbed", "haz-block", "read-current-block", "mode", "sort", "reading-count", "", "", "", ""},
00018     {"pc", "connect", "disconnect", "", "", "", "", "", "reply-commands", "exit"},
00019     {"colour_sensor", "i-time", "preview", "value", "block-value", "test", "", "", "", ""},
00020     {"servos", "test", "reset", "toggle", "", "", "", "", "", ""},
00021     {"port", "init", "b-rate", "parity", "", "", "", "", "", ""},
00022     {"break_beam", "test", "", "", "", "", "", "", "", ""},
00023 };
00024 
00025 /// Creates new commander object
00026 Commander::Commander()
00027 {
00028     replyCommands = false;
00029     this->resetVariables();
00030 }
00031 /// Desctructor
00032 Commander::~Commander()
00033 {
00034 
00035 }
00036 
00037 /// Recieves and decodes the command
00038 void Commander::decodeCommand(CommandTypeRaw type)
00039 {
00040     this->resetVariables();
00041     this->typeRaw = type;
00042     this->typeChar = CommandTypeValue[type];
00043 
00044     this->readCommandObject();
00045     if (this->objectRaw == InvalidObject) {
00046         pc.printf("DEBUG:Invalid command object.\n");
00047         return;
00048     }
00049 
00050     if (this->readCommand(this->objectRaw) == false) {
00051         pc.printf("DEBUG:Invalid command.\n");
00052         return;
00053     } else if (connectedToPC == true || (this->typeRaw == Set && this->objectRaw == PC && this->commandIndex[0] == 1)) {
00054         this->executeCommand();
00055         if (this->replyCommands == true) {
00056             pc.printf("VALUE:%s:VALUE\n", this->description().c_str());
00057         }
00058         //TODO: Send request with response descriptor.
00059         //        pc.("DEBUG:Finished executine command");
00060         return;
00061     }
00062 
00063     pc.printf("INFO:Not connected to PC. %i\n", this->commandIndex[0]);
00064     return;
00065 }
00066 
00067 /// Get the information about the parsed command in a readable manner.
00068 //TODO: Fix format
00069 std::string Commander::description()
00070 {
00071     string str;
00072     str.append("Type:\t\t");
00073     str.append(&this->typeChar);
00074     str.append("\nObject:\t\t" + this->object);
00075 
00076     for (int i = 0; i < sizeof(this->command)/sizeof(*this->command); i++) {
00077         if (this->command[i] == "") {
00078             break;
00079         }
00080         str.append("\nCommand:\t" + this->command[i] + "\nValue:\t\t" + this->commandValue[i].c_str() + " \n");
00081     }
00082     return str;
00083 }
00084 
00085 /// Reads and assignes the imminent commanding object.
00086 void Commander::readCommandObject()
00087 {
00088     char objectInitiator = '<';
00089     char objectTerminator = '>';
00090 
00091     char nextChar = '\0';
00092 
00093     do {
00094         nextChar = pc.getc();
00095     } while (nextChar != objectInitiator);
00096 
00097     char objectCharArray [kObjectBufferSize] = "";
00098     int i = 0;
00099     while (i < kObjectBufferSize) {
00100         nextChar = pc.getc();
00101         if (nextChar == '\n' || nextChar == '\r' || nextChar == ' ') {
00102             continue;
00103         }
00104         if (nextChar == objectTerminator)
00105             break;
00106         objectCharArray[i] = nextChar;
00107         i++;
00108     }
00109     string tempStr(objectCharArray);
00110     this->object = tempStr;
00111 
00112     for (int i = 0; i < (sizeof(CommandObjectValue)/sizeof(*CommandObjectValue)); i++) {
00113         if (CommandObjectValue[i] == this->object) {
00114             this->objectRaw = static_cast<CommandObjectRaw>(i);
00115             return;
00116         }
00117     }
00118 
00119     this->objectRaw = InvalidObject;
00120     return;
00121 }
00122 
00123 /// Reads the command for the previouly read object.
00124 /// Return true if is a possible command, false otherwise.
00125 bool Commander::readCommand(CommandObjectRaw objectRaw)
00126 {
00127     char nextChar = '\0';
00128     char commandCharArray [kMaxCommandCount - 1][kCommandValueBufferSize] = { '\0' };
00129     char commandValueArray [kMaxCommandCount -1][kCommandValueBufferSize] = { '\0' };
00130 
00131     int charIndex = 0;
00132     int valueCharIndex = 0;
00133     int commandValueIndex = 0;
00134     bool commandComplete = false;
00135     while (charIndex < kCommandValueBufferSize - 1 && valueCharIndex < kCommandValueBufferSize - 1) {
00136         nextChar = pc.getc();
00137 
00138         if (nextChar == '\n' || nextChar == '\r' || nextChar == ' ') {
00139             continue;
00140         } else if (nextChar == kCommandTerminator) {
00141             break;
00142         } else if (nextChar == '=') {
00143             commandComplete = true;
00144         } else if (nextChar == ',') {
00145             commandComplete = false;
00146             commandValueIndex++;
00147             charIndex = 0;
00148             valueCharIndex = 0;
00149         }
00150 
00151         if (commandComplete == false && nextChar != ',') {
00152             commandCharArray[commandValueIndex][charIndex] = nextChar;
00153             charIndex++;
00154         } else if (commandComplete == true && nextChar != '=') {
00155             commandValueArray[commandValueIndex][valueCharIndex] = nextChar;
00156             valueCharIndex++;
00157         }
00158     }
00159 
00160     for (int i = 0; i < kMaxCommandCount - 1; i++) {
00161         //        pc.printf("i: %i\n", i);
00162         if (commandCharArray[i][0] == '\0') {
00163             break;
00164         }
00165         string tempCommandStr(commandCharArray[i]);
00166         string tempValueStr(commandValueArray[i]);
00167         int row = this->objectRaw;
00168         int column = 1;
00169         for (; column < kMaxCommandCount - 1; column++) {
00170             if (CommandObjectCommandsValue[row][column] == tempCommandStr) {
00171                 this->command[i] = tempCommandStr;
00172                 this->commandIndex[i] = column;
00173                 this->commandValue[i] = tempValueStr;
00174                 break;
00175             }
00176         }
00177         if (this->commandIndex[i] == -1) {
00178             return false;
00179         }
00180     }
00181 
00182     return true;
00183 }
00184 
00185 /// Executes the previously read command with the object.
00186 void Commander::executeCommand()
00187 {
00188     switch (this->objectRaw) {
00189         case MBED:
00190             for (int i = 0; i < sizeof(this->command)/sizeof(*this->command); i++) {
00191                 if (this->commandIndex[i] == -1 || this->commandIndex[i] == 0) {
00192                     break;
00193                 }
00194                 if (this->commandIndex[i] == 1) {
00195                     if (this->typeRaw == Set) {
00196                         if (this->commandValue[i] == "start") {
00197                             hazBlock(this->typeRaw);
00198                         } else if (this->commandValue[i] == "pause") {
00199                             pc.printf("DEBUG: Exited set new haz block mode.\n");
00200                             setNewHazBlock = false;
00201                         }
00202                     } else if (this->typeRaw == Query) {
00203                     hazBlock(this->typeRaw);
00204                     }
00205                 } else if (this->commandIndex[i] == 2) {
00206                     getCurrentBlock(this->typeRaw);
00207                 } else if (this->commandIndex[i] == 3) {
00208                     if (this->commandValue[i] == "maintanence") {
00209                         currentMode = Maintanence;
00210                         pc.printf("INFO:Running in maintanence mode.\n");
00211                     } else if (this->commandValue[i] == "normal") {
00212                         currentMode = Normal;
00213                         pc.printf("INFO:Running in nomal mode.\n");
00214                     } else if (this->commandValue[i] == "none") {
00215                         currentMode = None;
00216                         pc.printf("INFo:Running in none mode.\n");
00217                     }
00218                 } else if (this->commandIndex[i] == 4 && currentMode == Normal) {
00219                     if (this->commandValue[i] == "start") {
00220                         currentState = Start;
00221                         pc.printf("INFO:Starting sorting.\n");
00222                     } else if (this->commandValue[i] == "pause") {
00223                         currentState = Pause;
00224                         pc.printf("INFO:Pausing sorting.\n");
00225                     }
00226                 } else if (this->commandIndex[i] == 5) {
00227                     int readingCount = 0;
00228                     sscanf(this->commandValue[i].c_str(), "%i", readingCount);
00229                     hazReadingCount = readingCount;
00230                 }
00231             }
00232             break;
00233         case PC:
00234             for (int i = 0; i < sizeof(this->command)/sizeof(*this->command); i++) {
00235                 if (this->commandIndex[i] == -1 || this->commandIndex[i] == 0) {
00236                     break;
00237                 }
00238                 if (this->commandIndex[i] == 1) {
00239                     connectToPC(this->typeRaw);
00240                 } else if (this->commandIndex[i] == 2) {
00241                     disconnectToPC(this->typeRaw);
00242                 } else if (this->commandIndex[i] == 8) {
00243                     if (this->commandValue[i] == "ON") {
00244                         this->replyCommands = true;
00245                     } else if (this->commandValue[i] == "OFF") {
00246                         this->replyCommands = false;
00247                     }
00248                 }
00249             }
00250             break;
00251         case ColourSensor:
00252             for (int i = 0; i < sizeof(this->command)/sizeof(*this->command); i++) {
00253                 if (this->commandIndex[i] == -1 || this->commandIndex[i] == 0) {
00254                     break;
00255                 }
00256                 if (this->commandIndex[i] == 1) {
00257                     float integrationTime;
00258                     sscanf(this->commandValue[i].c_str(), "%f", &integrationTime);
00259                     if (integrationTime < 2.4 || integrationTime > 600) {
00260                         pc.printf("ERROR:Integration Time invalid: %.3f\n", integrationTime);
00261                         continue;
00262                     }
00263                     setIntegrationTimeTo(integrationTime);
00264                 } else if (this->commandIndex[i] == 2) {
00265                     if (this->commandValue[i] == "ON") {
00266                         previewOnPC(true);
00267                     } else if (this->commandValue[i] == "OFF") {
00268                         previewOnPC(false);
00269                     }
00270                 } else if (this->commandIndex[i] == 3 && runColourSensorTest == true && getColourSensorValue == false) {
00271                     getColourSensorValue = true;
00272                 } else if (this->commandIndex[i] == 4 && runColourSensorTest == true && getBlockColourValue == false) {
00273                     getBlockColourValue = true;
00274                 } else if (this->commandIndex[i] == 5) {
00275                     if (this->commandValue[i] == "start") {
00276                         testColourSensor(Start);
00277                     } else if (this->commandValue[i] == "pause") {
00278                         testColourSensor(Pause);
00279                     }
00280                 }
00281             }
00282             break;
00283         case Servos:
00284             for (int i = 0; i < sizeof(this->command)/sizeof(*this->command); i++) {
00285                 if (this->commandIndex[i] == -1 || this->commandIndex[i] == 0) {
00286                     break;
00287                 }
00288                 if (this->commandIndex[i] == 1) {
00289                     if (this->commandValue[i] == "start") {
00290                         testServos(Start);
00291                     } else if (this->commandValue[i] == "pause") {
00292                         testServos(Pause);
00293                     }
00294                 } else if (this->commandIndex[i] == 2) {
00295                     resetServos();
00296                 } else if (this->commandIndex[i] == 3) {
00297                     if (this->commandValue[i] == "1") {
00298                         pc.printf("INFO: Moving top servo.\n");
00299                         gToggleServoNumber = 1;
00300                     } else if (this->commandValue[i] == "2") {
00301                         pc.printf("INFO: Moving bottom servo.\n");
00302                         gToggleServoNumber = 2;
00303                     } else if (this->commandValue[i] == "3") {
00304                         pc.printf("INFO: Moving both servos.\n");
00305                         gToggleServoNumber = 3;
00306                     }
00307                 }
00308             }
00309             break;
00310         case Port:
00311             for (int i = 0; i < sizeof(this->command)/sizeof(*this->command); i++) {
00312                 if (this->commandIndex[i] == -1 || this->commandIndex[i] == 0) {
00313                     break;
00314                 }
00315                 if (this->commandIndex[i] == 1) {
00316                     getPortInfo();
00317                 } else if (this->commandIndex[i] == 2) {
00318                     int baudRate;
00319                     sscanf(this->commandValue[i].c_str(), "%i", &baudRate);
00320                     setPortBaudRate(baudRate);
00321                 } else if (this->commandIndex[i] == 3) {
00322                     int parity = 0;
00323                     sscanf(this->commandValue[i].c_str(), "%i", &parity);
00324                     setPortParity(parity);
00325                 }
00326             }
00327             break;
00328         case BreakBeam:
00329             for (int i = 0; i < sizeof(this->command)/sizeof(*this->command); i++) {
00330                 if (this->commandIndex[i] == -1 || this->commandIndex[i] == 0) {
00331                     break;
00332                 }
00333                 if (this->commandIndex[i] == 1) {
00334                     if (this->commandValue[i] == "start") {
00335                         testBreakBeams(Start);
00336                     } else if (this->commandValue[i] == "pause") {
00337                         testBreakBeams(Pause);
00338                     }
00339                 }
00340             }
00341         default:
00342             break;
00343     }
00344     return;
00345 }
00346 
00347 // Cleans all the properties, ready to recieved next command.
00348 void Commander::resetVariables()
00349 {
00350     this->object = "";
00351     this->objectRaw = InvalidObject;
00352     for (int i = 0; i < sizeof(this->command)/sizeof(*this->command); i ++) {
00353         this->command[i].clear();
00354         this->commandValue[i].clear();
00355         this->commandIndex[i] = -1;
00356     }
00357     this->typeRaw = InvalidType;
00358     this->typeChar = '\0';
00359 }