get PerimeterMax while STATE_PERI_FIND only

Holoratte · Jun 5, 2017 · 548941f · 548941f
1 parent 401a5d8
commit 548941f
Show file tree

Hide file tree

Showing 3 changed files with 16 additions and 3 deletions.
diff --git a/code/ardumower/motor.h b/code/ardumower/motor.h
@@ -244,7 +244,7 @@ void Robot::motorControlPerimeter(){
     }
     return;
   }   
-  perimeterPID.x = 5*((double(perimeterMag)/double(perimeterMagMedian.getHighest())));
+  perimeterPID.x = 5*((double(perimeterMag)/double(perimeterMagMax)));
   if (perimeterInside){
       perimeterPID.w = -1;
       if (!lastPerimeterTrackInside) perimeterPID.reset();

diff --git a/code/ardumower/robot.cpp b/code/ardumower/robot.cpp
@@ -387,9 +387,10 @@ void Robot::readSensors(){
 
 
   if ((perimeterUse) && (millis() >= nextTimePerimeter)){    
-    nextTimePerimeter = millis() +  30; // 50    
+    if (stateCurr == STATE_PERI_TRACK) nextTimePerimeter = millis() +  30; // 50  
+    else nextTimePerimeter = millis() +  50;
     perimeterMag = readSensor(SEN_PERIM_LEFT);
-    perimeterMagMedian.add(abs(perimeterMag));
+    if (stateCurr == STATE_PERI_FIND)perimeterMagMedian.add(abs(perimeterMag));
     if ((perimeter.isInside(0) != perimeterInside)){      
       perimeterCounter++;
       perimeterLastTransitionTime = millis();
@@ -1168,6 +1169,7 @@ void Robot::setNextState(byte stateNew, byte dir){
   }
   if (stateNew == STATE_PERI_TRACK){        
     //motorMowEnable = false;     // FIXME: should be an option?
+    perimeterMagMax = perimeterMagMedian.getHighest()*1.1;
     setActuator(ACT_CHGRELAY, 0);
     perimeterPID.reset();
     //beep(6);

diff --git a/code/ardumower/robot.h b/code/ardumower/robot.h
@@ -68,6 +68,7 @@ enum {
   SEN_PERIM_RIGHT_EXTRA, // 0..MAX_PERIMETER
   SEN_LAWN_FRONT,        
   SEN_LAWN_BACK,         
+
   SEN_BAT_VOLTAGE,       // Volt * 100
   SEN_CHG_CURRENT,       // Ampere * 100
   SEN_CHG_VOLTAGE,       // Volt * 100
@@ -269,6 +270,8 @@ class Robot
     int motorRollTimeMin  ; // min. roll time (ms)
     int motorReverseTime ;  // max. reverse time (ms)
     long motorForwTimeMax; // max. forward time (ms) / timeout
+
+
     float motorBiDirSpeedRatio1 ;   // bidir mow pattern speed ratio 1
     float motorBiDirSpeedRatio2 ;   // bidir mow pattern speed ratio 2
     bool motorRightSwapDir     ;    // inverse right motor direction? 
@@ -289,6 +292,7 @@ class Robot
     int motorRightSenseCounter ;
     unsigned long nextTimeMotorSense ;
     unsigned long lastSetMotorSpeedTime;
+
     unsigned long motorLeftZeroTimeout;
     unsigned long motorRightZeroTimeout;
     boolean rotateLeft;
@@ -299,6 +303,7 @@ class Robot
     float motorMowAccel       ;  // motor mower acceleration (warning: do not set too high)
     int motorMowSpeedMaxPwm ;    // motor mower max PWM
     float motorMowPowerMax ;     // motor mower max power (Watt)
+
     char motorMowModulate  ;      // motor mower cutter modulation?
     int motorMowRPMSet        ;   // motor mower RPM (only for cutter modulation)
     float motorMowSenseScale ; // motor mower sense scale (mA=(ADC-zero)/scale)
@@ -363,6 +368,7 @@ class Robot
     PID perimeterPID ;             // perimeter PID controller
     int perimeterMag ;             // perimeter magnitude
     RunningMedian perimeterMagMedian = RunningMedian(300);
+    int perimeterMagMax; //maximum perimeter magnitude detected while perimeter find
     boolean lastPerimeterTrackInside; // was inside or outside
     boolean perimeterInside ;      // is inside perimeter?
     unsigned long perimeterTriggerTime; // time to trigger perimeter transition (timeout)
@@ -399,6 +405,9 @@ class Robot
     unsigned int sonarDistCenter ;
     unsigned int sonarDistRight ;
     unsigned int sonarDistLeft ; 
+
+
+
     unsigned int sonarDistCounter ;
     unsigned int tempSonarDistCounter ;
     unsigned long sonarObstacleTimeout ;
@@ -426,11 +435,13 @@ class Robot
     float startChargingIfBelow; // start charging if battery Voltage is below
     unsigned long chargingTimeout; // safety timer for charging
     int batADC;
+
     float chgSenseZero    ;       // charge current sense zero point
     float chgFactor       ;     // charge current conversion factor
     float chgSense        ;       // mV/A empfindlichkeit des Ladestromsensors in mV/A (Für ACS712 5A = 185)
     char chgChange        ;       // messwertumkehr von - nach +         1oder 0
     float batVoltage ;  // battery voltage (Volt)
+
     byte chgSelection     ;       // Senor Auswahl
     float batRefFactor ;
     float batCapacity ; // battery capacity (mAh)