//Function to be called when we have received the full packet
bool SerialParser::interpretData(){
float a,b,c,d;
char *data = getMessage();
if (data == NULL) return false;
debug_print("%s\n",data);
if (verifyCs(data) == false){
debug_print("error cs\n");
releaseMessage();
return true;
}
float incom_data;
if (getFloat(data+3, &incom_data) == 0 ){
goto scanf_error;
}
switch(data[2]){
case 'T': Ttwist = incom_data; debug_print("Ttwist %f\n", Ttwist); asser.angleCommand.SetSpeed(Ttwist); break;
case 'V': Vtwist = incom_data; debug_print("Vtwist %f\n", Vtwist); asser.distanceCommand.SetSpeed(Vtwist); break;
case 'C': distance = incom_data;
data = strchr(data,';'); if (data == NULL) goto scanf_error;
data += 1;
if (getFloat(data, &incom_data) == 0) goto scanf_error;
angle = incom_data;
data = strchr(data,';'); if (data == NULL) goto scanf_error;
data += 1;
if (getFloat(data, &incom_data) == 0) goto scanf_error;
finalDistanceSpeed = incom_data;
data = strchr(data,';'); if (data == NULL) goto scanf_error;
data += 1;
if (getFloat(data, &incom_data) == 0) goto scanf_error;
finalAngleSpeed = incom_data;
data = strchr(data,';'); if (data == NULL) goto scanf_error;
data += 1;
if (getInt(data, &uuid) == 0) goto scanf_error;
asser.setTarget(distance, angle, finalDistanceSpeed, finalAngleSpeed, uuid);
break;
case 'K': a = incom_data;
data = strchr(data,';'); if (data == NULL) goto scanf_error;
data += 1;
if (getFloat(data, &incom_data) == 0) goto scanf_error;
b = incom_data;
data = strchr(data,';'); if (data == NULL) goto scanf_error;
data += 1;
if (getFloat(data, &incom_data) == 0) goto scanf_error;
finalDistanceSpeed = incom_data;
data = strchr(data,';'); if (data == NULL) goto scanf_error;
data += 1;
if (getInt(data, &mode) == 0) goto scanf_error;
data = strchr(data,';'); if (data == NULL) goto scanf_error;
data += 1;
if (getInt(data, &uuid) == 0) goto scanf_error;
asser.setTargetXY(a, b, finalDistanceSpeed, mode, uuid);
break;
case 'P': a = incom_data;
data = strchr(data,';'); if (data == NULL) goto scanf_error;
data += 1;
if (getFloat(data, &incom_data) == 0) goto scanf_error;
b = incom_data;
data = strchr(data,';'); if (data == NULL) goto scanf_error;
data += 1;
if (getFloat(data, &incom_data) == 0) goto scanf_error;
c = incom_data;
data = strchr(data,';'); if (data == NULL) goto scanf_error;
data += 1;
if (getFloat(data, &incom_data) == 0) goto scanf_error;
d = incom_data;
asser.distanceCommand.setParameters(a,b,c,d);
break;
case 'Q': a = incom_data;
data = strchr(data,';'); if (data == NULL) goto scanf_error;
data += 1;
if (getFloat(data, &incom_data) == 0) goto scanf_error;
b = incom_data;
data = strchr(data,';'); if (data == NULL) goto scanf_error;
data += 1;
if (getFloat(data, &incom_data) == 0) goto scanf_error;
c = incom_data;
data = strchr(data,';'); if (data == NULL) goto scanf_error;
data += 1;
if (getFloat(data, &incom_data) == 0) goto scanf_error;
d = incom_data;
asser.angleCommand.setParameters(a,b,c,d);
break;
case 'E': a = incom_data;
data = strchr(data,';'); if (data == NULL) goto scanf_error;
data += 1;
if (getFloat(data, &incom_data) == 0) goto scanf_error;
b = incom_data;
asser.distanceCommand.setThreshold(a);
asser.angleCommand.setThreshold(b);
break;
case 'A': data = strchr(data,';'); if (data == NULL) goto scanf_error;
data += 1;
if (getInt(data, &uuid) == 0) goto scanf_error;
asser.setTargetAngle(incom_data, uuid);
break;
case 'D': //emergency stop
asser.distanceCommand.stop((bool)(incom_data));
asser.angleCommand.stop((bool)(incom_data));
break;
case 'S': SStatus = (int)incom_data;
asser.distanceCommand.Reset(asser.distance);
asser.distanceCommand.running = false;
asser.angleCommand.Reset(asser.angle);
asser.angleCommand.running = false;
asser.commandUUID = 0;
break;
case 'U': UPower = (bool)incom_data; debug_print("UPower %d\n", UPower); asser.enable(UPower); break;
case 'M': debug_print("metrics %d\n", (bool)incom_data); metricsEnabled = (bool)incom_data; break;
case 'O': debug_print("odo %d\n", (bool)incom_data); odoEnabled = (bool)incom_data; break;
case '{': KpPoLin = incom_data;
data = strchr(data,';'); if (data == NULL) goto scanf_error;
data += 1;
if (getFloat(data, &incom_data) == 0) goto scanf_error;
KiPoLin = incom_data;
data = strchr(data,';'); if (data == NULL) goto scanf_error;
data += 1;
if (getFloat(data, &incom_data) == 0) goto scanf_error;
KdPoLin = incom_data;
debug_print("lin pid %f %f %f\n", KpPoLin, KiPoLin, KdPoLin);
asser.PidDistance.SetTunings(KpPoLin, KiPoLin, KdPoLin);
break;
case '(': KpPoAng = incom_data;
data = strchr(data,';'); if (data == NULL) goto scanf_error;
data += 1;
if (getFloat(data, &incom_data) == 0) goto scanf_error;
KiPoAng = incom_data;
data = strchr(data,';'); if (data == NULL) goto scanf_error;
data += 1;
if (getFloat(data, &incom_data) == 0) goto scanf_error;
KdPoAng = incom_data;
debug_print("ang pid %f %f %f\n", KpPoAng, KiPoAng, KdPoAng);
asser.PidAngle.SetTunings(KpPoAng, KiPoAng, KdPoAng);
break;
case 'x': asser.setX(incom_data);
break;
case 'y': asser.setY(incom_data);
break;
case 'w': asser.setAngle(incom_data);
break;
case 'c': asser.setTickToAngle(incom_data);
data = strchr(data,';'); if (data == NULL) goto scanf_error;
data += 1;
if (getFloat(data, &incom_data) == 0) goto scanf_error;
asser.setTickToDistance(incom_data);
break;
case 'd': asser.angleCommand.SetSampleTime(incom_data);
data = strchr(data,';'); if (data == NULL) goto scanf_error;
data += 1;
if (getFloat(data, &incom_data) == 0) goto scanf_error;
asser.distanceCommand.SetSampleTime(incom_data);
break;
default: debug_print("error\n"); break;
}
goto end_parse;
scanf_error:
debug_print("error scanf\n");
end_parse:
releaseMessage();
return true;
}
void PeerToPeerChannel<T>::routeMessage(messages::Message* const message) {
// p2p messages should always have only one recipent.
const String& recipentName = *message->getRecipients().begin();
// Send the message to the recipent.
services::EngineService* const recipent = getServiceRegistry()->locate(recipentName);
recipent->receive(message);
// Release handled messages.
if (message->wasHandled()) {
if (getMessageReleaseStrategy() == messages::MessageReleaseStrategy::ReleaseWhenHandled) releaseMessage(message);
}
}
