Diff: commander.cpp

Revision:

23:db91aaa43a9e

Parent:

20:4e0f0944f28f

Child:

25:792540d69c49

--- a/commander.cpp	Thu Nov 26 15:47:17 2015 +0000
+++ b/commander.cpp	Sat Nov 28 23:28:22 2015 +0000
@@ -12,19 +12,20 @@
 const int kCommandValueBufferSize = 80;
 const int kObjectBufferSize = 20;
 
-string CommandObjectValue [5] = { "mbed", "pc", "colour_sensor", "servos", "port" };
-string CommandObjectCommandsValue [5][kMaxCommandCount] = {
-    {"mbed", "haz-block", "read-current-block", "mode", "sort", "", "", "", "", ""},
-    {"pc", "connect", "disconnect", "", "", "", "", "", "reply-commands", "exit"},
-    {"colour_sensor", "i-time", "preview", "", "", "", "", "", "", ""},
-    {"servos", "test", "reset", "stop-test", "", "", "", "", "", ""},
-    {"port", "init", "b-rate", "parity", "", "", "", "", "", ""}
+string CommandObjectValue [6] = { "mbed", "pc", "colour_sensor", "servos", "port", "break_beam" };
+string CommandObjectCommandsValue [6][kMaxCommandCount] = {
+		{"mbed", "haz-block", "read-current-block", "mode", "sort", "", "", "", "", ""},
+		{"pc", "connect", "disconnect", "", "", "", "", "", "reply-commands", "exit"},
+		{"colour_sensor", "i-time", "preview", "", "", "", "", "", "", ""},
+		{"servos", "test", "reset", "stop-test", "", "", "", "", "", ""},
+		{"port", "init", "b-rate", "parity", "", "", "", "", "", ""},
+		{"break_beam", "test", "", "", "", "", "", "", "", ""},
 };
 
 Commander::Commander()
 {
-    replyCommands = false;
-    this->resetVariables();
+	replyCommands = false;
+	this->resetVariables();
 }
 
 Commander::~Commander()
@@ -34,286 +35,299 @@
 
 void Commander::decodeCommand(CommandTypeRaw type)
 {
-    this->resetVariables();
-    this->typeRaw = type;
-    this->typeChar = CommandTypeValue[type];
+	this->resetVariables();
+	this->typeRaw = type;
+	this->typeChar = CommandTypeValue[type];
 
-    this->readCommandObject();
-    if (this->objectRaw == InvalidObject) {
-        pc.printf("DEBUG:Invalid command object.\n");
-        return;
-    }
+	this->readCommandObject();
+	if (this->objectRaw == InvalidObject) {
+		pc.printf("DEBUG:Invalid command object.\n");
+		return;
+	}
 
-    if (this->readCommand(this->objectRaw) == false) {
-//        this->description();
-        pc.printf("DEBUG:Invalid command.\n");
-        return;
-    } else if (connectedToPC == true || (this->typeRaw == Set && this->objectRaw == PC && this->commandIndex[0] == 1)) {
-        this->executeCommand();
-        if (this->replyCommands == true) {
-            pc.printf("VALUE:%s:VALUE\n", this->description().c_str());
-        }
-        //TODO: Send request with response descriptor.
-//        pc.("DEBUG:Finished executine command");
-        return;
-    }
+	if (this->readCommand(this->objectRaw) == false) {
+		//        this->description();
+		pc.printf("DEBUG:Invalid command.\n");
+		return;
+	} else if (connectedToPC == true || (this->typeRaw == Set && this->objectRaw == PC && this->commandIndex[0] == 1)) {
+		this->executeCommand();
+		if (this->replyCommands == true) {
+			pc.printf("VALUE:%s:VALUE\n", this->description().c_str());
+		}
+		//TODO: Send request with response descriptor.
+		//        pc.("DEBUG:Finished executine command");
+		return;
+	}
 
-    pc.printf("INFO:Not connected to PC. %i\n", this->commandIndex[0]);
-    return;
+	pc.printf("INFO:Not connected to PC. %i\n", this->commandIndex[0]);
+	return;
 }
 
 std::string Commander::description()
 {
-    string str;
-    str.append("Command type:\t");
-    str.append(&this->typeChar);
-    str.append("\nCommand object:\t" + this->object);
+	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("\nCommand:\t" + this->command[i] + "\nValue:\t" + this->commandValue[i].c_str() + " \n");
-    }
-    return str;
+	for (int i = 0; i < sizeof(this->command)/sizeof(*this->command); i++) {
+		if (this->command[i] == "") {
+			break;
+		}
+		str.append("\nCommand:\t" + this->command[i] + "\nValue:\t" + this->commandValue[i].c_str() + " \n");
+	}
+	return str;
 }
 
 void Commander::readCommandObject()
 {
-    char objectInitiator = '<';
-    char objectTerminator = '>';
+	char objectInitiator = '<';
+	char objectTerminator = '>';
 
-    char nextChar = '\0';
+	char nextChar = '\0';
 
-    do {
-        nextChar = pc.getc();
-    } while (nextChar != objectInitiator);
+	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;
+	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;
-        }
-    }
+	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;
-    return;
+	this->objectRaw = InvalidObject;
+	return;
 }
 
 bool Commander::readCommand(CommandObjectRaw objectRaw)
 {
-    char nextChar = '\0';
-    char commandCharArray [kMaxCommandCount - 1][kCommandValueBufferSize] = { '\0' };
-    char commandValueArray [kMaxCommandCount -1][kCommandValueBufferSize] = { '\0' };
+	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();
+	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 (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++;
-        }
-    }
+		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");
-                this->command[i] = tempCommandStr;
-//                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;
-        }
-    }
+	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");
+				this->command[i] = tempCommandStr;
+				//                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;
+	//	pc.printf("Returning\n");
+	return true;
 }
 
 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") {
-                        currentMode = Maintanence;
-                        pc.printf("INFO:Running in maintanence mode.\n");
-                    } else if (this->commandValue[i] == "normal") {
-                        currentMode = Normal;
-                        pc.printf("INFO:Running in nomal mode.\n");
-                    } else if (this->commandValue[i] == "none") {
-                        currentMode = None;
-                        pc.printf("INOF:Running in none mode");
-                    }
-                } else if (this->commandIndex[i] == 4 && currentMode == Normal) {
-                    if (this->commandValue[i] == "start") {
-                        currentState = Start;
-                        pc.printf("INFO:Starting sorting.\n");
-                    } else if (this->commandValue[i] == "pause") {
-                        currentState = Pause;
-                        pc.printf("INFO:Pausing sorting.\n");
-                    }
-                }
-            }
-            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("ERROR:Integration Time invalid: %.3f\n", 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) {
-                    if (this->commandValue[i] == "start") {
-                        testServos(Start);
-                    } else if (this->commandValue[i] == "pause") {
-                        testServos(Pause);
-                    }
-                } else if (this->commandIndex[i] == 2) {
-                    resetServos();
-                } else if (this->commandIndex[i] == 3) {
+	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") {
+					currentMode = Maintanence;
+					pc.printf("INFO:Running in maintanence mode.\n");
+				} else if (this->commandValue[i] == "normal") {
+					currentMode = Normal;
+					pc.printf("INFO:Running in nomal mode.\n");
+				} else if (this->commandValue[i] == "none") {
+					currentMode = None;
+					pc.printf("INOF:Running in none mode");
+				}
+			} else if (this->commandIndex[i] == 4 && currentMode == Normal) {
+				if (this->commandValue[i] == "start") {
+					currentState = Start;
+					pc.printf("INFO:Starting sorting.\n");
+				} else if (this->commandValue[i] == "pause") {
+					currentState = Pause;
+					pc.printf("INFO:Pausing sorting.\n");
+				}
+			}
+		}
+		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("ERROR:Integration Time invalid: %.3f\n", 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) {
+				if (this->commandValue[i] == "start") {
+					testServos(Start);
+				} else if (this->commandValue[i] == "pause") {
+					testServos(Pause);
+				}
+			} else if (this->commandIndex[i] == 2) {
+				resetServos();
+			} else if (this->commandIndex[i] == 3) {
 
-                }
-            }
-            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;
-    }
-    return;
+			}
+		}
+		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;
+	case BreakBeam:
+		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) {
+				if (this->commandValue[i] == "start"){
+					testBreakBeams(Start);
+				}else if (this->commandValue[i] == "pause"){
+					testBreakBeams(Pause);
+				}
+			}
+		}
+	default:
+		break;
+	}
+	return;
 }
 
 
 void Commander::resetVariables()
 {
-    this->object = "";
-    this->objectRaw = InvalidObject;
-    for (int i = 0; i < sizeof(this->command)/sizeof(*this->command); i ++) {
-        this->command[i].clear();
-        this->commandValue[i].clear();
-        this->commandIndex[i] = -1;
-    }
-    this->typeRaw = InvalidType;
-    this->typeChar = '\0';
+	this->object = "";
+	this->objectRaw = InvalidObject;
+	for (int i = 0; i < sizeof(this->command)/sizeof(*this->command); i ++) {
+		this->command[i].clear();
+		this->commandValue[i].clear();
+		this->commandIndex[i] = -1;
+	}
+	this->typeRaw = InvalidType;
+	this->typeChar = '\0';
 }