void RpiRemote::read_pfo()
{
int counter = 0;
char token[20];
Tokeniser tok(buf, ',');
while (tok.next(token, 3))
{
switch (counter)
{
case 1: //command as char
{
RpiCmd = token;
if (RpiCmd == "ro") {
// cmd: off
robot->setNextState(STATE_OFF, 0);
}
if (RpiCmd == "rh") {
// cmd: home
robot->setNextState(STATE_PERI_FIND, 0);
}
// ------------robot stats---------------------------
boolean statsOverride ;
boolean statsMowTimeTotalStart ;
unsigned int statsMowTimeMinutesTripCounter ;
unsigned long statsMowTimeMinutesTotal ;
float statsMowTimeHoursTotal ;
int statsMowTimeMinutesTrip ;
unsigned long nextTimeRobotStats ;
[b] // ------------robot mower communication standard---
boolean rmcsUse;
unsigned long RMCS_interval_state;
unsigned long RMCS_interval_motor_current;
unsigned long RMCS_interval_sonar;
unsigned long RMCS_interval_bumper;
unsigned long RMCS_interval_odometry;
unsigned long RMCS_interval_perimeter;
unsigned long nextTimeRMCSInfo;
unsigned long rmcsInfoLastSendState;
unsigned long rmcsInfoLastSendMotorCurrent;
unsigned long rmcsInfoLastSendSonar;
unsigned long rmcsInfoLastSendBumper;
unsigned long rmcsInfoLastSendOdometry;
unsigned long rmcsInfoLastSendPeri;[/b]if (millis() >= nextTimePfodLoop){
nextTimePfodLoop = millis() + 200;
rc.run();
}
[b]
if (rmcsUse == true and millis() >= nextTimeRMCSInfo ) {
nextTimeRMCSInfo = millis() + 100;
rmcsPrintInfo(Console);
}
if (millis() >= nextTimeInfo) {
nextTimeInfo = millis() + 1000;
if (rmcsUse == false) {
printInfo(Console);[/b]
printErrors();
ledState = ~ledState;// ------ mower stats-------------------------------------------
statsOverride = false; // if set to true mower stats are overwritten - be careful
statsMowTimeMinutesTotal = 300;
statsBatteryChargingCounterTotal = 11;
statsBatteryChargingCapacityTotal = 30000;
[b] // ------------robot mower communication standard---
rmcsUse = true; // if set robot mower communication standard (NMEA) is used.
RMCS_interval_state = 1000;
RMCS_interval_motor_current = 1000;
RMCS_interval_sonar = 1000;
RMCS_interval_bumper = 1000;
RMCS_interval_odometry = 1000;
RMCS_interval_perimeter = 1000;[/b]
// -----------configuration end-------------------------------------// robot mower communication standard
void Robot::rmcsPrintInfo(Stream &s){
// Period since last send
unsigned long now = millis();
unsigned long durationSinceLastSendState = now - rmcsInfoLastSendState;
unsigned long durationSinceLastSendMotorCurrent = now - rmcsInfoLastSendMotorCurrent;
unsigned long durationSinceLastSendSonar = now - rmcsInfoLastSendSonar;
unsigned long durationSinceLastSendBumper = now - rmcsInfoLastSendBumper;
unsigned long durationSinceLastSendOdometry = now - rmcsInfoLastSendOdometry;
unsigned long durationSinceLastSendPeri = now - rmcsInfoLastSendPeri;
if (consoleMode == CONSOLE_OFF) {
} else {
if (durationSinceLastSendState > RMCS_interval_state){
rmcsInfoLastSendState = now;
// ROBOT State, Timestamp, state, error code, battery, charging
Streamprint(s, "$RMSTA,%6u,",((millis()-stateStartTime)/1000));
Streamprint(s, "%4s,", stateNames[stateCurr]);
Streamprint(s, "%4s,", "E000");
Streamprint(s, "%2d.%01d,",(int)batVoltage, (int)((batVoltage *10) - ((int)batVoltage*10)));
Streamprint(s, "%2d.%01d,",(int)chgVoltage, (int)((chgVoltage *10) - ((int)chgVoltage*10)));
Streamprint(s, "%2d.%01d",(int)chgCurrent, (int)((abs(chgCurrent) *10) - ((int)abs(chgCurrent)*10)) );
Streamprint(s, "rn");
}
if (durationSinceLastSendMotorCurrent > RMCS_interval_motor_current){
rmcsInfoLastSendMotorCurrent = now;
// ROBOT motor current, Timestamp, Motor left A, Motor right A, Motor Mow A, Motor left trigger, motor right trigger, motor mow trigger
Streamprint(s, "$RMMOT,%6u,", (millis()-stateStartTime)/1000);
Streamprint(s, "%4d ,",(int)motorLeftSense);
Streamprint(s, "%4d ,",(int)motorRightSense);
Streamprint(s, "%4d ,",(int)motorMowSense);
Streamprint(s, "%4d ,",motorLeftSenseCounter);
Streamprint(s, "%4d ,",motorRightSenseCounter);
Streamprint(s, "%4d ",motorMowSenseCounter);
Streamprint(s, "rn");
}
if (durationSinceLastSendSonar > RMCS_interval_sonar and sonarUse){
rmcsInfoLastSendSonar = now;
// ROBOT sonar sensor data, Timestamp, sonar left dist, sonar right dist, sonar center dist, sonar left trigger, sonar right trigger, sonar center trigger
Streamprint(s, "$RMSON,%6u,", (millis()-stateStartTime)/1000);
Streamprint(s, "%4d ,",sonarDistLeft);
Streamprint(s, "%4d ,",sonarDistRight);
Streamprint(s, "%4d ,",sonarDistCenter);
Streamprint(s, "%4d ,",sonarDistCounter);
Streamprint(s, "%4d ,",sonarDistCounter);
Streamprint(s, "%4d ",sonarDistCounter);
Streamprint(s, "rn");
}
if (durationSinceLastSendBumper > RMCS_interval_bumper and bumperUse){
rmcsInfoLastSendBumper = now;
// ROBOT bumper data, Timestamp, bumper left value, bumper right value, bumper center value, bumper left trigger, bumper right trigger, bumper center trigger
Streamprint(s, "$RMBUM,%6u,", (millis()-stateStartTime)/1000);
Streamprint(s, "%4d ,",bumperLeftCounter);
Streamprint(s, "%4d ,",bumperRightCounter);
Streamprint(s, "%4d ,",0);
Streamprint(s, "%4d ,",bumperLeft);
Streamprint(s, "%4d ,",bumperRight);
Streamprint(s, "%4d ",0);
Streamprint(s, "rn");
}
if (durationSinceLastSendOdometry > RMCS_interval_odometry and odometryUse){
rmcsInfoLastSendOdometry = now;
// ROBOT odometry data, Timestamp, odometry left value, odometry right value
Streamprint(s, "$RMODO,%6u,", (millis()-stateStartTime)/1000);
Streamprint(s, "%4d ,",odometryLeft);
Streamprint(s, "%4d ",odometryRight);
Streamprint(s, "rn");
}
if (durationSinceLastSendPeri > RMCS_interval_perimeter and perimeterUse){
rmcsInfoLastSendPeri = now;
// ROBOT Perimeter data, Timestamp, perimeter value, perimeter in/out, perimeter counter
Streamprint(s, "$RMPER,%6u,", (millis()-stateStartTime)/1000);
Streamprint(s, "%4d ,",perimeterMag);
Streamprint(s, "%4d ,",perimeterInside);
Streamprint(s, "%4d ",perimeterCounter);
Streamprint(s, "rn");
}
}
}Look at the last picture.
void Robot::RaspberryPISendStat () {
String lineToSend;
lineToSend = "RMSTA,";
lineToSend = lineToSend + millis();
lineToSend = lineToSend + ",";
lineToSend = lineToSend + stateNames[stateCurr];
lineToSend = lineToSend + ",";
lineToSend = lineToSend + odometryX;
lineToSend = lineToSend + ",";
lineToSend = lineToSend + odometryY;
lineToSend = lineToSend + ",";
lineToSend = lineToSend + prevYawCalcOdo;
lineToSend = lineToSend + ",";
lineToSend = lineToSend + batVoltage;
lineToSend = lineToSend + ",";
lineToSend = lineToSend + imu.ypr.yaw;
lineToSend = lineToSend + ",";
lineToSend = lineToSend + imu.ypr.pitch;
lineToSend = lineToSend + ",";
lineToSend = lineToSend + imu.ypr.roll;
lineToSend = lineToSend + ",";
MyRpi.writePi(lineToSend);
}
void Robot::RaspberryPISendDebug (String data) {
String lineToSend;
lineToSend = "RMDEB,";
lineToSend = lineToSend + millis();
lineToSend = lineToSend + ",";
lineToSend = lineToSend + data;
lineToSend = lineToSend + ",";
MyRpi.writePi(lineToSend);
}class STA(TalkerSentence):
""" non stop message from DUE to PI for state, localisation etc....
"""
fields = (
("Millis", "millis"),
("State", "state"),
("OdometryX", "odox"),
("OdometryY", "odoy"),
("PrevYaw", "prevYaw"),
("BatVoltage", "batVoltage"),
("ActualYaw", "yaw"),
("ActualPitch", "pitch"),
("ActualRoll", "roll"),
)
class DEB(TalkerSentence):
""" message from DUE TO PI for debug etc....
"""
fields = (
("Hubtype", "Hubtype"),
("Debug", "debug"),
)
class PFO(TalkerSentence):
""" message between pi and due with same def as pfod
"""
fields = (
("Command", "command"),
("Val1", "val1"),
("Val2", "val2"),
("Val3", "val3"),
("Val4", "val4"),
("Val5", "val5"),
("Val6", "val6"),
("Val7", "val7"),
("Val8", "val8"),
("Val9", "val9"),
("Val10", "val10"),
)