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/
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"
 )
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..c0710a4
--- /dev/null
+++ b/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
\ No newline at end of file
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;
 };
 
 }
diff --git a/include/math_utils/pid/pid.h b/include/math_utils/pid/pid.h
new file mode 100644
index 0000000..29fef4b
--- /dev/null
+++ b/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
+
+
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/include/services/kinematics/kinematics_pid.h b/include/services/kinematics/kinematics_pid.h
new file mode 100644
index 0000000..982c27b
--- /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
+*/
+double controlCarKinematics();
+
+
+#endif
\ No newline at end of file
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..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
@@ -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;
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..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
@@ -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() {
     
 }
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
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..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
@@ -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;
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..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
@@ -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);
 }
diff --git a/src/RB19.cpp b/src/RB19.cpp
index da9912e..475e237 100644
--- a/src/RB19.cpp
+++ b/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);
     }
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/apps/car_app/car_line_following_pid.cpp b/src/apps/car_app/car_line_following_pid.cpp
new file mode 100644
index 0000000..e66a9df
--- /dev/null
+++ b/src/apps/car_app/car_line_following_pid.cpp
@@ -0,0 +1,49 @@
+#include "car_line_following_pid.h"
+
+Infrared5Channel* infraredSensor;
+Infrared5Channel::detectedChannels sensorDetectedChannels;
+
+double lineError;
+PID lineFollowingPid(0.9, 3.80, 1.5);
+double PIDOutput;
+
+int cornerSpeedDiff;
+
+void carAppInit() {
+    infraredSensor = BSP::getDefaultInfraredSensor();
+    carAppRun();
+}
+
+void carAppRun() {
+    sensorDetectedChannels = infraredSensor->read();
+    switch (sensorDetectedChannels)
+    {
+    case Infrared5Channel::detectedChannels::centralChannel:
+        lineError = 0.0;
+        cornerSpeedDiff = 2000;
+        break;
+    case Infrared5Channel::detectedChannels::halfLeftChannel:
+        lineError = -0.7;
+        cornerSpeedDiff = 2000;
+        break;
+    case Infrared5Channel::detectedChannels::farLeftChannel:
+        lineError = -1.4;
+        cornerSpeedDiff = 2000;
+        break;
+    case Infrared5Channel::detectedChannels::halfRightChannel:
+        lineError = 0.7;
+        cornerSpeedDiff = 2000;
+        break;
+    case Infrared5Channel::detectedChannels::farRightChannel:
+        lineError = 1.4;
+        cornerSpeedDiff = 2000;
+        break;
+    default:
+        break;
+    }
+
+    lineFollowingPid._dt = 0.25;
+    PIDOutput = lineFollowingPid.calculateOutput(0, lineError);
+
+
+}
\ No newline at end of file
diff --git a/src/math_utils/pid/pid.cpp b/src/math_utils/pid/pid.cpp
new file mode 100644
index 0000000..9764ec3
--- /dev/null
+++ b/src/math_utils/pid/pid.cpp
@@ -0,0 +1,54 @@
+#include "pid.h"
+#include <cstdlib>
+#include <stdio.h>
+
+PID::PID(double kp, double ki, double kd):
+    _kp(kp), _ki(ki), _kd(kd)
+{
+
+}
+
+#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;
+    _processVariable = processVariable;
+
+    _error = _setPoint - _processVariable;
+    _integral += 0;
+    if (_integral > MAX_INTEGRAL_VALUE) {
+        _integral = MAX_INTEGRAL_VALUE;
+    }
+    if (_integral < MIN_INTEGRAL_VALUE) {
+        _integral = MIN_INTEGRAL_VALUE;
+    }
+    _derivative = (_error - _previousError)/_dt;
+
+
+
+    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;
+    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");
+
+    
+
+    return output;
+}
\ No newline at end of file
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() {
 
diff --git a/src/services/kinematics_pid.cpp b/src/services/kinematics_pid.cpp
new file mode 100644
index 0000000..afe5d8b
--- /dev/null
+++ b/src/services/kinematics_pid.cpp
@@ -0,0 +1,32 @@
+#include "kinematics_pid.h"
+#include <stdio.h>
+#include "bsp.h"
+
+BSP::DcMotor* kinematicsLeftMotor = BSP::getDefaultKinematicsLeftMotor();
+BSP::DcMotor* kinematicsRightMotor = BSP::getDefaultKinematicsRightMotor();
+
+#define LEFT_MOTOR_BASE_SPEED 83200.0f
+#define RIGHT_MOTOR_BASE_SPEED 82750.0f
+// #define RIGHT_MOTOR_MAX_SPEED 79
+
+
+unsigned int leftMotorSpeed;
+unsigned int rightMotorSpeed;
+
+double controlCarKinematics() {
+
+    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;
+    // }
+
+     printf("Left Motor: %u || Right Motor: %u\n", leftMotorSpeed, rightMotorSpeed);
+
+    kinematicsLeftMotor->set_dir(BSP::CLOCKWISE);
+    kinematicsLeftMotor->set_raw_speed(leftMotorSpeed);
+
+    kinematicsRightMotor->set_dir(BSP::CLOCKWISE);
+    kinematicsRightMotor->set_raw_speed(rightMotorSpeed);
+    return PIDOutput;
+}
\ No newline at end of file