// Display the menu on screen
void ShowMenu()
{
// Show menu item on top row
Clear();
Cursor(TOP, 0);
Print(parameters[menuIndex]->Name);
Print(" ", parameters[menuIndex]->Value);
// Show sensor info on bottom row. Useful for threshold calibration
Cursor(BOTTOM, 0);
Print(" ", analogRead(LEFT_SENSOR));
Print(" ", analogRead(RIGHT_SENSOR));
Print(" ", analogRead(CENTER_SENSOR));
Print(" ");
lcd.print(float(lapTime)/10.0); Print("s");
switch(ReadButton())
{
case UP: // Increase item value
holdCounter = (previousButton == UP) ? (holdCounter + 1) : 0;
previousButton = UP;
parameters[menuIndex]->Value += 1 + (holdCounter / 20);
break;
case DOWN: // Lower item value
holdCounter = (previousButton == DOWN) ? (holdCounter + 1) : 0;
previousButton = DOWN;
parameters[menuIndex]->Value -= (1 + (holdCounter / 20));
break;
case LEFT: // Next menu item
menuIndex = (menuIndex > 0) ? (menuIndex - 1) : (PARAMETER_COUNT - 1);
break;
case RIGHT: // Previous menu item
menuIndex = (menuIndex < PARAMETER_COUNT - 1) ? (menuIndex + 1) : 0;
break;
case SELECT:// Exit menu
delay(500);
if (ReadButton() == SELECT)
{
Clear();
Cursor(TOP, 0);
Print("Exiting menu");
SaveToEEPROM(); // Save values to EEPROM before exiting
delay(750);
currentState = moveStraight;
intersectionTurnFlag = 0;
lapTime = 0;
batteryCount = 1;
Clear();
return;
}
break;
}
delay(150);
}
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("|");
}
}
}
