void Robot::checkSonarPeriTrack() {
if (!sonarUse) return;
if (millis() < nextTimeCheckSonar) return;
nextTimeCheckSonar = millis() + 200;
if (millis() > timeToResetSpeedPeri) {
timeToResetSpeedPeri = 0; //brake the tracking during 6 secondes
ActualSpeedPeriPWM = MaxSpeedperiPwm ;
}
if (sonarRightUse) sonarDistRight = readSensor(SEN_SONAR_RIGHT);
//if (sonarLeftUse) sonarDistLeft = readSensor(SEN_SONAR_LEFT);
//if (sonarCenterUse) sonarDistCenter = readSensor(SEN_SONAR_CENTER);
//if (sonarDistCenter < 30 || sonarDistCenter > 150) sonarDistCenter = NO_ECHO; //need to be adjust if sonar is directly in front of mower 25Cm in my case
if (sonarDistRight < 30 || sonarDistRight > 150) sonarDistRight = NO_ECHO; // Object is too close to the sensor JSN SR04T can't read <20 CM . Sensor value is useless
//if (sonarDistLeft < 30 || sonarDistLeft > 150) sonarDistLeft = NO_ECHO;
//disabled the left and center sonar during tracking with this line
sonarDistLeft = NO_ECHO;
sonarDistCenter = NO_ECHO;
if ((sonarDistRight != NO_ECHO) && (sonarDistRight < sonarTriggerBelow)) {
//if (((sonarDistCenter != NO_ECHO) && (sonarDistCenter < sonarTriggerBelow)) || ((sonarDistRight != NO_ECHO) && (sonarDistRight < sonarTriggerBelow)) || ((sonarDistLeft != NO_ECHO) && (sonarDistLeft < sonarTriggerBelow)) ) {
setBeeper(1000, 50, 50, 60, 60);
nextTimeCheckSonar = millis() + 2500; //wait before next reading
timeToResetSpeedPeri = millis() + 3000; //brake the tracking during 6 secondes
ActualSpeedPeriPWM = MaxSpeedperiPwm / 1.7;
}
}