cleanup/simplify motor code, bugfix line tracker memory leak

alfred/config.h

@@ -160,19 +160,6 @@ Also, you may choose the serial port below for serial monitor output (CONSOLE).
 #define MOTOR_LEFT_SWAP_DIRECTION 1  // uncomment to swap left motor direction
 #define MOTOR_RIGHT_SWAP_DIRECTION 1  // uncomment to swap right motor direction
 
-// ------------ dynamic gear motors speed ----------------
-// speed will be adjusted by the mowing motor current. If USE_MOWMOTOR_CURRENT_AVERAGE is set to false, the Speed 
-// will be changed if the mow Current is lower or higher than MOWMOTOR_CURRENT_FACTOR * MOW_OVERLOAD_CURRENT.
-// If USE_MOWMOTOR_CURRENT_AVERAGE is set to true the algorithm will detect the current at the middle PWM of the mowMotor.
-// The mowing average will be calculate over 10000 loops and start at MOWMOTOR_CURRENT_FACTOR.
-#define ENABLE_DYNAMIC_MOWER_SPEED false
-#define SPEED_ACCELERATION 0.005 // Speed factor will be changed with every programm loop 
-
-#define SPEED_FACTOR_MAX 1.2
-#define SPEED_FACTOR_MIN 0.5
-#define USE_MOWMOTOR_CURRENT_AVERAGE true
-#define MOWMOTOR_CURRENT_FACTOR 0.25
-
 
 // ----- mowing motor -------------------------------------------------
 // NOTE: motor drivers will indicate 'fault' signal if motor current (e.g. due to a stall on a molehole) or temperature is too high for a 
@@ -200,14 +187,6 @@ Also, you may choose the serial port below for serial monitor output (CONSOLE).
 // should the robot trigger obstacle avoidance on motor errors if motor recovery failed?
 #define ENABLE_FAULT_OBSTACLE_AVOIDANCE true  
 
-// ----------- dynamic mowingm motor RPM --------------
-// RPM of the mow motor will be adjust over the actual current of the mow motor. If the motor needs more current the PWM will be higher.
-// it can be used 3 different functions for the calculation of the PWM depended´nt on the mowMotor current. The root-Function is recommended
-#define ENABLE_DYNAMIC_MOWMOTOR false // set true to activate, set false to deactivate
-#define DYNAMIC_MOWMOTOR_ALGORITHM 2 // 1 - linear; 2 - root-Function; 3 - square-Function
-#define MIN_MOW_RPM 170   //minimum value of mow RPM
-#define MAX_MOW_RPM 255   // maximum value is 255
-
 
 // ------ WIFI module (ESP8266 ESP-01 with ESP firmware 2.2.1) --------------------------------
 // NOTE: all settings (maps, absolute position source etc.) are stored in your phone - when using another

sunray/LineTracker.cpp

@@ -14,6 +14,7 @@
 
 //PID pidLine(0.2, 0.01, 0); // not used
 //PID pidAngle(2, 0.1, 0);  // not used
+Polygon circle(8);
 
 float stanleyTrackingNormalK = STANLEY_CONTROL_K_NORMAL;
 float stanleyTrackingNormalP = STANLEY_CONTROL_P_NORMAL;    
@@ -33,7 +34,6 @@ bool printmotoroverload = false;
 bool trackerDiffDelta_positive = false;
 
 int get_turn_direction_preference() {
-  Polygon circle(8);
   Point target = maps.targetPoint;
   float targetDelta = pointsAngle(stateX, stateY, target.x(), target.y());
   float center_x = stateX;

sunray/motor.cpp

@@ -25,9 +25,7 @@ void Motor::begin() {
   #endif
 
   pwmSpeedOffset = 1.0;
-  mowMotorCurrentAverage = MOWMOTOR_CURRENT_FACTOR * MOW_OVERLOAD_CURRENT;
-  currentFactor = MOWMOTOR_CURRENT_FACTOR;
-
+
   //ticksPerRevolution = 1060/2;
   ticksPerRevolution = TICKS_PER_REVOLUTION;
 	wheelBaseCm = WHEEL_BASE_CM;    // wheel-to-wheel distance (cm) 36
@@ -120,101 +118,11 @@ void Motor::begin() {
 
 void Motor::speedPWM ( int pwmLeft, int pwmRight, int pwmMow )
 {
-  //########################  Declaration ############################
-
-  int pwmVariableMow = 0;
-
-  //########################  Modify pwm depend to to actual Mower Current ############################
-
-  if ((pwmMow != 0) && (ENABLE_DYNAMIC_MOWMOTOR))
-  {
-    switch (DYNAMIC_MOWMOTOR_ALGORITHM){
-      case 1:
-        pwmVariableMow = (int)((MAX_MOW_RPM - MIN_MOW_RPM) * (motorMowSenseLP / MOW_OVERLOAD_CURRENT));
-        break;
-      case 2:
-        pwmVariableMow = (int)((MAX_MOW_RPM - MIN_MOW_RPM) * sqrt((motorMowSenseLP / MOW_OVERLOAD_CURRENT)));
-        break;
-      case 3:
-        pwmVariableMow = (int)((MAX_MOW_RPM - MIN_MOW_RPM) * sq((motorMowSenseLP / MOW_OVERLOAD_CURRENT)));
-        break;
-      default:
-        pwmVariableMow = (int)((MAX_MOW_RPM - MIN_MOW_RPM) * (motorMowSenseLP / MOW_OVERLOAD_CURRENT));
-        break;
-    }
-
-    if (motorMowSenseLP > MOW_OVERLOAD_CURRENT) pwmVariableMow = 0; // failure detection if mower is stuck.
-    if (pwmMow < 0) // check motor direction
-    {
-      pwmMow = (MIN_MOW_RPM + pwmVariableMow) * - 1;
-    }
-    else
-    {
-      pwmMow = MIN_MOW_RPM + pwmVariableMow;
-    }
-
-//    CONSOLE.print("setpwmMow: ");
-//    CONSOLE.print(pwmMow);
-//    CONSOLE.print(" motorMowSenseLP: "); 
-//    CONSOLE.println(motorMowSenseLP);
-  }
-
-  //########################  Correct Motor Direction ############################
-
+  //Correct Motor Direction
   if (motorLeftSwapDir) pwmLeft *= -1;
   if (motorRightSwapDir) pwmRight *= -1;
 
-  //########################  Set Mower Speed depend to actual Mower Current ############################
-
-  if ((pwmMow != 0) && (ENABLE_DYNAMIC_MOWER_SPEED))
-  {    
-    if (USE_MOWMOTOR_CURRENT_AVERAGE)
-    {
-      float pwmAverageMow = (MAX_MOW_RPM - MIN_MOW_RPM) / 2 + MIN_MOW_RPM;
-
-      if ((pwmAverageMow - (MAX_MOW_RPM - MIN_MOW_RPM) / 10) < abs(pwmMow) || abs(pwmMow) > (pwmAverageMow + (pwmMaxMow - MIN_MOW_RPM) / 10))
-      {
-        mowMotorCurrentAverage = (( mowMotorCurrentAverage * 10000) + (motorMowSenseLP)) / (10000 + 1);
-        currentFactor = mowMotorCurrentAverage / MOW_OVERLOAD_CURRENT;
-//        CONSOLE.print("ADJUST CURRENT FACTOR ");
-      }
-    }
-
-//    CONSOLE.print("mowMotorCurrentAverage: ");
-//    CONSOLE.print(mowMotorCurrentAverage);
-//    CONSOLE.print(" currentFactor: ");
-//    CONSOLE.println(currentFactor);
-
-    if (motorMowSenseLP > currentFactor * MOW_OVERLOAD_CURRENT * 0.9) pwmSpeedOffset -= SPEED_ACCELERATION * 2;
-    if (motorMowSenseLP < currentFactor * MOW_OVERLOAD_CURRENT * 1.1) pwmSpeedOffset += SPEED_ACCELERATION;
-
-    pwmSpeedOffset  = min(SPEED_FACTOR_MAX, max(SPEED_FACTOR_MIN, pwmSpeedOffset));
-
-    //########################  Detect a curve ############################
-
-    pwmSpeedCurveDetection = false;
-
-    if (abs(pwmLeft - pwmRight) > (abs(pwmLeft + pwmRight) / 8))
-    { 
-      pwmSpeedCurveDetection = true;
-//      CONSOLE.println("at curves, speed will not be adjusted");
-    }
-
-//    CONSOLE.print("pwmLeftMotor: ");
-//    CONSOLE.print(pwmLeft);
-//    CONSOLE.print(" pwmRightMotor: ");
-//    CONSOLE.print(pwmRight);
-//    CONSOLE.print(" setpwmSpeedOffset: ");
-//    CONSOLE.println(pwmSpeedOffset);
-
-    //########################  set modified pwm value ############################
-
-    //pwmLeft  = (int)(pwmLeft * pwmSpeedOffset);
-    //pwmRight = (int)(pwmRight * pwmSpeedOffset);
-  }
-
-  //########################  Check pwm higher than Max ############################
-
+  // ensure pwm is lower than Max
   pwmLeft = min(pwmMax, max(-pwmMax, pwmLeft));
   pwmRight = min(pwmMax, max(-pwmMax, pwmRight));  
   pwmMow = min(pwmMaxMow, max(-pwmMaxMow, pwmMow)); 

sunray/robot.cpp

@@ -418,8 +418,6 @@ void outputConfig(){
   #ifdef MOTOR_RIGHT_SWAP_DIRECTION
     CONSOLE.println("MOTOR_RIGHT_SWAP_DIRECTION");
   #endif
-  CONSOLE.print("ENABLE_DYNAMIC_MOWER_SPEED: ");
-  CONSOLE.println(ENABLE_DYNAMIC_MOWER_SPEED);
   CONSOLE.print("MOW_FAULT_CURRENT: ");
   CONSOLE.println(MOW_FAULT_CURRENT);
   CONSOLE.print("MOW_OVERLOAD_CURRENT: ");
@@ -432,8 +430,6 @@ void outputConfig(){
   CONSOLE.println(ENABLE_FAULT_OBSTACLE_AVOIDANCE);
   CONSOLE.print("ENABLE_RPM_FAULT_DETECTION: ");
   CONSOLE.println(ENABLE_RPM_FAULT_DETECTION);
-  CONSOLE.print("ENABLE_DYNAMIC_MOWMOTOR: ");
-  CONSOLE.println(ENABLE_DYNAMIC_MOWMOTOR);
   #ifdef SONAR_INSTALLED
     CONSOLE.println("SONAR_INSTALLED");
     CONSOLE.print("SONAR_ENABLE: ");  

sunray/robot.h

@@ -35,7 +35,7 @@
 #include "PubSubClient.h"
 
 
-#define VER "Sunray,1.0.297"
+#define VER "Sunray,1.0.298"
 
 // operation types
 enum OperationType {