Diff: commander.cpp

Revision:

15:777390eb5afd

Parent:

14:cf2f255b5560

Child:

16:4a41cb9e525f

diff -r cf2f255b5560 -r 777390eb5afd commander.cpp
--- a/commander.cpp	Sun Nov 22 17:53:36 2015 +0000
+++ b/commander.cpp	Sun Nov 22 20:29:08 2015 +0000
@@ -6,6 +6,7 @@
 #include "commander.h"
 
 using namespace std;
+#define _DEBUG_
 
 //const int kCommandBufferSize = 40;
 const int kCommandValueBufferSize = 80;
@@ -13,228 +14,279 @@
 
 string CommandObjectValue [5] = { "mbed", "pc", "colour_sensor", "servos", "port" };
 string CommandObjectCommandsValue [5][kMaxCommandCount] = {
-		{"mbed", "haz-block", "read-current-block", "mode", "", "", "", "", "", ""},
-		{"pc", "connect", "disconnect", "", "", "", "", "", "reply-commands", "exit"},
-		{"colour_sensor", "i-time", "preview", "", "", "", "", "", "", ""},
-		{"servos", "test", "reset", "", "", "", "", "", "", ""},
-		{"port", "init", "b-rate", "parity", "", "", "", "", "", ""},
+    {"mbed", "haz-block", "read-current-block", "mode", "", "", "", "", "", ""},
+    {"pc", "connect", "disconnect", "", "", "", "", "", "reply-commands", "exit"},
+    {"colour_sensor", "i-time", "preview", "", "", "", "", "", "", ""},
+    {"servos", "test", "reset", "", "", "", "", "", "", ""},
+    {"port", "init", "b-rate", "parity", "", "", "", "", "", ""},
 };
 
-Commander::Commander() {
-	replyCommands = false;
-	this->resetVariables();
+Commander::Commander()
+{
+    replyCommands = false;
+    this->resetVariables();
 }
 
-Commander::~Commander() {
+Commander::~Commander()
+{
 
 }
 
-void Commander::decodeCommand(CommandTypeRaw type){
-	this->resetVariables();
-	this->typeRaw = type;
-	this->typeChar = CommandTypeValue[type];
-
-	this->readCommandObject();
-	if (this->objectRaw == InvalidObject) { return; }
-
-	if (this->readCommand(this->objectRaw) == false) { return; }
-	//	else if ((connectedToPC == false) && (this->typeRaw != Set || this->objectRaw != PC || this->commandIndex[0] != 1)) { return; }
-	else if (connectedToPC == true || (this->typeRaw == Set && this->objectRaw == PC && this->commandIndex[0] == 1)) { this->executeCommand(); }
+void Commander::decodeCommand(CommandTypeRaw type)
+{
+    this->resetVariables();
+    this->typeRaw = type;
+    this->typeChar = CommandTypeValue[type];
 
-	if (this->replyCommands) { this->description(); }
-}
-
-std::string Commander::description(){
-	string str;
-	str.append("Command type:\t");
-	str.append(&this->typeChar);
-	str.append("\nCommand object:\t" + this->object);
-	
-	for (int i = 0; i < sizeof(this->command)/sizeof(*this->command); i++) {
-		if (this->command[i] == "") { break; }
-	str.append("Command:\t" + this->command[i] + "\nValue:\t" + this->commandValue[i].c_str() + " \n");
-	}
-
-	return str;
-}
+    this->readCommandObject();
+    if (this->objectRaw == InvalidObject) {
+        pc.printf("Invalid command object.\n");
+        return;
+    }
 
-void Commander::readCommandObject(){
-	char objectInitiator = '<';
-	char objectTerminator = '>';
-
-	char nextChar = '\0';
-
-	do {
-		nextChar = pc.getc();
-	} while (nextChar != objectInitiator);
-
-	char objectCharArray [kObjectBufferSize] = "";
-	int i = 0;
-	while (i < kObjectBufferSize) {
-		nextChar = pc.getc();
-		if (nextChar == '\n' || nextChar == '\r' || nextChar == ' ') { continue; }
-		if (nextChar == objectTerminator)
-			break;
-		objectCharArray[i] = nextChar;
-		i++;
-	}
-	string tempStr(objectCharArray);
-	this->object = tempStr;
-
-	for (int i = 0; i < (sizeof(CommandObjectValue)/sizeof(*CommandObjectValue)); i++) {
-		if (CommandObjectValue[i] == this->object){
-			this->objectRaw = static_cast<CommandObjectRaw>(i);
-			return;
-		}
-	}
-
-	this->objectRaw = InvalidObject;
+    if (this->readCommand(this->objectRaw) == false) {
+//        this->description();
+        pc.printf("Invalid command.\n");
+        return;
+    }
+    else if (connectedToPC == true || (this->typeRaw == Set && this->objectRaw == PC && this->commandIndex[0] == 1)) {
+        this->executeCommand();
+        if (this->replyCommands) {
+        this->description();
+    	}
+        return;
+    }
+    
+    pc.printf("Not connected to PC. %i\n", this->commandIndex[0]);
 	return;
 }
 
-bool Commander::readCommand(CommandObjectRaw objectRaw){
-	char nextChar = '\0';
-	char commandCharArray [kMaxCommandCount - 1][79] = { "" };
-	char commandValue [kMaxCommandCount -1][79] = { "" };
+std::string Commander::description()
+{
+    string str;
+    str.append("Command type:\t");
+    str.append(&this->typeChar);
+    str.append("\nCommand object:\t" + this->object);
 
-	int charIndex = 0;
-	int valueCharIndex = 0;
-	int commandValueIndex = 0;
-	bool commandComplete = false;
-	while (charIndex < kCommandValueBufferSize && valueCharIndex < kCommandValueBufferSize) {
-		scanf("%1c", &nextChar);
-		if (nextChar == '\n' || nextChar == '\r' || nextChar == ' ') { continue; }
-		else if (nextChar == kCommandTerminator) { break; }
+    for (int i = 0; i < sizeof(this->command)/sizeof(*this->command); i++) {
+        if (this->command[i] == "") {
+            break;
+        }
+        str.append("Command:\t" + this->command[i] + "\nValue:\t" + this->commandValue[i].c_str() + " \n");
+    }
 
-		else if (nextChar == '=') { commandComplete = true; }
-		else if (nextChar == ',') {
-			commandComplete = false;
-			commandValueIndex++;
-			charIndex = 0;
-			valueCharIndex = 0;
-		}
+    return str;
+}
+
+void Commander::readCommandObject()
+{
+    char objectInitiator = '<';
+    char objectTerminator = '>';
+
+    char nextChar = '\0';
 
-		if (commandComplete == false && nextChar != ',') {
-			commandCharArray[commandValueIndex][charIndex] = nextChar;
-			charIndex++;
-		}else if (commandComplete == true && nextChar != '='){
-			commandValue[commandValueIndex][valueCharIndex] = nextChar;
-			valueCharIndex++;
-		}
-
-	}
+    do {
+        nextChar = pc.getc();
+    } while (nextChar != objectInitiator);
 
-	for (int i = 0; i < sizeof(this->command)/sizeof(*this->command); i++) {
-		if (strcmp(commandCharArray[i], "")) { break; }
-		
-		string tempCommandStr(commandCharArray[i]);
-		string tempValueStr(commandValue[i]);
+    char objectCharArray [kObjectBufferSize] = "";
+    int i = 0;
+    while (i < kObjectBufferSize) {
+        nextChar = pc.getc();
+        if (nextChar == '\n' || nextChar == '\r' || nextChar == ' ') {
+            continue;
+        }
+        if (nextChar == objectTerminator)
+            break;
+        objectCharArray[i] = nextChar;
+        i++;
+    }
+    string tempStr(objectCharArray);
+    this->object = tempStr;
 
-		int row = objectRaw;
-		int column = 1;
-		for (;column < kMaxCommandCount; column++){
-			if (CommandObjectCommandsValue[row][column] == tempCommandStr) {
-				this->command[i] = tempCommandStr;
-				this->commandIndex[i] = column;
-				this->commandValue[i] = tempValueStr;
-				break;
-			}
-		}
-		if (this->commandIndex[i] == -1) { return false; }
-	}
+    for (int i = 0; i < (sizeof(CommandObjectValue)/sizeof(*CommandObjectValue)); i++) {
+        if (CommandObjectValue[i] == this->object) {
+            this->objectRaw = static_cast<CommandObjectRaw>(i);
+            return;
+        }
+    }
 
-	return true;
+    this->objectRaw = InvalidObject;
+    return;
 }
 
-void Commander::executeCommand(){
-	switch (this->objectRaw) {
-	case MBED:
-		for (int i = 0; i < sizeof(this->command)/sizeof(*this->command); i++) {
-			if (this->commandIndex[i] == -1 || this->commandIndex[i] == 0) { break; }
-			if (this->commandIndex[i] == 1) {
-				hazBlock(this->typeRaw);
-			}else if (this->commandIndex[i] == 2){
-				getCurrentBlock(this->typeRaw);
-			}else if (this->commandIndex[i] == 3){
-				if (this->commandValue[i] == "maintanence"){
+bool Commander::readCommand(CommandObjectRaw objectRaw)
+{
+    char nextChar = '\0';
+    char commandCharArray [kMaxCommandCount - 1][kCommandValueBufferSize] = { '\0' };
+    char commandValueArray [kMaxCommandCount -1][kCommandValueBufferSize] = { '\0' };
+
+    int charIndex = 0;
+    int valueCharIndex = 0;
+    int commandValueIndex = 0;
+    bool commandComplete = false;
+    while (charIndex < kCommandValueBufferSize - 1 && valueCharIndex < kCommandValueBufferSize - 1) {
+        nextChar = pc.getc();
+		
+        if (nextChar == '\n' || nextChar == '\r' || nextChar == ' ') {
+            continue;
+        } else if (nextChar == kCommandTerminator) {
+            break;
+        }else if (nextChar == '=') {
+            commandComplete = true;
+        } else if (nextChar == ',') {
+            commandComplete = false;
+            commandValueIndex++;
+            charIndex = 0;
+            valueCharIndex = 0;
+        }
+
+        if (commandComplete == false && nextChar != ',') {
+            commandCharArray[commandValueIndex][charIndex] = nextChar;
+            charIndex++;
+        } else if (commandComplete == true && nextChar != '=') {
+            commandValueArray[commandValueIndex][valueCharIndex] = nextChar;
+            valueCharIndex++;
+        }
+    }
+
+    for (int i = 0; i < kMaxCommandCount - 1; i++) {
+        pc.printf("i: %i\n", i);
+        if (commandCharArray[i][0] == '\0') { break; }
+        string tempCommandStr(commandCharArray[i]);
+        string tempValueStr(commandValueArray[i]);
+		pc.printf("%s\n", tempCommandStr.c_str());
+        int row = this->objectRaw;
+        pc.printf("Row: %i\n", this->objectRaw);
+        int column = 1;
+        pc.printf("Column: %i\n", column);
+        for (; column < kMaxCommandCount - 1; column++) {
+            pc.printf("%i\n", column);
+            if (CommandObjectCommandsValue[row][column] == tempCommandStr) {
+                pc.printf("Found matching command.\n");
+                //pc.printf("%s\n", this->command[i].c_str());
+                
+//                pc.printf("%s\n", this->command[i].c_str());
+                this->commandIndex[i] = column;
+                pc.printf("%i\n", this->commandIndex[i]);
+                this->commandValue[i] = tempValueStr;
+                pc.printf("%s\n", this->commandValue[i].c_str());
+//                pc.printf("%s\n", this->description().c_str());
+                break;
+            }
+        }
+        if (this->commandIndex[i] == -1) {
+            pc.printf("index = -1\n");
+            return false;
+        }
+    }
+	
+	pc.printf("Returning\n");
+    return true;
+}
 
-				}
-			}
-		}
-		break;
-	case PC:
-		for (int i = 0; i < sizeof(this->command)/sizeof(*this->command); i++) {
-			if (this->commandIndex[i] == -1 || this->commandIndex[i] == 0) { break; }
-			if (this->commandIndex[i] == 1){
-				connectToPC(this->typeRaw);
-			}else if (this->commandIndex[i] == 2){
-				disconnectToPC(this->typeRaw);
-			}else if (this->commandIndex[i] == 8){
-				if (this->commandValue[i] == "ON") { this->replyCommands = true; }
-				else if (this->commandValue[i] == "OFF") this->replyCommands = false; }
-			}
-		}
-		break;
-	case ColourSensor:
-		for (int i = 0; i < sizeof(this->command)/sizeof(*this->command); i++) {
-			if (this->commandIndex[i] == -1 || this->commandIndex[i] == 0) { break; }
-			if (this->commandIndex[i] == 1) {
-				float integrationTime;
-				sscanf(this->commandValue[i].c_str(), "%f", &integrationTime);
-				if (integrationTime < 2.4 || integrationTime > 600) {
-					pc.printf("Integration Time invalid: %.3f", integrationTime);
-					continue;
-				}
-				setIntegrationTime(integrationTime);
-			}else if (this->commandIndex[i] == 2){
-				if (this->commandValue[i] == "ON"){
-					previewOnPC(true);
-				}else if (this->commandValue[i] == "OFF"){
-					previewOnPC(false);
-				}
-			}else if (this->commandIndex[i] == 3){
-				readColourSensor();
-			}
-		}
-			break;
-	case Servos:
-		for (int i = 0; i < sizeof(this->command)/sizeof(*this->command); i++) {
-			if (this->commandIndex[i] == -1 || this->commandIndex[i] == 0) { break; }
-			if (this->commandIndex[i] == 1){
-				testServos();
-			}else if (this->commandIndex[i] == 2){
-				resetServos();
-			}
-		}
-		break;
-	case Port:
-		for (int i = 0; i < sizeof(this->command)/sizeof(*this->command); i++) {
-			if (this->commandIndex[i] == -1 || this->commandIndex[i] == 0) { break; }
-			if (this->commandIndex[i] == 1) {
-				getPortInfo();
-			}else if (this->commandIndex[i] == 2){
-				int baudRate;
-				sscanf(this->commandValue[i].c_str(), "%i", &baudRate);
-				setPortBaudRate(baudRate);
-			}else if (this->commandIndex[i] == 3){
-				int parity = 0;
-				sscanf(this->commandValue[i].c_str(), "%i", &parity);
-				setPortParity(parity);
-			}
-		}
-		break;
-	default:
-		break;
-		}
-	}
+void Commander::executeCommand()
+{
+    switch (this->objectRaw) {
+        case MBED:
+            for (int i = 0; i < sizeof(this->command)/sizeof(*this->command); i++) {
+                if (this->commandIndex[i] == -1 || this->commandIndex[i] == 0) {
+                    break;
+                }
+                if (this->commandIndex[i] == 1) {
+                    hazBlock(this->typeRaw);
+                } else if (this->commandIndex[i] == 2) {
+                    getCurrentBlock(this->typeRaw);
+                } else if (this->commandIndex[i] == 3) {
+                    if (this->commandValue[i] == "maintanence") {
+
+                    }
+                }
+            }
+            break;
+        case PC:
+            for (int i = 0; i < sizeof(this->command)/sizeof(*this->command); i++) {
+                if (this->commandIndex[i] == -1 || this->commandIndex[i] == 0) {
+                    break;
+                }
+                if (this->commandIndex[i] == 1) {
+                    connectToPC(this->typeRaw);
+                } else if (this->commandIndex[i] == 2) {
+                    disconnectToPC(this->typeRaw);
+                } else if (this->commandIndex[i] == 8) {
+                    if (this->commandValue[i] == "ON") {
+                        this->replyCommands = true;
+                    } else if (this->commandValue[i] == "OFF") this->replyCommands = false;
+                }
+            }
+            break;
+        case ColourSensor:
+            for (int i = 0; i < sizeof(this->command)/sizeof(*this->command); i++) {
+                if (this->commandIndex[i] == -1 || this->commandIndex[i] == 0) {
+                    break;
+                }
+                if (this->commandIndex[i] == 1) {
+                    float integrationTime;
+                    sscanf(this->commandValue[i].c_str(), "%f", &integrationTime);
+                    if (integrationTime < 2.4 || integrationTime > 600) {
+                        pc.printf("Integration Time invalid: %.3f", integrationTime);
+                        continue;
+                    }
+                    setIntegrationTimeTo(integrationTime);
+                } else if (this->commandIndex[i] == 2) {
+                    if (this->commandValue[i] == "ON") {
+                        previewOnPC(true);
+                    } else if (this->commandValue[i] == "OFF") {
+                        previewOnPC(false);
+                    }
+                } else if (this->commandIndex[i] == 3) {
+                    readColourSensor();
+                }
+            }
+            break;
+        case Servos:
+            for (int i = 0; i < sizeof(this->command)/sizeof(*this->command); i++) {
+                if (this->commandIndex[i] == -1 || this->commandIndex[i] == 0) {
+                    break;
+                }
+                if (this->commandIndex[i] == 1) {
+                    testServos();
+                } else if (this->commandIndex[i] == 2) {
+                    resetServos();
+                }
+            }
+            break;
+        case Port:
+            for (int i = 0; i < sizeof(this->command)/sizeof(*this->command); i++) {
+                if (this->commandIndex[i] == -1 || this->commandIndex[i] == 0) {
+                    break;
+                }
+                if (this->commandIndex[i] == 1) {
+                    getPortInfo();
+                } else if (this->commandIndex[i] == 2) {
+                    int baudRate;
+                    sscanf(this->commandValue[i].c_str(), "%i", &baudRate);
+                    setPortBaudRate(baudRate);
+                } else if (this->commandIndex[i] == 3) {
+                    int parity = 0;
+                    sscanf(this->commandValue[i].c_str(), "%i", &parity);
+                    setPortParity(parity);
+                }
+            }
+            break;
+        default:
+            break;
+    }
+}
 
 
-	void Commander::resetVariables(){
-		this->object = "";
-		this->objectRaw = InvalidObject;
-		memset(this->command, 0, sizeof(this->command));
-		memset(this->commandValue, 0, sizeof(this->command));
-		memset(this->commandIndex, 0, sizeof(this->commandIndex));
-		this->typeRaw = InvalidType;
-		this->typeChar = '\0';
-	}
+void Commander::resetVariables()
+{
+    this->object = "";
+    this->objectRaw = InvalidObject;
+    memset(this->command, 0, sizeof(this->command));
+    memset(this->commandValue, 0, sizeof(this->command));
+    memset(this->commandIndex, -1, sizeof(this->commandIndex));
+    this->typeRaw = InvalidType;
+    this->typeChar = '\0';
+}