Add PID control

.gitignore

@@ -7,3 +7,5 @@ build /
 cmake-build-debug-gcc_arm/
 cmake_build/
 cmake_build_clion/
+cmake_build/
+cmake-build-debug/

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"
 )

include/apps/car_app/car_line_following_pid.h

@@ -0,0 +1,15 @@
+#ifndef __CAR_LF__PID
+#define __CAR_LF__PID
+
+#include "pid.h"
+#include "bsp.h"
+
+extern double PIDOutput;
+
+extern int cornerSpeedDiff;
+
+void carAppInit();
+
+void carAppRun();
+
+#endif

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;
 };
 
 }

include/math_utils/pid/pid.h

@@ -0,0 +1,33 @@
+#ifndef __PID__
+#define __PID__
+class PID
+{
+
+public:
+    PID(double kp, double ki, double kd);
+    // ~pid();
+public:
+
+    double output;
+    double _dt;
+
+    double calculateOutput(double setPoint, double processVariable);
+
+private:
+    /* data */
+    double _setPoint;
+    double _processVariable;
+    double _error;
+    double _previousError;
+    double _integral;
+    double _derivative;
+    double _kp;
+    double _ki;
+    double _kd;
+    double _previousOutput;
+
+};
+
+#endif
+
+

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

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
+*/
+double controlCarKinematics();
+
+
+#endif

ports/ESP32/include/drivers/actuators/dc_motor/esp32_dc_motor_ft_smc.h

@@ -24,6 +24,16 @@ 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;
+
+    /**
+     * @brief Validate the set motor speed value
+    */
+    unsigned int validate_motor_speed(unsigned int speed);
 private:
     gpio_num_t _signal;
     unsigned int _pwmDuty;

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;
+
 
 
 };

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;
 
@@ -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
@@ -121,6 +121,60 @@ void ESP32DCMotorFtSmc::set_speed(BSP::motorSpeeds speed) {
     printf("Motor speed set to %u\n", _pwmDuty);
 }
 
+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);
+}
+#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 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() {
 
 }

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) {
+
 }

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
+    */
+    float 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;

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
+
+float MbedDCMotor::validate_motor_speed(unsigned int speed) {
+    float 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 = static_cast<float>(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 = static_cast<float>(speed);
+                    return validatedSpeed;
+                }
+        break;
+
+        default:
+            validatedSpeed = 0;
+        return validatedSpeed/1048576.0f;
+        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);
 }

src/RB19.cpp

@@ -1,8 +1,8 @@
 #include <stdio.h>
 #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 <freertos/FreeRTOS.h>
 // #include <freertos/task.h>
@@ -24,15 +24,18 @@ int main()
     //     steerCar();
     //     controlCarKinematics();
     // }
-    BSP::BSP_Delay(10000);
+    BSP::BSP_Delay(20000);
     BSP::BSPInit();
     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);
     }

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;