// SETUP void setup() { // For algorithm speed checking loopPeriod = 0; lastLoop = micros(); // Lap timer lapTimer.setInterval(100, IncrementTime); lapTimer.disable(0); // Initialize LCD lcd.begin(16, 2); lcd.setCursor(0,0); // Turn signals pinMode(LEFT_TURN_LED, OUTPUT); pinMode(RIGHT_TURN_LED, OUTPUT); digitalWrite(LEFT_TURN_LED, 0); digitalWrite(RIGHT_TURN_LED, 0); turnSignalTimer.setInterval(50, TurnSignal); // Force motors off by default MotorSpeed(LEFT_MOTOR, 0); MotorSpeed(RIGHT_MOTOR, 0); // Parameters need to be loaded from EEPROM LoadFromEEPROM(); // Intro text Clear(); Cursor(TOP, 0); Print("FAST ORANGE"); Cursor(BOTTOM, 0); #ifdef DEBUG_MODE Print("Debug Mode"); #else Print("Race Mode"); #endif delay(1000); currentState = menu; }
void handleSerialCommands() { DebugPrint("Function handleSerialCommands"); StringArray commandList = GetCommandList(INPUT_STRING); String command = commandList.GetString(0); DebugPrint("Command was: " + command); // RobotControl // Switch Robot On if (command == "ON") { GLOBAL_IS_CONNECTED = true; digitalWrite(13,HIGH); Servos_Init(); PrintMessage(command); } // Switch Robot Off else if (command == "OFF") { GLOBAL_IS_CONNECTED = false; PrintMessage("OFF"); digitalWrite(13,LOW); Servos_Release(); } // EEPROM Handling // Store Movement List else if (command == "STORE") { SaveToEEPROM(); } // Retrieve Movement List else if (command == "GET") { // PrintMessage("get"); LoadFromEEPROM(); } // Erase Movement List else if (command == "ERASE") { ClearEEPROM(); } // READ Specified EEPROM Address else if (command == "READ") { handleReadCommand(commandList); } // Servohandling // Move Servo else if (command == "MOVE") { Move_Servo(commandList); /* int oldValue = servo.read(); int newValue = GetIntFromString(commandList.GetString(1)); if (oldValue <= newValue) { for (int i = oldValue; i <= newValue; i++) { servo.write(i); delay(10 * (5 - GLOBAL_SERVO_SPEED)); } } else { for (int i = oldValue; i >= newValue; i--) { servo.write(i); delay(10 * (5 - GLOBAL_SERVO_SPEED)); } } */ /* if (commandList.GetElementCount() >= 4) Move_Servo(commandList); else PrintMessage("Error: not enough Arguments, Example MOVE;SERVONUMBER;SPEED;VALUE"); */ } // Set Speed else if (command == "SPEED") { int newSpeed = GetIntFromString(commandList.GetString(1)); if (newSpeed >= 0 && newSpeed <= 5) { DebugPrint("GLOBAL_SERVO_SPEED set to: " + String(newSpeed)); GLOBAL_SERVO_SPEED = newSpeed; SaveSpeed(newSpeed); } PrintMessage("Speed set to: " + String(GLOBAL_SERVO_SPEED)); } else if (command == "SPEED?") { PrintMessage(String(GLOBAL_SERVO_SPEED)); } // Play LISTPOS #position else if (command == "LISTPOS") { int numberPositions = commandList.GetElementCount(); for (int i = 1; i < numberPositions; i++) { int pos = GetIntFromString(commandList.GetString(i)); DebugPrint("Try to get position " + String(pos) + " from List..." ); DebugPrint("Number of Stored Commands is " + String(GetAnzahlBefehle())); if (pos > -1 && GetAnzahlBefehle() > pos) { String listCommand = GetCommandWithServoNumber(pos); StringArray tempArray = GetCommandList(listCommand); DebugPrint(tempArray.GetAsConcatedString()); Move_Servo(tempArray); } } } // Opens the Gripper else if (command == "OPEN_GRIPPER") { Open_Gripper(); PrintMessage(String("Gripper opened...")); } // Closes the Gripper else if (command == "CLOSE_GRIPPER") { Close_Gripper(); PrintMessage(String("Gripper closed...")); } // Other Command else { DebugPrint("Unknown Command"); for (int i = 0; i < commandList.GetElementCount(); i++) { PrintMessage(commandList.GetString(i)); PrintMessage("|"); } } }