void RealConnection::handleReadCommandHeader(const boost::system::error_code & error, size_t size)
{
if (error) {
_lastErrorCode = error;
disconnect();
return;
}
std::istream inputStream(&_incoming);
CommandSize commandSize;
inputStream.read(reinterpret_cast<char *>(&commandSize), sizeof(commandSize));
size -= sizeof(commandSize);
if (size >= commandSize) {
handleReadCommand(error, size, commandSize);
} else {
boost::asio::async_read(_socket, _incoming, boost::asio::transfer_at_least(commandSize - size),
std::bind(&RealConnection::handleReadCommand, getDerivedPointer(),
std::placeholders::_1,
std::placeholders::_2,
commandSize));
}
}
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("|");
}
}
}