// 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("|");
}
}
}
