// check perimeter as a boundary
void Robot::checkPerimeterBoundary(){
int Peri_Left_P, Peri_Right_P;
int Peri_Schwellwert = 70; // 70% vom Max Peri Wert
if (millis() >= nextTimeRotationChange){
nextTimeRotationChange = millis() + 60000;
rotateLeft = !rotateLeft;
}
if (mowPatternCurr == MOW_BIDIR){
if ((millis() < stateStartTime + 3000)) return;
if (!perimeterInside) {
if ((rand() % 2) == 0){
reverseOrBidir(LEFT);
} else {
reverseOrBidir(RIGHT);
}
}
} else {
if (stateCurr == STATE_FORWARD) {
if (perimeterTriggerTime != 0) {
if (millis() >= perimeterTriggerTime){
perimeterTriggerTime = 0;
Peri_Trigger_Block = millis() + 5000; // 2TTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTT
//if ((rand() % 2) == 0){
if ((perimeterInside && (!perimeterInside_R)) || ((!perimeterInside && !perimeterInside_R) && rotateLeft)){
// if(rotateLeft){ PPPPPPPPPPPPPPPPPPPPPPPPPPPPPPPPPPPPPPPPPPPPPPP
setNextState(STATE_PERI_OUT_REV, LEFT);
} else {
setNextState(STATE_PERI_OUT_REV, RIGHT);
}
}
}
else // perimeterTriggerTime !=0
{
// 2TTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTT
Peri_Left_P = (motorLeftSenseADC_readmax *100) / motorLeftSenseADC_MAX;
Peri_Right_P = (motorRightSenseADC_readmax *100) / motorRightSenseADC_MAX;
if ((motorPowerIgnoreTime < 2000)&& (perimeterCounter > 8) && (Peri_Trigger_Block < millis()))
{
Console.print (F(" Left_P / Right_P : "));
Console.print(Peri_Left_P);
Console.print (F(" L / "));
Console.println(Peri_Right_P);
if ((Peri_Left_P > Peri_Schwellwert) or (Peri_Right_P > Peri_Schwellwert))
{
if (Peri_Trigger_Block > Peri_Roll_Start) Peri_Roll_Start= millis();
if (Peri_Left_P > Peri_Right_P)
{
motorRightSpeedRpmSet = motorSpeedMaxRpm / (map(Peri_Left_P,Peri_Schwellwert,100,11,18)/10);
motorLeftSpeedRpmSet = motorSpeedMaxRpm;
}
else
{
motorRightSpeedRpmSet = motorSpeedMaxRpm;
motorLeftSpeedRpmSet = motorSpeedMaxRpm / (map(Peri_Left_P,Peri_Schwellwert,100,11,18)/10);
}
if ((Peri_Roll_Start + 10000) < millis()) Peri_Trigger_Block = millis() +5000; // wenn Robby 10 sekunden in diesem Modus ist dann Abbruch und min 5 sek normaler Modus
}
else // wenn kein Schwellwert mehr überschritten dann volle Geschwindigkeit vorwärts
{
motorRightSpeedRpmSet = motorSpeedMaxRpm;
motorLeftSpeedRpmSet = motorSpeedMaxRpm;
Peri_Trigger_Block = millis() + 5000; // nach Schleife 5 sek normaler Betrieb
}
}
// 2TTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTT
}
}
else if ((stateCurr == STATE_ROLL)) {
if (perimeterTriggerTime != 0) {
if (millis() >= perimeterTriggerTime){
perimeterTriggerTime = 0;
setMotorPWM( 0, 0, false );
//if ((rand() % 2) == 0){
if (rotateLeft){
setNextState(STATE_PERI_OUT_FORW, LEFT);
} else {
setNextState(STATE_PERI_OUT_FORW, RIGHT);
}
}
}
}
}
}