From 5462dae4506c46850fa65ae0998111a1084a68a2 Mon Sep 17 00:00:00 2001 From: MK Date: Mon, 13 May 2024 22:00:36 +0300 Subject: [PATCH 01/28] f(pid_for_speed_control): Add PID code --- include/math_utils/pid/pid.h | 32 ++++++++++++++++++++++++++++++++ main/math_utils/pid/pid.cpp | 19 +++++++++++++++++++ 2 files changed, 51 insertions(+) create mode 100644 include/math_utils/pid/pid.h create mode 100644 main/math_utils/pid/pid.cpp diff --git a/include/math_utils/pid/pid.h b/include/math_utils/pid/pid.h new file mode 100644 index 0000000..2342b5b --- /dev/null +++ b/include/math_utils/pid/pid.h @@ -0,0 +1,32 @@ +#ifndef __PID__ +#define __PID__ +class PID +{ + +public: + PID(/* args */); + // ~pid(); +public: + + double output; + + double calculateOutput(double setPoint, double processVariable); + +private: + /* data */ + double _dt; + double _setPoint; + double _processVariable; + double error; + double previousError; + double integral; + double derivative; + double kp; + double ki; + double kd; + +}; + +#endif + + diff --git a/main/math_utils/pid/pid.cpp b/main/math_utils/pid/pid.cpp new file mode 100644 index 0000000..fe12705 --- /dev/null +++ b/main/math_utils/pid/pid.cpp @@ -0,0 +1,19 @@ +#include "pid.h" + +PID::PID() { + +} + +double PID::calculateOutput(double setPoint, double processVariable) { + _setPoint = setPoint; + _processVariable = processVariable; + + error = _setPoint - _processVariable; + integral += error * _dt; + derivative = (error - previousError)/_dt; + + output = kp*error + ki*integral + kd * derivative; + previousError = error; + + return output; +} \ No newline at end of file From 671fd5721c19fbaf27740daec57a03ac9dc8dfd4 Mon Sep 17 00:00:00 2001 From: MK Date: Sun, 26 May 2024 14:18:00 +0300 Subject: [PATCH 02/28] f(scale_certification): Set raw motor speed --- include/bsp/drivers/actuators/bsp_dc_motor.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/include/bsp/drivers/actuators/bsp_dc_motor.h b/include/bsp/drivers/actuators/bsp_dc_motor.h index 9f6e8a5..8e49e70 100644 --- a/include/bsp/drivers/actuators/bsp_dc_motor.h +++ b/include/bsp/drivers/actuators/bsp_dc_motor.h @@ -54,6 +54,11 @@ class DcMotor { * @author @MK */ virtual void set_speed(BSP::motorSpeeds speed) = 0; + + /** + * @brief Sets a motor's raw speed + */ + virtual void set_raw_speed(unsigned int speed) = 0; }; } From 5cabb771dd7a385304725954ade89045d2abea47 Mon Sep 17 00:00:00 2001 From: MK Date: Sun, 26 May 2024 14:20:34 +0300 Subject: [PATCH 03/28] f(scale_certification): Use extern in the header file --- include/services/kinematics/kinematics.h | 3 ++- src/apps/car_app/car_line_following.cpp | 1 - src/services/kinematics.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/include/services/kinematics/kinematics.h b/include/services/kinematics/kinematics.h index 66e1bcf..e230ee3 100644 --- a/include/services/kinematics/kinematics.h +++ b/include/services/kinematics/kinematics.h @@ -4,7 +4,6 @@ #include "bsp_dc_motor.h" - enum carKinematicsStates { idling, drivingLeft, @@ -17,6 +16,8 @@ enum carKinematicsStates { reversingRight }; +extern carKinematicsStates RB19Kinematics; + /** * @brief Control vehicular kinematics: braking and accelerating diff --git a/src/apps/car_app/car_line_following.cpp b/src/apps/car_app/car_line_following.cpp index efe33eb..c5b693a 100644 --- a/src/apps/car_app/car_line_following.cpp +++ b/src/apps/car_app/car_line_following.cpp @@ -7,7 +7,6 @@ Infrared5Channel* infraredSensor; Infrared5Channel::detectedChannels sensorDetectedChannels; carSteerStates RB19Steer; -carKinematicsStates RB19Kinematics; void carAppInit() { RB19Kinematics = idling; diff --git a/src/services/kinematics.cpp b/src/services/kinematics.cpp index e611127..a0230c0 100644 --- a/src/services/kinematics.cpp +++ b/src/services/kinematics.cpp @@ -14,8 +14,8 @@ BSP::DcMotor* kinematicsLeftMotor; BSP::DcMotor* kinematicsRightMotor; -extern carKinematicsStates RB19Kinematics; +carKinematicsStates RB19Kinematics; void controlCarKinematics() { From fa802de2f1d344db8851d765340f97a33c5a192f Mon Sep 17 00:00:00 2001 From: MK Date: Sun, 26 May 2024 14:21:40 +0300 Subject: [PATCH 04/28] f(scale_certification): Use _ for class members --- include/math_utils/pid/pid.h | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/include/math_utils/pid/pid.h b/include/math_utils/pid/pid.h index 2342b5b..24205b0 100644 --- a/include/math_utils/pid/pid.h +++ b/include/math_utils/pid/pid.h @@ -4,26 +4,26 @@ class PID { public: - PID(/* args */); + PID(double kp, double ki, double kd); // ~pid(); public: double output; + double _dt; double calculateOutput(double setPoint, double processVariable); private: /* data */ - double _dt; double _setPoint; double _processVariable; - double error; - double previousError; - double integral; - double derivative; - double kp; - double ki; - double kd; + double _error; + double _previousError; + double _integral; + double _derivative; + double _kp; + double _ki; + double _kd; }; From 1f9a6e820fc0c60238f35c81803be3e4538a00e1 Mon Sep 17 00:00:00 2001 From: MK Date: Sun, 26 May 2024 14:22:46 +0300 Subject: [PATCH 05/28] f(scale_certification): Use _ for class members and initialize them in constructor --- main/math_utils/pid/pid.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/main/math_utils/pid/pid.cpp b/main/math_utils/pid/pid.cpp index fe12705..df57652 100644 --- a/main/math_utils/pid/pid.cpp +++ b/main/math_utils/pid/pid.cpp @@ -1,6 +1,8 @@ #include "pid.h" -PID::PID() { +PID::PID(double kp, double ki, double kd): + _kp(kp), _ki(ki), _kd(kd) +{ } @@ -8,12 +10,12 @@ double PID::calculateOutput(double setPoint, double processVariable) { _setPoint = setPoint; _processVariable = processVariable; - error = _setPoint - _processVariable; - integral += error * _dt; - derivative = (error - previousError)/_dt; + _error = _setPoint - _processVariable; + _integral += _error * _dt; + _derivative = (_error - _previousError)/_dt; - output = kp*error + ki*integral + kd * derivative; - previousError = error; + output = _kp*_error + _ki*_integral + _kd * _derivative; + _previousError = _error; return output; } \ No newline at end of file From 068ed056360141cefaa49f26d97f3166d8da33c7 Mon Sep 17 00:00:00 2001 From: MK Date: Sun, 26 May 2024 14:24:34 +0300 Subject: [PATCH 06/28] f(scale_certification): Add set raw speed motor functions to driver files --- .../drivers/actuators/dc_motor/esp32_dc_motor_ft_smc.h | 5 +++++ .../drivers/actuators/dc_motor/esp32_dc_motor_l298n.h | 5 +++++ .../drivers/actuators/dc_motor/esp32_dc_motor_ft_smc.cpp | 7 +++++++ .../drivers/actuators/dc_motor/esp32_dc_motor_l298n.cpp | 4 ++++ 4 files changed, 21 insertions(+) diff --git a/ports/ESP32/include/drivers/actuators/dc_motor/esp32_dc_motor_ft_smc.h b/ports/ESP32/include/drivers/actuators/dc_motor/esp32_dc_motor_ft_smc.h index 7a784be..c06f3ae 100644 --- a/ports/ESP32/include/drivers/actuators/dc_motor/esp32_dc_motor_ft_smc.h +++ b/ports/ESP32/include/drivers/actuators/dc_motor/esp32_dc_motor_ft_smc.h @@ -24,6 +24,11 @@ class ESP32DCMotorFtSmc : public BSP::DcMotor { void run() override; void stop() override; + + /** + * @brief Sets a motor's raw speed + */ + void set_raw_speed(unsigned int speed) override; private: gpio_num_t _signal; unsigned int _pwmDuty; diff --git a/ports/ESP32/include/drivers/actuators/dc_motor/esp32_dc_motor_l298n.h b/ports/ESP32/include/drivers/actuators/dc_motor/esp32_dc_motor_l298n.h index 765d5c2..8f54595 100644 --- a/ports/ESP32/include/drivers/actuators/dc_motor/esp32_dc_motor_l298n.h +++ b/ports/ESP32/include/drivers/actuators/dc_motor/esp32_dc_motor_l298n.h @@ -45,6 +45,11 @@ class ESP32DcMotorL298N: public BSP::DcMotor { */ void set_speed(BSP::motorSpeeds speed) override; + /** + * @brief Sets a motor's raw speed + */ + void set_raw_speed(unsigned int speed) override; + }; diff --git a/ports/ESP32/src/drivers/actuators/dc_motor/esp32_dc_motor_ft_smc.cpp b/ports/ESP32/src/drivers/actuators/dc_motor/esp32_dc_motor_ft_smc.cpp index bcaefaa..99d604f 100644 --- a/ports/ESP32/src/drivers/actuators/dc_motor/esp32_dc_motor_ft_smc.cpp +++ b/ports/ESP32/src/drivers/actuators/dc_motor/esp32_dc_motor_ft_smc.cpp @@ -121,6 +121,13 @@ void ESP32DCMotorFtSmc::set_speed(BSP::motorSpeeds speed) { printf("Motor speed set to %u\n", _pwmDuty); } +void ESP32DCMotorFtSmc::set_raw_speed(unsigned int speed) { + _pwmDuty = speed; + ledc_set_duty(LEDC_LOW_SPEED_MODE, _motorChannel, _pwmDuty); + ledc_update_duty(LEDC_LOW_SPEED_MODE, _motorChannel); + printf("Motor speed set to %u\n", _pwmDuty); +} + void ESP32DCMotorFtSmc::run() { } diff --git a/ports/ESP32/src/drivers/actuators/dc_motor/esp32_dc_motor_l298n.cpp b/ports/ESP32/src/drivers/actuators/dc_motor/esp32_dc_motor_l298n.cpp index 341b3ad..01c84ed 100644 --- a/ports/ESP32/src/drivers/actuators/dc_motor/esp32_dc_motor_l298n.cpp +++ b/ports/ESP32/src/drivers/actuators/dc_motor/esp32_dc_motor_l298n.cpp @@ -45,4 +45,8 @@ void ESP32DcMotorL298N::stop() { void ESP32DcMotorL298N::set_speed(BSP::motorSpeeds speed) { +} + +void ESP32DcMotorL298N::set_raw_speed(unsigned int speed) { + } \ No newline at end of file From e4f0ae9ff07eb367cce740929ca3a581eac76854 Mon Sep 17 00:00:00 2001 From: MK Date: Sun, 26 May 2024 14:28:52 +0300 Subject: [PATCH 07/28] f(scale_certification): Check the status of the car and calculate the appropriate PID output --- include/apps/car_app/car_line_following_pid.h | 13 ++++++ src/apps/car_app/car_line_following_pid.cpp | 42 +++++++++++++++++++ 2 files changed, 55 insertions(+) create mode 100644 include/apps/car_app/car_line_following_pid.h create mode 100644 src/apps/car_app/car_line_following_pid.cpp diff --git a/include/apps/car_app/car_line_following_pid.h b/include/apps/car_app/car_line_following_pid.h new file mode 100644 index 0000000..2fef2b8 --- /dev/null +++ b/include/apps/car_app/car_line_following_pid.h @@ -0,0 +1,13 @@ +#ifndef __CAR_LF__PID +#define __CAR_LF__PID + +#include "pid.h" +#include "bsp.h" + +extern double PIDOutput; + +void carAppInit(); + +void carAppRun(); + +#endif \ No newline at end of file diff --git a/src/apps/car_app/car_line_following_pid.cpp b/src/apps/car_app/car_line_following_pid.cpp new file mode 100644 index 0000000..63e14fa --- /dev/null +++ b/src/apps/car_app/car_line_following_pid.cpp @@ -0,0 +1,42 @@ +#include "car_line_following_pid.h" + +Infrared5Channel* infraredSensor; +Infrared5Channel::detectedChannels sensorDetectedChannels; + +double lineError; +PID lineFollowingPid(1.00, 1.00, 1.00); +double PIDOutput; + +void carAppInit() { + infraredSensor = BSP::getDefaultInfraredSensor(); + carAppRun(); +} + +void carAppRun() { + sensorDetectedChannels = infraredSensor->read(); + switch (sensorDetectedChannels) + { + case Infrared5Channel::detectedChannels::centralChannel: + lineError = 0.0; + break; + case Infrared5Channel::detectedChannels::halfLeftChannel: + lineError = -1.0; + break; + case Infrared5Channel::detectedChannels::farLeftChannel: + lineError = -2.0; + break; + case Infrared5Channel::detectedChannels::halfRightChannel: + lineError = 1.0; + break; + case Infrared5Channel::detectedChannels::farRightChannel: + lineError = 2.0; + break; + default: + break; + } + + lineFollowingPid._dt = 0.025; + PIDOutput = lineFollowingPid.calculateOutput(0, lineError); + + +} \ No newline at end of file From 9ee06de9e4f390353f8dc0d0563fac50d9a660fd Mon Sep 17 00:00:00 2001 From: MK Date: Sun, 26 May 2024 14:31:12 +0300 Subject: [PATCH 08/28] f(pid_for_speed_control): Vary motor speed wrt to the PID output --- include/services/kinematics/kinematics_pid.h | 12 ++++++++++++ src/services/kinematics_pid.cpp | 16 ++++++++++++++++ 2 files changed, 28 insertions(+) create mode 100644 include/services/kinematics/kinematics_pid.h create mode 100644 src/services/kinematics_pid.cpp diff --git a/include/services/kinematics/kinematics_pid.h b/include/services/kinematics/kinematics_pid.h new file mode 100644 index 0000000..984811d --- /dev/null +++ b/include/services/kinematics/kinematics_pid.h @@ -0,0 +1,12 @@ +#ifndef __KINEMATICS_PID__ +#define __KINEMATICS_PID__ + +#include "car_line_following_pid.h" + +/** + * @brief Control vehicular kinematics: braking and accelerating +*/ +void controlCarKinematics(); + + +#endif \ No newline at end of file diff --git a/src/services/kinematics_pid.cpp b/src/services/kinematics_pid.cpp new file mode 100644 index 0000000..52a5cf4 --- /dev/null +++ b/src/services/kinematics_pid.cpp @@ -0,0 +1,16 @@ +#include "kinematics_pid.h" +#include "bsp.h" + +BSP::DcMotor* kinematicsLeftMotor = BSP::getDefaultKinematicsLeftMotor(); +BSP::DcMotor* kinematicsRightMotor = BSP::getDefaultKinematicsRightMotor(); + +unsigned int leftMotorSpeed; +unsigned int RightMotorSpeed; + +void controlCarKinematics() { + leftMotorSpeed = PIDOutput * 2; + RightMotorSpeed = PIDOutput / 2; + + kinematicsLeftMotor->set_raw_speed(leftMotorSpeed); + kinematicsRightMotor->set_raw_speed(RightMotorSpeed); +} \ No newline at end of file From c203cc114ce055d8fca831b50d2c29d84dca083b Mon Sep 17 00:00:00 2001 From: MK Date: Sun, 26 May 2024 14:32:38 +0300 Subject: [PATCH 09/28] f(pid_for_speed_control): Add PID app and controller files to CMakeLists --- main/CMakeLists.txt | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 main/CMakeLists.txt diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt new file mode 100644 index 0000000..e69de29 From 136c00fb5166161c74a4352308e3cb2f6103a823 Mon Sep 17 00:00:00 2001 From: MK Date: Fri, 7 Jun 2024 09:12:57 +0300 Subject: [PATCH 10/28] f(scale_certification): Track previous output to discard huge changes in ouput --- include/math_utils/pid/pid.h | 1 + main/math_utils/pid/pid.cpp | 10 ++++++++++ 2 files changed, 11 insertions(+) diff --git a/include/math_utils/pid/pid.h b/include/math_utils/pid/pid.h index 24205b0..29fef4b 100644 --- a/include/math_utils/pid/pid.h +++ b/include/math_utils/pid/pid.h @@ -24,6 +24,7 @@ class PID double _kp; double _ki; double _kd; + double _previousOutput; }; diff --git a/main/math_utils/pid/pid.cpp b/main/math_utils/pid/pid.cpp index df57652..0507ba5 100644 --- a/main/math_utils/pid/pid.cpp +++ b/main/math_utils/pid/pid.cpp @@ -1,4 +1,5 @@ #include "pid.h" +#include PID::PID(double kp, double ki, double kd): _kp(kp), _ki(ki), _kd(kd) @@ -6,6 +7,10 @@ PID::PID(double kp, double ki, double kd): } +#define MAX_PID_OUTPUT 40 +#define MIN_PID_OUTPUT 40 +#define MAX_PID_DIFF 1 + double PID::calculateOutput(double setPoint, double processVariable) { _setPoint = setPoint; _processVariable = processVariable; @@ -15,7 +20,12 @@ double PID::calculateOutput(double setPoint, double processVariable) { _derivative = (_error - _previousError)/_dt; output = _kp*_error + _ki*_integral + _kd * _derivative; + if (abs(output - _previousOutput) > MAX_PID_DIFF) { + output = _previousOutput; + } _previousError = _error; + _previousOutput = output; + return output; } \ No newline at end of file From c9a11517ac7411cd912cf6b56c25bee5b78d7c7a Mon Sep 17 00:00:00 2001 From: MK Date: Sat, 8 Jun 2024 01:37:40 +0300 Subject: [PATCH 11/28] f(f/pid_for_speed_control): Make control kinematics return a value --- include/services/kinematics/kinematics_pid.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/services/kinematics/kinematics_pid.h b/include/services/kinematics/kinematics_pid.h index 984811d..982c27b 100644 --- a/include/services/kinematics/kinematics_pid.h +++ b/include/services/kinematics/kinematics_pid.h @@ -6,7 +6,7 @@ /** * @brief Control vehicular kinematics: braking and accelerating */ -void controlCarKinematics(); +double controlCarKinematics(); #endif \ No newline at end of file From c4c919addb75ec3659c11b4d0725dd126aab80ca Mon Sep 17 00:00:00 2001 From: MK Date: Sat, 8 Jun 2024 01:38:22 +0300 Subject: [PATCH 12/28] f(f/pid_for_speed_control): Tune PID on bench --- src/apps/car_app/car_line_following_pid.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/apps/car_app/car_line_following_pid.cpp b/src/apps/car_app/car_line_following_pid.cpp index 63e14fa..e6f6b70 100644 --- a/src/apps/car_app/car_line_following_pid.cpp +++ b/src/apps/car_app/car_line_following_pid.cpp @@ -4,7 +4,7 @@ Infrared5Channel* infraredSensor; Infrared5Channel::detectedChannels sensorDetectedChannels; double lineError; -PID lineFollowingPid(1.00, 1.00, 1.00); +PID lineFollowingPid(0.5, 3.80, 0.10); double PIDOutput; void carAppInit() { From d7a56e92e9cbf3153be9203548869607e8166560 Mon Sep 17 00:00:00 2001 From: MK Date: Sat, 8 Jun 2024 01:40:00 +0300 Subject: [PATCH 13/28] f(f/pid_for_speed_control): Add function to validate motor speed and alter preset speed values --- .../dc_motor/esp32_dc_motor_ft_smc.h | 5 ++ .../dc_motor/esp32_dc_motor_ft_smc.cpp | 51 ++++++++++++++++++- 2 files changed, 54 insertions(+), 2 deletions(-) diff --git a/ports/ESP32/include/drivers/actuators/dc_motor/esp32_dc_motor_ft_smc.h b/ports/ESP32/include/drivers/actuators/dc_motor/esp32_dc_motor_ft_smc.h index c06f3ae..4323451 100644 --- a/ports/ESP32/include/drivers/actuators/dc_motor/esp32_dc_motor_ft_smc.h +++ b/ports/ESP32/include/drivers/actuators/dc_motor/esp32_dc_motor_ft_smc.h @@ -29,6 +29,11 @@ class ESP32DCMotorFtSmc : public BSP::DcMotor { * @brief Sets a motor's raw speed */ void set_raw_speed(unsigned int speed) override; + + /** + * @brief Validate the set motor speed value + */ + unsigned int validate_motor_speed(unsigned int speed); private: gpio_num_t _signal; unsigned int _pwmDuty; diff --git a/ports/ESP32/src/drivers/actuators/dc_motor/esp32_dc_motor_ft_smc.cpp b/ports/ESP32/src/drivers/actuators/dc_motor/esp32_dc_motor_ft_smc.cpp index 99d604f..3b4238a 100644 --- a/ports/ESP32/src/drivers/actuators/dc_motor/esp32_dc_motor_ft_smc.cpp +++ b/ports/ESP32/src/drivers/actuators/dc_motor/esp32_dc_motor_ft_smc.cpp @@ -13,7 +13,7 @@ void ESP32DCMotorFtSmc::init() { _clockWiseSpeedMap[BSP::motorSpeeds::ZERO] = 77; _clockWiseSpeedMap[BSP::motorSpeeds::SUPERSLOW] = 78; _clockWiseSpeedMap[BSP::motorSpeeds::LOW] = 79; - _clockWiseSpeedMap[BSP::motorSpeeds::INTERMEDIATE] = 80; + _clockWiseSpeedMap[BSP::motorSpeeds::INTERMEDIATE] = 81; _clockWiseSpeedMap[BSP::motorSpeeds::MEDIUM] = 93; _clockWiseSpeedMap[BSP::motorSpeeds::HIGH] = 101; @@ -122,11 +122,58 @@ void ESP32DCMotorFtSmc::set_speed(BSP::motorSpeeds speed) { } void ESP32DCMotorFtSmc::set_raw_speed(unsigned int speed) { - _pwmDuty = speed; + _pwmDuty = validate_motor_speed(speed); ledc_set_duty(LEDC_LOW_SPEED_MODE, _motorChannel, _pwmDuty); ledc_update_duty(LEDC_LOW_SPEED_MODE, _motorChannel); printf("Motor speed set to %u\n", _pwmDuty); } +#define MAX_CW_PWM_VALUE 79 +#define MIN_CW_PWM_VALUE 77 + +#define MAX_CCW_PWM_VALUE 77 +#define MIN_CCW_PWM_VALUE 73 + +unsigned int ESP32DCMotorFtSmc::validate_motor_speed(unsigned int speed) { + unsigned int validatedSpeed; + switch (_direction) + { + case BSP::motorDirections::CLOCKWISE: + /* code */ + if(speed > MAX_CW_PWM_VALUE) { + validatedSpeed = MAX_CW_PWM_VALUE; + return validatedSpeed; + } + else if (speed < MIN_CW_PWM_VALUE) { + validatedSpeed = MIN_CW_PWM_VALUE; + return validatedSpeed; + } + else { + validatedSpeed = speed; + return validatedSpeed; + } + break; + case BSP::motorDirections::COUNTERCLOCKWISE: + /* code */ + if(speed > MAX_CCW_PWM_VALUE) { + validatedSpeed = MAX_CCW_PWM_VALUE; + return validatedSpeed; + } + else if (speed < MIN_CCW_PWM_VALUE) { + validatedSpeed = MIN_CCW_PWM_VALUE; + return validatedSpeed; + } + else { + validatedSpeed = speed; + return validatedSpeed; + } + break; + + default: + validatedSpeed = 0; + return validatedSpeed; + break; + } +} void ESP32DCMotorFtSmc::run() { From 95eebb91dceec7dfc44085d6d8e319b3b306860b Mon Sep 17 00:00:00 2001 From: MK Date: Sat, 8 Jun 2024 01:40:40 +0300 Subject: [PATCH 14/28] f(f/pid_for_speed_control): Clamp integral and PID output --- main/math_utils/pid/pid.cpp | 24 +++++++++++++++++++++--- 1 file changed, 21 insertions(+), 3 deletions(-) diff --git a/main/math_utils/pid/pid.cpp b/main/math_utils/pid/pid.cpp index 0507ba5..a856b68 100644 --- a/main/math_utils/pid/pid.cpp +++ b/main/math_utils/pid/pid.cpp @@ -1,5 +1,6 @@ #include "pid.h" #include +#include PID::PID(double kp, double ki, double kd): _kp(kp), _ki(ki), _kd(kd) @@ -7,9 +8,12 @@ PID::PID(double kp, double ki, double kd): } -#define MAX_PID_OUTPUT 40 -#define MIN_PID_OUTPUT 40 -#define MAX_PID_DIFF 1 +#define MAX_PID_OUTPUT 3 +#define MIN_PID_OUTPUT -3 +#define MAX_PID_DIFF 2 + +#define MAX_INTEGRAL_VALUE 2 +#define MIN_INTEGRAL_VALUE -2 double PID::calculateOutput(double setPoint, double processVariable) { _setPoint = setPoint; @@ -17,12 +21,26 @@ double PID::calculateOutput(double setPoint, double processVariable) { _error = _setPoint - _processVariable; _integral += _error * _dt; + if (_integral > MAX_INTEGRAL_VALUE) { + _integral = MAX_INTEGRAL_VALUE; + } + if (_integral < MIN_INTEGRAL_VALUE) { + _integral = MIN_INTEGRAL_VALUE; + } _derivative = (_error - _previousError)/_dt; + printf("Integral: %.2f || Derivative: %.2f\n", _integral, _derivative); + output = _kp*_error + _ki*_integral + _kd * _derivative; if (abs(output - _previousOutput) > MAX_PID_DIFF) { output = _previousOutput; } + if (output > MAX_PID_OUTPUT) { + output = _previousOutput; + } + if (output < MIN_PID_OUTPUT) { + output = _previousOutput; + } _previousError = _error; _previousOutput = output; From 13201ba3bd8d30fa03f8dff3972f3decaf733df0 Mon Sep 17 00:00:00 2001 From: MK Date: Sat, 8 Jun 2024 01:41:39 +0300 Subject: [PATCH 15/28] f(f/pid_for_speed_control): Set motor speeds based on PID output --- src/services/kinematics_pid.cpp | 26 +++++++++++++++++++++----- 1 file changed, 21 insertions(+), 5 deletions(-) diff --git a/src/services/kinematics_pid.cpp b/src/services/kinematics_pid.cpp index 52a5cf4..41f65a4 100644 --- a/src/services/kinematics_pid.cpp +++ b/src/services/kinematics_pid.cpp @@ -1,16 +1,32 @@ #include "kinematics_pid.h" +#include #include "bsp.h" BSP::DcMotor* kinematicsLeftMotor = BSP::getDefaultKinematicsLeftMotor(); BSP::DcMotor* kinematicsRightMotor = BSP::getDefaultKinematicsRightMotor(); +#define LEFT_MOTOR_BASE_SPEED 79 +#define RIGHT_MOTOR_BASE_SPEED 78 +// #define RIGHT_MOTOR_MAX_SPEED 79 + + unsigned int leftMotorSpeed; -unsigned int RightMotorSpeed; +unsigned int rightMotorSpeed; + +double controlCarKinematics() { -void controlCarKinematics() { - leftMotorSpeed = PIDOutput * 2; - RightMotorSpeed = PIDOutput / 2; + leftMotorSpeed = LEFT_MOTOR_BASE_SPEED - PIDOutput; + rightMotorSpeed = RIGHT_MOTOR_BASE_SPEED + PIDOutput; + // if (rightMotorSpeed > RIGHT_MOTOR_MAX_SPEED) { + // rightMotorSpeed = RIGHT_MOTOR_MAX_SPEED; + // } + // printf("Left Motor: %u || Right Motor: %u\n", leftMotorSpeed, rightMotorSpeed); + + kinematicsLeftMotor->set_dir(BSP::CLOCKWISE); kinematicsLeftMotor->set_raw_speed(leftMotorSpeed); - kinematicsRightMotor->set_raw_speed(RightMotorSpeed); + + kinematicsRightMotor->set_dir(BSP::CLOCKWISE); + kinematicsRightMotor->set_raw_speed(rightMotorSpeed); + return PIDOutput; } \ No newline at end of file From d0eb60090168b3fa06ef6d44e48122cbc3b106f3 Mon Sep 17 00:00:00 2001 From: MK Date: Sat, 8 Jun 2024 01:42:31 +0300 Subject: [PATCH 16/28] f(f/pid_for_speed_control): Change to use PID source files for drive --- src/RB19.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/RB19.cpp b/src/RB19.cpp index da9912e..9ae2c33 100644 --- a/src/RB19.cpp +++ b/src/RB19.cpp @@ -1,8 +1,8 @@ #include #include "bsp.h" #include "car.h" -#include "car_line_following.h" -#include "kinematics.h" +#include "car_line_following_pid.h" +#include "kinematics_pid.h" #include "steering.h" // #include // #include @@ -29,10 +29,13 @@ int main() carAppInit(); // steerCar(); controlCarKinematics(); + double carPIDOutput; while(1) { carAppRun(); // steerCar(); - controlCarKinematics(); + + carPIDOutput = controlCarKinematics(); + printf("PID Output: %.2f\n", carPIDOutput); BSP::BSP_Delay(100); // vTaskDelay(500/portTICK_PERIOD_MS); } From 52f08f4d51ffcbb81e2426af75d96ecac17a81ad Mon Sep 17 00:00:00 2001 From: MK Date: Wed, 10 Jul 2024 22:21:15 +0300 Subject: [PATCH 17/28] f(pid_for_speed_control): Remove integral component --- main/math_utils/pid/pid.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/main/math_utils/pid/pid.cpp b/main/math_utils/pid/pid.cpp index a856b68..c585ba8 100644 --- a/main/math_utils/pid/pid.cpp +++ b/main/math_utils/pid/pid.cpp @@ -20,7 +20,7 @@ double PID::calculateOutput(double setPoint, double processVariable) { _processVariable = processVariable; _error = _setPoint - _processVariable; - _integral += _error * _dt; + _integral += 0; if (_integral > MAX_INTEGRAL_VALUE) { _integral = MAX_INTEGRAL_VALUE; } From 03282ea1ada0ac3d6ad58c831e70a3b6b9152293 Mon Sep 17 00:00:00 2001 From: MK Date: Wed, 10 Jul 2024 22:22:37 +0300 Subject: [PATCH 18/28] f(pid_for_speed_control): Vary min and max speeds --- .../drivers/actuators/dc_motor/esp32_dc_motor_ft_smc.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ports/ESP32/src/drivers/actuators/dc_motor/esp32_dc_motor_ft_smc.cpp b/ports/ESP32/src/drivers/actuators/dc_motor/esp32_dc_motor_ft_smc.cpp index 3b4238a..68221ee 100644 --- a/ports/ESP32/src/drivers/actuators/dc_motor/esp32_dc_motor_ft_smc.cpp +++ b/ports/ESP32/src/drivers/actuators/dc_motor/esp32_dc_motor_ft_smc.cpp @@ -125,10 +125,10 @@ void ESP32DCMotorFtSmc::set_raw_speed(unsigned int speed) { _pwmDuty = validate_motor_speed(speed); ledc_set_duty(LEDC_LOW_SPEED_MODE, _motorChannel, _pwmDuty); ledc_update_duty(LEDC_LOW_SPEED_MODE, _motorChannel); - printf("Motor speed set to %u\n", _pwmDuty); +// printf("Motor speed set to %u\n", _pwmDuty); } -#define MAX_CW_PWM_VALUE 79 -#define MIN_CW_PWM_VALUE 77 +#define MAX_CW_PWM_VALUE 80 +#define MIN_CW_PWM_VALUE 78 #define MAX_CCW_PWM_VALUE 77 #define MIN_CCW_PWM_VALUE 73 From ceac1c132707f9e6a49a2ad2de0efac89f8979f1 Mon Sep 17 00:00:00 2001 From: MK Date: Wed, 10 Jul 2024 22:23:07 +0300 Subject: [PATCH 19/28] f(pid_for_speed_control): Vary pid gains --- src/apps/car_app/car_line_following_pid.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/apps/car_app/car_line_following_pid.cpp b/src/apps/car_app/car_line_following_pid.cpp index e6f6b70..c80ffe8 100644 --- a/src/apps/car_app/car_line_following_pid.cpp +++ b/src/apps/car_app/car_line_following_pid.cpp @@ -4,7 +4,7 @@ Infrared5Channel* infraredSensor; Infrared5Channel::detectedChannels sensorDetectedChannels; double lineError; -PID lineFollowingPid(0.5, 3.80, 0.10); +PID lineFollowingPid(0.5, 3.80, 1.10); double PIDOutput; void carAppInit() { From 0ece09415f847f12e1760c04ac5624d25ff51a65 Mon Sep 17 00:00:00 2001 From: MK Date: Wed, 10 Jul 2024 22:25:30 +0300 Subject: [PATCH 20/28] f(pid_for_speed_control): Vary left and right motor base speeds to overcome left right motor variance --- src/services/kinematics_pid.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/services/kinematics_pid.cpp b/src/services/kinematics_pid.cpp index 41f65a4..727ff2e 100644 --- a/src/services/kinematics_pid.cpp +++ b/src/services/kinematics_pid.cpp @@ -5,8 +5,8 @@ BSP::DcMotor* kinematicsLeftMotor = BSP::getDefaultKinematicsLeftMotor(); BSP::DcMotor* kinematicsRightMotor = BSP::getDefaultKinematicsRightMotor(); -#define LEFT_MOTOR_BASE_SPEED 79 -#define RIGHT_MOTOR_BASE_SPEED 78 +#define LEFT_MOTOR_BASE_SPEED 80 +#define RIGHT_MOTOR_BASE_SPEED 79 // #define RIGHT_MOTOR_MAX_SPEED 79 @@ -21,7 +21,7 @@ double controlCarKinematics() { // rightMotorSpeed = RIGHT_MOTOR_MAX_SPEED; // } - // printf("Left Motor: %u || Right Motor: %u\n", leftMotorSpeed, rightMotorSpeed); + printf("Left Motor: %u || Right Motor: %u\n", leftMotorSpeed, rightMotorSpeed); kinematicsLeftMotor->set_dir(BSP::CLOCKWISE); kinematicsLeftMotor->set_raw_speed(leftMotorSpeed); From 1a809ff602c55b11d03760c825d04d1680399097 Mon Sep 17 00:00:00 2001 From: MK Date: Wed, 10 Jul 2024 22:26:58 +0300 Subject: [PATCH 21/28] f(pid_for_speed_control): Ignore clion cmake files --- .gitignore | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.gitignore b/.gitignore index 0a0d0c7..745099f 100644 --- a/.gitignore +++ b/.gitignore @@ -7,3 +7,5 @@ build / cmake-build-debug-gcc_arm/ cmake_build/ cmake_build_clion/ +cmake_build/ +cmake-build-debug/ From a1dd0d49d8d1159ed9b67b129b75767d5ef294a6 Mon Sep 17 00:00:00 2001 From: MK Date: Wed, 7 Aug 2024 16:39:22 +0300 Subject: [PATCH 22/28] f(pid_for_speed_control): Tweak params. PC switch lol --- include/apps/car_app/car_line_following_pid.h | 2 ++ .../actuators/dc_motor/esp32_dc_motor_ft_smc.cpp | 6 +++--- src/RB19.cpp | 2 +- src/apps/car_app/car_line_following_pid.cpp | 13 ++++++++++--- src/services/kinematics_pid.cpp | 8 ++++---- 5 files changed, 20 insertions(+), 11 deletions(-) diff --git a/include/apps/car_app/car_line_following_pid.h b/include/apps/car_app/car_line_following_pid.h index 2fef2b8..c0710a4 100644 --- a/include/apps/car_app/car_line_following_pid.h +++ b/include/apps/car_app/car_line_following_pid.h @@ -6,6 +6,8 @@ extern double PIDOutput; +extern int cornerSpeedDiff; + void carAppInit(); void carAppRun(); diff --git a/ports/ESP32/src/drivers/actuators/dc_motor/esp32_dc_motor_ft_smc.cpp b/ports/ESP32/src/drivers/actuators/dc_motor/esp32_dc_motor_ft_smc.cpp index 68221ee..3afb10f 100644 --- a/ports/ESP32/src/drivers/actuators/dc_motor/esp32_dc_motor_ft_smc.cpp +++ b/ports/ESP32/src/drivers/actuators/dc_motor/esp32_dc_motor_ft_smc.cpp @@ -30,7 +30,7 @@ void ESP32DCMotorFtSmc::init() { void ESP32DCMotorFtSmc::configure_pwm(gpio_num_t signal) { ledc_timer_config_t pwmTimer = { .speed_mode = LEDC_LOW_SPEED_MODE, - .duty_resolution = LEDC_TIMER_10_BIT, + .duty_resolution = LEDC_TIMER_20_BIT, .timer_num = LEDC_TIMER_0, .freq_hz = 50, .clk_cfg = LEDC_AUTO_CLK @@ -127,8 +127,8 @@ void ESP32DCMotorFtSmc::set_raw_speed(unsigned int speed) { ledc_update_duty(LEDC_LOW_SPEED_MODE, _motorChannel); // printf("Motor speed set to %u\n", _pwmDuty); } -#define MAX_CW_PWM_VALUE 80 -#define MIN_CW_PWM_VALUE 78 +#define MAX_CW_PWM_VALUE 88000 +#define MIN_CW_PWM_VALUE 80000 #define MAX_CCW_PWM_VALUE 77 #define MIN_CCW_PWM_VALUE 73 diff --git a/src/RB19.cpp b/src/RB19.cpp index 9ae2c33..475e237 100644 --- a/src/RB19.cpp +++ b/src/RB19.cpp @@ -24,7 +24,7 @@ int main() // steerCar(); // controlCarKinematics(); // } - BSP::BSP_Delay(10000); + BSP::BSP_Delay(20000); BSP::BSPInit(); carAppInit(); // steerCar(); diff --git a/src/apps/car_app/car_line_following_pid.cpp b/src/apps/car_app/car_line_following_pid.cpp index c80ffe8..ca0f5de 100644 --- a/src/apps/car_app/car_line_following_pid.cpp +++ b/src/apps/car_app/car_line_following_pid.cpp @@ -4,9 +4,11 @@ Infrared5Channel* infraredSensor; Infrared5Channel::detectedChannels sensorDetectedChannels; double lineError; -PID lineFollowingPid(0.5, 3.80, 1.10); +PID lineFollowingPid(0.9, 3.80, 959.80); double PIDOutput; +int cornerSpeedDiff; + void carAppInit() { infraredSensor = BSP::getDefaultInfraredSensor(); carAppRun(); @@ -18,18 +20,23 @@ void carAppRun() { { case Infrared5Channel::detectedChannels::centralChannel: lineError = 0.0; + cornerSpeedDiff = 1200; break; case Infrared5Channel::detectedChannels::halfLeftChannel: - lineError = -1.0; + lineError = -0.7; + cornerSpeedDiff = 2000; break; case Infrared5Channel::detectedChannels::farLeftChannel: lineError = -2.0; + cornerSpeedDiff = 2000; break; case Infrared5Channel::detectedChannels::halfRightChannel: - lineError = 1.0; + lineError = 0.7; + cornerSpeedDiff = 2000; break; case Infrared5Channel::detectedChannels::farRightChannel: lineError = 2.0; + cornerSpeedDiff = 2000; break; default: break; diff --git a/src/services/kinematics_pid.cpp b/src/services/kinematics_pid.cpp index 727ff2e..b507425 100644 --- a/src/services/kinematics_pid.cpp +++ b/src/services/kinematics_pid.cpp @@ -5,8 +5,8 @@ BSP::DcMotor* kinematicsLeftMotor = BSP::getDefaultKinematicsLeftMotor(); BSP::DcMotor* kinematicsRightMotor = BSP::getDefaultKinematicsRightMotor(); -#define LEFT_MOTOR_BASE_SPEED 80 -#define RIGHT_MOTOR_BASE_SPEED 79 +#define LEFT_MOTOR_BASE_SPEED 83200 +#define RIGHT_MOTOR_BASE_SPEED 82700 // #define RIGHT_MOTOR_MAX_SPEED 79 @@ -15,8 +15,8 @@ unsigned int rightMotorSpeed; double controlCarKinematics() { - leftMotorSpeed = LEFT_MOTOR_BASE_SPEED - PIDOutput; - rightMotorSpeed = RIGHT_MOTOR_BASE_SPEED + PIDOutput; + leftMotorSpeed = int((LEFT_MOTOR_BASE_SPEED - cornerSpeedDiff) - (PIDOutput*700)); + rightMotorSpeed = int((RIGHT_MOTOR_BASE_SPEED- (float (cornerSpeedDiff)*(float (82700)/float (83200)))) + (PIDOutput*700)); // if (rightMotorSpeed > RIGHT_MOTOR_MAX_SPEED) { // rightMotorSpeed = RIGHT_MOTOR_MAX_SPEED; // } From 941889144fc57e93160f11277c8dc45cc403b1e6 Mon Sep 17 00:00:00 2001 From: MK Date: Sat, 30 Nov 2024 23:58:17 +0300 Subject: [PATCH 23/28] f(pid_for_speed_control): Add missing interfaces to mbed dc_motor --- .../actuators/dc_motor/mbed_dc_motor_ft_smc.h | 10 ++++ .../dc_motor/mbed_dc_motor_ft_smc.cpp | 53 +++++++++++++++++++ 2 files changed, 63 insertions(+) diff --git a/ports/mbed_common/include/drivers/actuators/dc_motor/mbed_dc_motor_ft_smc.h b/ports/mbed_common/include/drivers/actuators/dc_motor/mbed_dc_motor_ft_smc.h index e82f190..669ce93 100644 --- a/ports/mbed_common/include/drivers/actuators/dc_motor/mbed_dc_motor_ft_smc.h +++ b/ports/mbed_common/include/drivers/actuators/dc_motor/mbed_dc_motor_ft_smc.h @@ -25,6 +25,16 @@ class MbedDCMotor: public BSP::DcMotor { void stop() override; + /** + * @brief Validate the set motor speed value + */ + unsigned int validate_motor_speed(unsigned int speed); + + /** + * @brief Sets a motor's raw speed + */ + void set_raw_speed(unsigned int speed) override; + private: PwmOut _motor_signal_pin; BSP::motorDirections _direction; diff --git a/ports/mbed_common/src/drivers/actuators/dc_motor/mbed_dc_motor_ft_smc.cpp b/ports/mbed_common/src/drivers/actuators/dc_motor/mbed_dc_motor_ft_smc.cpp index 55438f9..429f6c8 100644 --- a/ports/mbed_common/src/drivers/actuators/dc_motor/mbed_dc_motor_ft_smc.cpp +++ b/ports/mbed_common/src/drivers/actuators/dc_motor/mbed_dc_motor_ft_smc.cpp @@ -99,6 +99,59 @@ void MbedDCMotor::set_speed(const BSP::motorSpeeds speed) { printf("Motor speed set to %.2f\n", _pwmDuty); } +#define MAX_CW_PWM_VALUE 88000 +#define MIN_CW_PWM_VALUE 80000 + +#define MAX_CCW_PWM_VALUE 77 +#define MIN_CCW_PWM_VALUE 73 + +unsigned int MbedDCMotor::validate_motor_speed(unsigned int speed) { + unsigned int validatedSpeed; + switch (_direction) + { + case BSP::motorDirections::CLOCKWISE: + /* code */ + if(speed > MAX_CW_PWM_VALUE) { + validatedSpeed = MAX_CW_PWM_VALUE; + return validatedSpeed; + } + else if (speed < MIN_CW_PWM_VALUE) { + validatedSpeed = MIN_CW_PWM_VALUE; + return validatedSpeed; + } + else { + validatedSpeed = speed; + return validatedSpeed; + } + break; + case BSP::motorDirections::COUNTERCLOCKWISE: + /* code */ + if(speed > MAX_CCW_PWM_VALUE) { + validatedSpeed = MAX_CCW_PWM_VALUE; + return validatedSpeed; + } + else if (speed < MIN_CCW_PWM_VALUE) { + validatedSpeed = MIN_CCW_PWM_VALUE; + return validatedSpeed; + } + else { + validatedSpeed = speed; + return validatedSpeed; + } + break; + + default: + validatedSpeed = 0; + return validatedSpeed; + break; + } +} + +void MbedDCMotor::set_raw_speed(unsigned int speed) { + _pwmDuty = validate_motor_speed(speed); + // printf("Motor speed set to %u\n", _pwmDuty); +} + void MbedDCMotor::run() { _motor_signal_pin.write(_pwmDuty); } From de32a6d8d2b1b1ef62bb77f14731f632636e58c5 Mon Sep 17 00:00:00 2001 From: MK Date: Sun, 1 Dec 2024 00:09:54 +0300 Subject: [PATCH 24/28] f(pid_for_speed_control): Customize pwm values for mbed pwm driver --- .../drivers/actuators/dc_motor/mbed_dc_motor_ft_smc.h | 2 +- .../actuators/dc_motor/mbed_dc_motor_ft_smc.cpp | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/ports/mbed_common/include/drivers/actuators/dc_motor/mbed_dc_motor_ft_smc.h b/ports/mbed_common/include/drivers/actuators/dc_motor/mbed_dc_motor_ft_smc.h index 669ce93..a9b60c3 100644 --- a/ports/mbed_common/include/drivers/actuators/dc_motor/mbed_dc_motor_ft_smc.h +++ b/ports/mbed_common/include/drivers/actuators/dc_motor/mbed_dc_motor_ft_smc.h @@ -28,7 +28,7 @@ class MbedDCMotor: public BSP::DcMotor { /** * @brief Validate the set motor speed value */ - unsigned int validate_motor_speed(unsigned int speed); + float validate_motor_speed(unsigned int speed); /** * @brief Sets a motor's raw speed diff --git a/ports/mbed_common/src/drivers/actuators/dc_motor/mbed_dc_motor_ft_smc.cpp b/ports/mbed_common/src/drivers/actuators/dc_motor/mbed_dc_motor_ft_smc.cpp index 429f6c8..e9e8b48 100644 --- a/ports/mbed_common/src/drivers/actuators/dc_motor/mbed_dc_motor_ft_smc.cpp +++ b/ports/mbed_common/src/drivers/actuators/dc_motor/mbed_dc_motor_ft_smc.cpp @@ -105,8 +105,8 @@ void MbedDCMotor::set_speed(const BSP::motorSpeeds speed) { #define MAX_CCW_PWM_VALUE 77 #define MIN_CCW_PWM_VALUE 73 -unsigned int MbedDCMotor::validate_motor_speed(unsigned int speed) { - unsigned int validatedSpeed; +float MbedDCMotor::validate_motor_speed(unsigned int speed) { + float validatedSpeed; switch (_direction) { case BSP::motorDirections::CLOCKWISE: @@ -120,7 +120,7 @@ unsigned int MbedDCMotor::validate_motor_speed(unsigned int speed) { return validatedSpeed; } else { - validatedSpeed = speed; + validatedSpeed = static_cast(speed); return validatedSpeed; } break; @@ -135,14 +135,14 @@ unsigned int MbedDCMotor::validate_motor_speed(unsigned int speed) { return validatedSpeed; } else { - validatedSpeed = speed; + validatedSpeed = static_cast(speed); return validatedSpeed; } break; default: validatedSpeed = 0; - return validatedSpeed; + return validatedSpeed/1048576.0f; break; } } From 0b63d54059356ca93ffa10fdea93fc68efd77949 Mon Sep 17 00:00:00 2001 From: MK Date: Sun, 1 Dec 2024 00:10:50 +0300 Subject: [PATCH 25/28] f(pid_for_speed_control): Change of folders due to rebase --- main/CMakeLists.txt | 0 {main => src}/math_utils/pid/pid.cpp | 0 2 files changed, 0 insertions(+), 0 deletions(-) delete mode 100644 main/CMakeLists.txt rename {main => src}/math_utils/pid/pid.cpp (100%) diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt deleted file mode 100644 index e69de29..0000000 diff --git a/main/math_utils/pid/pid.cpp b/src/math_utils/pid/pid.cpp similarity index 100% rename from main/math_utils/pid/pid.cpp rename to src/math_utils/pid/pid.cpp From f46d1d244ec6d46fa61310bb2c95753582d8afd5 Mon Sep 17 00:00:00 2001 From: MK Date: Sun, 1 Dec 2024 00:11:12 +0300 Subject: [PATCH 26/28] f(pid_for_speed_control): Suppress cmake warning --- cmake/app.cmake | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/cmake/app.cmake b/cmake/app.cmake index 650180d..289aadf 100644 --- a/cmake/app.cmake +++ b/cmake/app.cmake @@ -8,6 +8,8 @@ set(APP_COMMON_INCLUDE_DIRS "${CMAKE_SOURCE_DIR}/include/bsp/drivers/actuators" "${CMAKE_SOURCE_DIR}/include/bsp/drivers/sensors" + "${CMAKE_SOURCE_DIR}/include/math_utils/pid" + "${CMAKE_SOURCE_DIR}/include/apps/car_app" "${CMAKE_SOURCE_DIR}/include/services/kinematics" @@ -17,7 +19,10 @@ set(APP_COMMON_INCLUDE_DIRS set(APP_COMMON_SOURCES "${CMAKE_SOURCE_DIR}/src/RB19.cpp" - "${CMAKE_SOURCE_DIR}/src/apps/car_app/car_line_following.cpp" - "${CMAKE_SOURCE_DIR}/src/services/kinematics.cpp" + "${CMAKE_SOURCE_DIR}/src/math_utils/pid/pid.cpp" +# "${CMAKE_SOURCE_DIR}/src/apps/car_app/car_line_following.cpp" + "${CMAKE_SOURCE_DIR}/src/apps/car_app/car_line_following_pid.cpp" +# "${CMAKE_SOURCE_DIR}/src/services/kinematics.cpp" + "${CMAKE_SOURCE_DIR}/src/services/kinematics_pid.cpp" "${CMAKE_SOURCE_DIR}/src/services/steering.cpp" ) From bbcf6f3da1d4746370211760d2802cc150b84c7d Mon Sep 17 00:00:00 2001 From: MK Date: Wed, 25 Dec 2024 23:43:54 +0300 Subject: [PATCH 27/28] f(pid_for_speed_control): Remove max PID checks --- src/math_utils/pid/pid.cpp | 22 ++++++++++++---------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/src/math_utils/pid/pid.cpp b/src/math_utils/pid/pid.cpp index c585ba8..5f90ecf 100644 --- a/src/math_utils/pid/pid.cpp +++ b/src/math_utils/pid/pid.cpp @@ -29,20 +29,22 @@ double PID::calculateOutput(double setPoint, double processVariable) { } _derivative = (_error - _previousError)/_dt; - printf("Integral: %.2f || Derivative: %.2f\n", _integral, _derivative); + printf("Proportional: %.2f || Integral: %.2f || Derivative: %.2f\n",_error, _integral, _derivative); output = _kp*_error + _ki*_integral + _kd * _derivative; - if (abs(output - _previousOutput) > MAX_PID_DIFF) { - output = _previousOutput; - } - if (output > MAX_PID_OUTPUT) { - output = _previousOutput; - } - if (output < MIN_PID_OUTPUT) { - output = _previousOutput; - } + // if (abs(output - _previousOutput) > MAX_PID_DIFF) { + // output = _previousOutput; + // } + // if (output > MAX_PID_OUTPUT) { + // output = _previousOutput; + // } + // if (output < MIN_PID_OUTPUT) { + // output = _previousOutput; + // } _previousError = _error; _previousOutput = output; + printf("PID D Output: %.2f\n", output); + return output; From c3149dda96206333cd1ab140d8924f423cab5a53 Mon Sep 17 00:00:00 2001 From: MK Date: Thu, 30 Jan 2025 23:54:46 +0300 Subject: [PATCH 28/28] f(pid_for_speed_control): Tune PI constants --- src/apps/car_app/car_line_following_pid.cpp | 10 +++++----- src/math_utils/pid/pid.cpp | 5 ++++- src/services/kinematics_pid.cpp | 8 ++++---- 3 files changed, 13 insertions(+), 10 deletions(-) diff --git a/src/apps/car_app/car_line_following_pid.cpp b/src/apps/car_app/car_line_following_pid.cpp index ca0f5de..e66a9df 100644 --- a/src/apps/car_app/car_line_following_pid.cpp +++ b/src/apps/car_app/car_line_following_pid.cpp @@ -4,7 +4,7 @@ Infrared5Channel* infraredSensor; Infrared5Channel::detectedChannels sensorDetectedChannels; double lineError; -PID lineFollowingPid(0.9, 3.80, 959.80); +PID lineFollowingPid(0.9, 3.80, 1.5); double PIDOutput; int cornerSpeedDiff; @@ -20,14 +20,14 @@ void carAppRun() { { case Infrared5Channel::detectedChannels::centralChannel: lineError = 0.0; - cornerSpeedDiff = 1200; + cornerSpeedDiff = 2000; break; case Infrared5Channel::detectedChannels::halfLeftChannel: lineError = -0.7; cornerSpeedDiff = 2000; break; case Infrared5Channel::detectedChannels::farLeftChannel: - lineError = -2.0; + lineError = -1.4; cornerSpeedDiff = 2000; break; case Infrared5Channel::detectedChannels::halfRightChannel: @@ -35,14 +35,14 @@ void carAppRun() { cornerSpeedDiff = 2000; break; case Infrared5Channel::detectedChannels::farRightChannel: - lineError = 2.0; + lineError = 1.4; cornerSpeedDiff = 2000; break; default: break; } - lineFollowingPid._dt = 0.025; + lineFollowingPid._dt = 0.25; PIDOutput = lineFollowingPid.calculateOutput(0, lineError); diff --git a/src/math_utils/pid/pid.cpp b/src/math_utils/pid/pid.cpp index 5f90ecf..9764ec3 100644 --- a/src/math_utils/pid/pid.cpp +++ b/src/math_utils/pid/pid.cpp @@ -29,7 +29,7 @@ double PID::calculateOutput(double setPoint, double processVariable) { } _derivative = (_error - _previousError)/_dt; - printf("Proportional: %.2f || Integral: %.2f || Derivative: %.2f\n",_error, _integral, _derivative); + output = _kp*_error + _ki*_integral + _kd * _derivative; // if (abs(output - _previousOutput) > MAX_PID_DIFF) { @@ -43,7 +43,10 @@ double PID::calculateOutput(double setPoint, double processVariable) { // } _previousError = _error; _previousOutput = output; + printf("\n\n\n ============= PID LOGGING =============\n"); + printf("Proportional: %.2f || Integral: %.2f || Derivative: %.2f\n",_error, _integral, _derivative); printf("PID D Output: %.2f\n", output); + printf("============= PID LOGGING ============= \n\n\n"); diff --git a/src/services/kinematics_pid.cpp b/src/services/kinematics_pid.cpp index b507425..afe5d8b 100644 --- a/src/services/kinematics_pid.cpp +++ b/src/services/kinematics_pid.cpp @@ -5,8 +5,8 @@ BSP::DcMotor* kinematicsLeftMotor = BSP::getDefaultKinematicsLeftMotor(); BSP::DcMotor* kinematicsRightMotor = BSP::getDefaultKinematicsRightMotor(); -#define LEFT_MOTOR_BASE_SPEED 83200 -#define RIGHT_MOTOR_BASE_SPEED 82700 +#define LEFT_MOTOR_BASE_SPEED 83200.0f +#define RIGHT_MOTOR_BASE_SPEED 82750.0f // #define RIGHT_MOTOR_MAX_SPEED 79 @@ -15,8 +15,8 @@ unsigned int rightMotorSpeed; double controlCarKinematics() { - leftMotorSpeed = int((LEFT_MOTOR_BASE_SPEED - cornerSpeedDiff) - (PIDOutput*700)); - rightMotorSpeed = int((RIGHT_MOTOR_BASE_SPEED- (float (cornerSpeedDiff)*(float (82700)/float (83200)))) + (PIDOutput*700)); + leftMotorSpeed = int((LEFT_MOTOR_BASE_SPEED - float(cornerSpeedDiff)) - (PIDOutput*700)); + rightMotorSpeed = int((RIGHT_MOTOR_BASE_SPEED- (float (cornerSpeedDiff)*(float (RIGHT_MOTOR_BASE_SPEED)/float (LEFT_MOTOR_BASE_SPEED)))) + (PIDOutput*700)); // if (rightMotorSpeed > RIGHT_MOTOR_MAX_SPEED) { // rightMotorSpeed = RIGHT_MOTOR_MAX_SPEED; // }