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("|");
}
}
}
void Setup() {
/* Initialize LCD */
lcd_initialize();
/* Clear LCD */
lcd_clear();
/* Display message on LCD */
lcd_buffer_print(LCD_LINE2, " TEST ");
/* Initialize motors */
Motors_Init();
/* Turn on motors relay */
Motors_On();
/* Send arm signal to motors */
Motor_Arm(MOTOR_UPPER);
Motor_Arm(MOTOR_BOTTOM);
/* Initialize servos */
Servos_Init();
/* Initialize sonar */
sonarInitialize(); //must be initialized before IIC, otherwise it will not work
/* Setup the 12-bit A/D converter */
S12ADC_init();
/* Initialize I2C with control */
riic_ret_t iic_ret = RIIC_OK;
iic_ret |= riic_master_init();
while (RIIC_OK != iic_ret) {
nop(); /* Failure to initialize here means demo can not proceed. */
}
/* Setup Compare Match Timer */
CMT_init();
/* Initialize PID structure used for PID properties */
PID_Init(&z_axis_PID, 0.7, 0.05, 0.30, dt, 0, 0.5); //0.7 0.05 0.15
PID_Init(&Pitch_PID, 1, 0, 0.01, dt, -30, 30);
PID_Init(&Roll_PID, 1, 0, 0.01, dt, -30, 30);
Init_AVG(0, &pitchAVG);
Init_AVG(0, &rollAVG);
/* Make the port connected to SW1 an input */
PORT4.PDR.BIT.B0 = 0;
/*MPU6050 Initialization*/
MPU6050_Test_I2C();
Setup_MPU6050();
Calibrate_Gyros();
// Calibrate_Accel();
/*Kalman Initialization*/
init_Kalman();
//MS5611-01BA01 init
// MS5611_Init();
desiredState.key.motor_diff_us = 0;
desiredState.key.abs.pos.z = 0.20;
altitudeValue = 200;
mainWDT = WDT_Init(500, Fallback);
WDT_Start(&mainWDT);
sonarWDT = WDT_Init(60, Sonar_Fallback);
WDT_Start(&sonarWDT);
}
