Refactor CLI submodule

Move command parsing to doCommand
Parse command with splitString instead of stringToken
Trim commands
Move cliTestMotor to the bottom
Rename parseInput to handleInput, which is more clear
Move motor test function to motors.ino

gazebo/Arduino.h

@@ -41,13 +41,17 @@ class __FlashStringHelper;
 // https://www.arduino.cc/reference/en/language/variables/data-types/stringobject/
 class String: public std::string {
 public:
-	String(const char *str = "") : std::string(str) {}
+	String(const char *str = "") : std::string(str ? str : "") {}
 	long toInt() const { return atol(this->c_str()); }
 	float toFloat() const { return atof(this->c_str()); }
 	bool isEmpty() const { return this->empty(); }
 	void toCharArray(char *buf, unsigned int bufsize, unsigned int index = 0) const {
 		strlcpy(buf, this->c_str() + index, bufsize);
 	}
+	void trim() {
+		this->erase(0, this->find_first_not_of(" \t\n\r"));
+		this->erase(this->find_last_not_of(" \t\n\r") + 1);
+	}
 };
 
 class Print;

gazebo/flix.h

@@ -34,8 +34,8 @@ void controlTorque();
 void showTable();
 void sendMotors();
 bool motorsActive();
+void doCommand(String str);
 void cliTestMotor(uint8_t n);
-String stringToken(char* str, const char* delim);
 void normalizeRC();
 void printRCCal();
 void processMavlink();

gazebo/simulator.cpp

@@ -81,7 +81,7 @@ public:
 		attitude.setYaw(this->model->WorldPose().Yaw());
 
 		control();
-		parseInput();
+		handleInput();
 		processMavlink();
 
 		applyMotorForces();