diff --git a/alfred/config.h b/alfred/config.h index 0c3da84fb..8681610a7 100644 --- a/alfred/config.h +++ b/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 diff --git a/sunray/LineTracker.cpp b/sunray/LineTracker.cpp index d6c53b355..54191e580 100644 --- a/sunray/LineTracker.cpp +++ b/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; diff --git a/sunray/motor.cpp b/sunray/motor.cpp index 29d31726e..329c11952 100644 --- a/sunray/motor.cpp +++ b/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)); diff --git a/sunray/robot.cpp b/sunray/robot.cpp index 40b3242cf..928563e8b 100644 --- a/sunray/robot.cpp +++ b/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: "); diff --git a/sunray/robot.h b/sunray/robot.h index 6a07d3cc1..e245a8ad5 100644 --- a/sunray/robot.h +++ b/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 {