#include "serial.h"
void execute_function()
{
int data = Serial.read();
int param1, param2;
Serial.println(data);
if(data >= SET_SERVO_POS) //This means we have parameters
{
param1 = Serial.read();
param2 = Serial.read();
Serial.println(param1);
Serial.println(param2);
}
//command is stored in first position of the array
switch(data)
{
//parameterless functions
case LED_ON:
led_on(); break;
case LED_OFF:
led_off(); break;
case LIFT_CANS:
liftCans(); break;
case GRAB_FIRST:
grabFirst(); break;
case SCORE:
scoreCans(); break;
case CLOSEWINGS:
closeWings(); break;
case OPENWINGS:
openWings(); break;
case REVERSE:
reverse(); break;
//two-parameter functions
case SET_SERVO_POS:
set_servo_position(param1, param2);
break;
//functions with return values
case ISCAN:
Serial.write(isCan()); break;
case ISBUTTON:
Serial.println("Trying to return button");
Serial.write(isButton());
break;
default:
Serial.println("CMD UNRECOGNIZED: ");
Serial.println(data);
// Command was not recognized
break;
}
}