diff --git a/Core/Inc/CommCallbacks.h b/Core/Inc/CommCallbacks.h
index 6443c49..8514b3d 100644
--- a/Core/Inc/CommCallbacks.h
+++ b/Core/Inc/CommCallbacks.h
@@ -11,8 +11,9 @@ void Comm_SetPos(float x, float y, float t);
 
 void Comm_GetSpeed(float& vx, float& vy, float& omega);
 
-void Comm_GetPID(float& p, float& i, float& d);
-void Comm_SetPID(float p, float i, float d);
+bool Comm_GetPID(char *pid, float &p, float &i, float &d);
+
+bool Comm_SetPID(char *pid, float p, float i, float d);
 
 void Comm_StartOdometry(bool start);
 
@@ -20,6 +21,8 @@ void Comm_AddWaypoint(int id, int type, float x, float y, float t);
 
 float Comm_GetDistance(int sensorId);
 
+void Comm_SetPWM(float left, float right);
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/Core/Inc/commSTM.h b/Core/Inc/commSTM.h
index 16848ab..2942cd4 100644
--- a/Core/Inc/commSTM.h
+++ b/Core/Inc/commSTM.h
@@ -8,7 +8,7 @@
 #ifndef INC_COMMSTM_H_
 #define INC_COMMSTM_H_
 
-#include "usbd_cdc_if.h"
+#include <stdint.h>
 
 #ifdef __cplusplus
 extern "C" {
diff --git a/Core/Inc/modelec.h b/Core/Inc/modelec.h
index a4db7d5..bb75c6d 100644
--- a/Core/Inc/modelec.h
+++ b/Core/Inc/modelec.h
@@ -8,23 +8,23 @@
 #ifndef MODELEC_H
 #define MODELEC_H
 
-#include "motors.h"
-#include "main.h"
 #include "stm32g4xx_hal.h"
+#include "usbd_cdc_if.h"
 
-#include <cstdio>
-#include <cstring>
-#include <cmath>
-#include <algorithm>
+#include "motors.h"
 #include "pid.h"
 #include "point.h"
-#include "CommCallbacks.h"
-#include "usbd_cdc_if.h"
-#include "commSTM.h"
 
-#ifdef __cplusplus
-extern "C" {
-#endif
+#define MAX_WAYPOINTS 16
+#define PWM_MAX 626.0f
+#define ENCODER_RES 2400.0f
+#define WHEEL_DIAMETER 0.081f
+#define WHEEL_RADIUS (WHEEL_DIAMETER/2.0f)
+#define WHEEL_BASE 0.287f
+#define WHEEL_BASE_2 (WHEEL_BASE/2.0f)
+#define PRECISE_ANGLE 0.017f // radians
+#define PRECISE_POS_FINAL 0.005f // meters
+#define PRECISE_POS 0.1f // meters
 
 extern TIM_HandleTypeDef htim3;
 extern TIM_HandleTypeDef htim2;
@@ -33,11 +33,13 @@ class DiffBot {
 public:
 	Point pose;
 
-	Point targets[10];
+	Point targets[MAX_WAYPOINTS];
 	uint8_t index = 0;
 
     Motor motor;
 
+	float vx, vy, vtheta;
+
     float dt;
 
     PID pidLeft, pidRight;
@@ -48,13 +50,6 @@ public:
 	bool odo_active = false;
 	bool arrive = false;
 
-	int16_t PWM_MAX = 626;
-	float ENCODER_RES = 2400.0f;
-	float WHEEL_DIAMETER = 0.081f;
-	float WHEEL_RADIUS = 0.0405f;
-	float WHEEL_BASE = 0.287f;
-	float WHEEL_BASE_2 = 0.1435f;
-
 	uint32_t lastTick = 0;
 
 	static bool isDelayPassedFrom(uint32_t delay, uint32_t& lastTick);
@@ -75,10 +70,7 @@ public:
 
     void addTarget(int id, int type, float x, float y, float theta);
 
+	void resetPID();
 };
 
-#ifdef __cplusplus
-}
-#endif
-
 #endif // MODELEC_H
diff --git a/Core/Inc/motors.h b/Core/Inc/motors.h
index 220d6b0..d773ee8 100644
--- a/Core/Inc/motors.h
+++ b/Core/Inc/motors.h
@@ -1,14 +1,13 @@
 #ifndef INC_MOTORS_H_
 #define INC_MOTORS_H_
 
-//#include "stm32l0xx_hal.h"
-#include "stm32g4xx_hal.h"
+float approach(float current, float target, float step);
 
 class Motor {
 public:
-    int16_t leftTarget_PWM = 0, rightTarget_PWM = 0;
-    int16_t leftCurrent_PWM = 0, rightCurrent_PWM = 0;
-    bool bStop;
+    float leftTarget_PWM = 0, rightTarget_PWM = 0;
+    float leftCurrent_PWM = 0, rightCurrent_PWM = 0;
+    bool bStop = false;
 
     void update();
     void stop(bool stop);
diff --git a/Core/Inc/pid.h b/Core/Inc/pid.h
index 96658bb..356b9ca 100644
--- a/Core/Inc/pid.h
+++ b/Core/Inc/pid.h
@@ -8,8 +8,6 @@
 #ifndef INC_PID_H_
 #define INC_PID_H_
 
-#include <algorithm>
-
 class PID {
 protected:
 
@@ -22,6 +20,18 @@ public:
     PID(float kp = 0.0f, float ki = 0.0f, float kd = 0.0f, float outMin = 0.0f, float outMax = 0.0f);
 
     float compute(float setpoint, float measurement, float dt);
+
+    void reset();
+
+    void setTunings(float kp, float ki, float kd) {
+        this->kp = kp;
+        this->ki = ki;
+        this->kd = kd;
+    }
+
+    float getKp() const { return kp; }
+    float getKi() const { return ki; }
+    float getKd() const { return kd; }
 };
 
 
diff --git a/Core/Inc/point.h b/Core/Inc/point.h
index 2226bfa..54ed4b4 100644
--- a/Core/Inc/point.h
+++ b/Core/Inc/point.h
@@ -24,7 +24,7 @@ public:
     float x;
     float y;
     float theta;
-    bool active = false;
+    bool active;
 
     Point(uint8_t id = 0, StatePoint state = StatePoint::INTERMEDIAIRE, float x = 0.0, float y = 0.0, float theta = 0.0, bool active = false);
 };
diff --git a/Core/Src/CommCallbacks.cpp b/Core/Src/CommCallbacks.cpp
index fd75b1c..c814727 100644
--- a/Core/Src/CommCallbacks.cpp
+++ b/Core/Src/CommCallbacks.cpp
@@ -30,10 +30,50 @@ void Comm_SetPos(float x, float y, float theta) {
 void Comm_GetSpeed(float& vx, float& vy, float& omega){
 }
 
-void Comm_GetPID(float& p, float& i, float& d) {
+bool Comm_GetPID(char *pid, float &p, float &i, float &d) {
+	if (strcmp(pid, "LEFT") == 0) {
+		p = bot.pidLeft.getKp();
+		i = bot.pidLeft.getKi();
+		d = bot.pidLeft.getKd();
+	}
+	else if (strcmp(pid, "RIGHT") == 0) {
+		p = bot.pidRight.getKp();
+		i = bot.pidRight.getKi();
+		d = bot.pidRight.getKd();
+	}
+	else if (strcmp(pid, "POS") == 0) {
+		p = bot.pidPos.getKp();
+		i = bot.pidPos.getKi();
+		d = bot.pidPos.getKd();
+	}
+	else if (strcmp(pid, "THETA") == 0) {
+		p = bot.pidTheta.getKp();
+		i = bot.pidTheta.getKi();
+		d = bot.pidTheta.getKd();
+	}
+	else {
+		return false;
+	}
+	return true;
 }
 
-void Comm_SetPID(float p, float i, float d) {
+bool Comm_SetPID(char *pid, float p, float i, float d) {
+	if (strcmp(pid, "LEFT") == 0) {
+		bot.pidLeft.setTunings(p, i, d);
+	}
+	else if (strcmp(pid, "RIGHT") == 0) {
+		bot.pidRight.setTunings(p, i, d);
+	}
+	else if (strcmp(pid, "POS") == 0) {
+		bot.pidPos.setTunings(p, i, d);
+	}
+	else if (strcmp(pid, "THETA") == 0) {
+		bot.pidTheta.setTunings(p, i, d);
+	}
+	else {
+		return false;
+	}
+	return true;
 }
 
 void Comm_StartOdometry(bool on) {
@@ -47,3 +87,8 @@ void Comm_AddWaypoint(int id, int type, float x, float y, float theta) {
 float Comm_GetDistance(int n) {
     return 0.0f;
 }
+
+void Comm_SetPWM(float left, float right) {
+	bot.motor.leftTarget_PWM = left;
+	bot.motor.rightTarget_PWM = right;
+}
diff --git a/Core/Src/commSTM.cpp b/Core/Src/commSTM.cpp
index 5cfef62..4f3569c 100644
--- a/Core/Src/commSTM.cpp
+++ b/Core/Src/commSTM.cpp
@@ -5,9 +5,11 @@
  *      Author: maxch
  */
 
-#include <CommCallbacks.h>
+#include "CommCallbacks.h"
 #include "commSTM.h"
-#include "usbd_cdc.h"
+
+#include "modelec.h"
+#include "usbd_cdc_if.h"
 
 // fourni par CubeMX dans usb_device.c
 extern USBD_HandleTypeDef hUsbDeviceFS;
@@ -75,11 +77,16 @@ void USB_Comm_Process(void) {
             USB_Comm_Send(response);
         }
         else if (strcmp(token, "PID") == 0) {
+            char* pid = strtok(NULL, ";");
             float p, i, d;
-            Comm_GetPID(p, i, d);
-            char response[64];
-            snprintf(response, sizeof(response), "SET;PID;%.4f;%.4f;%.4f\n", p, i, d);
-            USB_Comm_Send(response);
+            if (Comm_GetPID(pid, p, i, d)) {
+                char response[64];
+                snprintf(response, sizeof(response), "SET;PID;%.4f;%.4f;%.4f\n", p, i, d);
+                USB_Comm_Send(response);
+            }
+            else {
+                USB_Comm_Send("KO;PID;UNKNOWN\n");
+            }
         }
         else if (strcmp(token, "DIST") == 0) {
             token = strtok(NULL, ";");
@@ -106,11 +113,17 @@ void USB_Comm_Process(void) {
             USB_Comm_Send("OK;POS\n");
         }
         else if (strcmp(token, "PID") == 0) {
+            char* pid = strtok(NULL, ";");
             float p = atof(strtok(NULL, ";"));
             float i = atof(strtok(NULL, ";"));
             float d = atof(strtok(NULL, ";"));
-            Comm_SetPID(p, i, d);
-            USB_Comm_Send("OK;PID\n");
+
+            if (Comm_SetPID(pid, p, i, d)) {
+                USB_Comm_Send("OK;PID\n");
+            }
+            else {
+                USB_Comm_Send("KO;PID;UNKNOWN\n");
+            }
         }
         else if (strcmp(token, "WAYPOINT") == 0) {
 
@@ -142,6 +155,19 @@ void USB_Comm_Process(void) {
 
         	USB_Comm_Send("OK;START\n");
         }
+        else if (strcmp(token, "MOTOR") == 0) {
+            float left = atof(strtok(NULL, ";"));
+            float right = atof(strtok(NULL, ";"));
+
+            if (left < -PWM_MAX || left > PWM_MAX || right < -PWM_MAX || right > PWM_MAX) {
+                USB_Comm_Send("KO;MOTOR;RANGE\n");
+                return;
+            }
+
+            Comm_SetPWM(left, right);
+
+            USB_Comm_Send("OK;MOTOR\n");
+        }
         else {
             USB_Comm_Send("KO;UNKNOWN\n");
         }
diff --git a/Core/Src/modelec.cpp b/Core/Src/modelec.cpp
index 5b8a993..72f9de4 100644
--- a/Core/Src/modelec.cpp
+++ b/Core/Src/modelec.cpp
@@ -1,28 +1,14 @@
 
 #include "modelec.h"
 
-// Variables globales
-// Constants
+#include <algorithm>
+#include <cmath>
+
 // #define COUNTS_PER_REV    2400.0f    // 600 PPR × 4
 // #define WHEEL_DIAMETER    0.081f       // meters
 // #define WHEEL_BASE        0.287f       // meters
 // #define WHEEL_CIRCUMFERENCE (M_PI * WHEEL_DIAMETER)
 
-	/**out_pid = new PidPosition(
-		0.8,   // kp — un poil plus agressif, il pousse plus vers la cible
-	    0.0,   // ki — toujours off pour éviter du dépassement imprévu
-	    0.015, // kd — un peu moins de freinage anticipé
-
-	    0.5,   // kpTheta — peut rester soft pour éviter les oscillations d’orientation
-	    0.0,   // kiTheta
-	    0.15,  // kdTheta — un peu moins de frein sur la rotation
-		2,
-	    Point()
-	);*/
-
-	// *out_pidG = new PidVitesse(0.2, 0.0, 0.01, 0);
-	// *out_pidD = new PidVitesse(0.2, 0.0, 0.01, 0);
-
 bool DiffBot::isDelayPassedFrom(uint32_t delay, uint32_t &lastTick) {
 	if (HAL_GetTick() - lastTick >= delay) {
 		lastTick = HAL_GetTick();
@@ -54,6 +40,11 @@ float DiffBot::readEncoderRight() {
 DiffBot::DiffBot(Point pose, float dt) : pose(pose), motor(), dt(dt) {
 };
 
+void DiffBot::stop(bool stop) {
+	odo_active = !stop;
+	motor.stop(stop);
+}
+
 void DiffBot::setup() {
 	pidLeft = PID(1, 0.0, 0.0, -PWM_MAX, PWM_MAX);
 	pidRight = PID(1, 0.0, 0.0, -PWM_MAX, PWM_MAX);
@@ -64,11 +55,6 @@ void DiffBot::setup() {
 	prevCountRight = __HAL_TIM_GET_COUNTER(&htim3);
 }
 
-void DiffBot::stop(bool stop) {
-	odo_active = !stop;
-	motor.stop(stop);
-}
-
 void DiffBot::update(float dt) {
 	if (!isDelayPassed(dt*1000)) return;
 
@@ -91,47 +77,14 @@ void DiffBot::update(float dt) {
     	return;
     }
 
-    /*
-     *
-     *
-     * TODO
-     * check les valeurs htim (encodeur) et TIM (moteur)
-     *
-     *
-     * */
-
     // pid setup
     float dx = targets[index].x - pose.x;
     float dy = targets[index].y - pose.y;
 
-    const float minRes = 0.01f;
-    if (fabsf(dx) < minRes) dx = 0;
-    if (fabsf(dy) < minRes) dy = 0;
-
-    float dist = sqrtf(dx*dx + dy*dy);
-
-    float angleTarget = atan2f(dy, dx);
-    float angleError = angleTarget - pose.theta;
-
-    while (angleError > M_PI) angleError -= 2*M_PI;
-    while (angleError < -M_PI) angleError += 2*M_PI;
-
-    float direction = 1.0f;
-    if (fabs(angleError) > M_PI/2) {
-    	direction = -1.0f;
-        angleError > 0 ? angleError -= M_PI : angleError += M_PI;
-    }
-
-    if (fabs(angleError) <= 0.001) angleError = 0;
-
-    float distError = dist * cosf(angleError);
-
-    distError *= direction;
-
     switch (targets[index].state) {
     case FINAL:
 
-    	if (fabs(dx) <= 0.01 && fabs(dy) <= 0.01 && fabs(targets[index].theta - pose.theta) < 0.01) {
+    	if (std::fabs(dx) <= PRECISE_POS_FINAL && std::fabs(dy) <= PRECISE_POS_FINAL && std::fabs(targets[index].theta - pose.theta) < PRECISE_ANGLE) {
     		targets[index].active = false;
     		motor.stop(true);
 
@@ -139,6 +92,8 @@ void DiffBot::update(float dt) {
     		sprintf(log, "SET;WAYPOINT;%d\n", index);
     		CDC_Transmit_FS((uint8_t*)log, strlen(log));
 
+    		resetPID();
+
     		return;
     	}
 
@@ -149,7 +104,7 @@ void DiffBot::update(float dt) {
     	break;
     case INTERMEDIAIRE:
 
-    	if (std::fabs(dx) < 0.1 && std::fabs(dy) < 0.1) {
+    	if (std::fabs(dx) < PRECISE_POS && std::fabs(dy) < PRECISE_POS) {
 
     		char log[32];
     		sprintf(log, "SET;WAYPOINT;%d\n", index);
@@ -163,16 +118,10 @@ void DiffBot::update(float dt) {
     			return;
     		}
 
+    		resetPID();
 
-			// TODO when patch is done refactor this
     		dx = targets[index].x - pose.x;
     		dy = targets[index].y - pose.y;
-    	    distError = sqrtf(dx*dx + dy*dy);
-    	    angleTarget = atan2f(dy, dx);
-    	    angleError  = angleTarget - pose.theta;
-
-    	    while (angleError > M_PI) angleError -= 2*M_PI;
-    	    while (angleError < -M_PI) angleError += 2*M_PI;
 
     	}
 
@@ -181,29 +130,46 @@ void DiffBot::update(float dt) {
     	break;
     }
 
-    // check if final x and y are close but not theta so only turn
+	float dist = sqrtf(dx*dx + dy*dy);
+
+	float angleTarget = atan2f(dy, dx);
+	float angleError = angleTarget - pose.theta;
+
+	while (angleError > M_PI) angleError -= 2*M_PI;
+	while (angleError < -M_PI) angleError += 2*M_PI;
+
+	float direction = 1.0f;
+	if (std::fabs(angleError) > M_PI/2) {
+		direction = -1.0f;
+		angleError > 0 ? angleError -= M_PI : angleError += M_PI;
+	}
+
+	if (std::fabs(angleError) <= PRECISE_POS_FINAL) angleError = 0;
+
+	float distError = dist * cosf(angleError);
+
+	distError *= direction;
 
     float vRef = pidPos.compute(0.0, -distError, dt);
-    // float wRef = pidTheta.compute(targets[index].theta, pose.theta) /*+ 2.0f * angleError*/;
-    float wRef;
+	float wRef;
 
-    if (targets[index].state == FINAL && std::fabs(dx) <= 0.01 && std::fabs(dy) <= 0.01) {
+    if (targets[index].state == FINAL && std::fabs(dx) <= PRECISE_POS_FINAL && std::fabs(dy) <= PRECISE_POS_FINAL) {
         wRef = pidTheta.compute(targets[index].theta, pose.theta, dt);
         vRef = 0;
     }
     else {
-        wRef = pidTheta.compute(0.0, angleError, dt);
+        wRef = pidTheta.compute(0.0, -angleError, dt);
     }
 
-    float vLeft  = vRef - (WHEEL_BASE_2) * wRef;
+    float vLeft = vRef - (WHEEL_BASE_2) * wRef;
     float vRight = vRef + (WHEEL_BASE_2) * wRef;
 
     float v_max = 0.643f; // m/s
 
-    float pwm_ff_left  = (vLeft / v_max) * PWM_MAX;
+    float pwm_ff_left = (vLeft / v_max) * PWM_MAX;
     float pwm_ff_right = (vRight / v_max) * PWM_MAX;
 
-    float pwm_corr_left  = pidLeft.compute(vLeft, leftVel, dt);
+    float pwm_corr_left = pidLeft.compute(vLeft, leftVel, dt);
     float pwm_corr_right = pidRight.compute(vRight, rightVel, dt);
 
     float pwmLeft = pwm_ff_left + pwm_corr_left;
@@ -216,17 +182,17 @@ void DiffBot::update(float dt) {
         pwmRight = (pwmRight > 0) ? pwm_deadzone : -pwm_deadzone;
 
     // saturation
-    pwmLeft  = MAX(-PWM_MAX, MIN(PWM_MAX, pwmLeft));
-    pwmRight = MAX(-PWM_MAX, MIN(PWM_MAX, pwmRight));
+    pwmLeft  = std::max(-PWM_MAX, std::min(pwmLeft,  PWM_MAX));
+    pwmRight = std::max(-PWM_MAX, std::min(pwmRight, PWM_MAX));
 
-    motor.leftTarget_PWM  = static_cast<int16_t>(pwmLeft);
+    motor.leftTarget_PWM = static_cast<int16_t>(pwmLeft);
     motor.rightTarget_PWM = static_cast<int16_t>(pwmRight);
     motor.update();
 }
 
 void DiffBot::addTarget(int id, int type, float x, float y, float theta) {
 
-	if (id >= 10) return;
+	if (id >= MAX_WAYPOINTS) return;
 
 	targets[id] = Point(id, static_cast<StatePoint>(type), x, y, theta);
 	targets[id].active = true;
@@ -235,3 +201,10 @@ void DiffBot::addTarget(int id, int type, float x, float y, float theta) {
 
     arrive = false;
 }
+
+void DiffBot::resetPID() {
+	pidLeft.reset();
+	pidRight.reset();
+	pidPos.reset();
+	pidTheta.reset();
+}
diff --git a/Core/Src/motors.cpp b/Core/Src/motors.cpp
index f5c84e1..444b211 100644
--- a/Core/Src/motors.cpp
+++ b/Core/Src/motors.cpp
@@ -1,56 +1,44 @@
+#include <algorithm>
 #include <motors.h>
-#include <cstring>
-#include <cstdio>
-#include "usbd_cdc_if.h"
+#include <modelec.h>
+
+float approach(float current, float target, float step) {
+	if (current < target)
+		return std::min(current + step, target);
+
+	if (current > target)
+		return std::max(current - step, target);
+
+	return current;
+}
 
 void Motor::update() {
 
-	int16_t PWM_MAX_M = 626;
+    int16_t max_step = 50;
 
-    uint8_t max_step = 25;
+    leftTarget_PWM  = std::max(-PWM_MAX, std::min(leftTarget_PWM,  PWM_MAX));
+    rightTarget_PWM = std::max(-PWM_MAX, std::min(rightTarget_PWM, PWM_MAX));
 
-	if (leftTarget_PWM > PWM_MAX_M) leftTarget_PWM = PWM_MAX_M;
-	if (leftTarget_PWM < -PWM_MAX_M) leftTarget_PWM = -PWM_MAX_M;
-	if (rightTarget_PWM > PWM_MAX_M) rightTarget_PWM = PWM_MAX_M;
-	if (rightTarget_PWM < -PWM_MAX_M) rightTarget_PWM = -PWM_MAX_M;
+	leftCurrent_PWM  = approach(leftCurrent_PWM,  leftTarget_PWM,  max_step);
+	rightCurrent_PWM = approach(rightCurrent_PWM, rightTarget_PWM, max_step);
 
-    if (leftCurrent_PWM < leftTarget_PWM) {
-        leftCurrent_PWM += max_step;
-        if (leftCurrent_PWM > leftTarget_PWM)
-            leftCurrent_PWM = leftTarget_PWM;
-    } else if (leftCurrent_PWM > leftTarget_PWM) {
-        leftCurrent_PWM -= max_step;
-        if (leftCurrent_PWM < leftTarget_PWM)
-            leftCurrent_PWM = leftTarget_PWM;
-    }
-
-    if (rightCurrent_PWM < rightTarget_PWM) {
-        rightCurrent_PWM += max_step;
-        if (rightCurrent_PWM > rightTarget_PWM)
-            rightCurrent_PWM = rightTarget_PWM;
-    } else if (rightCurrent_PWM > rightTarget_PWM) {
-        rightCurrent_PWM -= max_step;
-        if (rightCurrent_PWM < rightTarget_PWM)
-            rightCurrent_PWM = rightTarget_PWM;
-    }
-
-	// moteur gauche -> TIM1 CH1/CH2
+	// left motor -> TIM1 CH1/CH2
 	if (leftCurrent_PWM >= 0) {
-		TIM8->CCR1 = static_cast<uint16_t>(leftCurrent_PWM);
-		TIM8->CCR2 = 0;
-	} else {
-		TIM8->CCR2 = static_cast<uint16_t>(-leftCurrent_PWM);
-		TIM8->CCR1 = 0;
-	}
-
-	// moteur droit -> TIM8 CH1/CH2
-	if (rightCurrent_PWM >= 0) {
-		TIM1->CCR1 = static_cast<uint16_t>(rightCurrent_PWM);
+		TIM1->CCR1 = static_cast<uint16_t>(leftCurrent_PWM);
 		TIM1->CCR2 = 0;
 	} else {
-		TIM1->CCR2 = static_cast<uint16_t>(-rightCurrent_PWM);
+		TIM1->CCR2 = static_cast<uint16_t>(-leftCurrent_PWM);
 		TIM1->CCR1 = 0;
 	}
+
+	// right motor -> TIM8 CH1/CH2
+	if (rightCurrent_PWM >= 0) {
+		TIM8->CCR1 = static_cast<uint16_t>(rightCurrent_PWM);
+		TIM8->CCR2 = 0;
+	} else {
+		TIM8->CCR2 = static_cast<uint16_t>(-rightCurrent_PWM);
+		TIM8->CCR1 = 0;
+	}
 }
 
 void Motor::stop(bool stop) {
diff --git a/Core/Src/pid.cpp b/Core/Src/pid.cpp
index f2d8f72..d870763 100644
--- a/Core/Src/pid.cpp
+++ b/Core/Src/pid.cpp
@@ -1,9 +1,11 @@
 
 #include "pid.h"
 
+#include <algorithm>
 
 PID::PID(float kp, float ki, float kd, float outMin, float outMax)
-	: kp(kp), ki(ki), kd(kd), integral(0.0), outMin(outMin), outMax(outMax) {
+	: kp(kp), ki(ki), kd(kd), outMin(outMin), outMax(outMax) {
+	reset();
 }
 
 float PID::compute(float setpoint, float measurement, float dt) {
@@ -12,8 +14,13 @@ float PID::compute(float setpoint, float measurement, float dt) {
 	float derivative = (error - prevError) / dt;
 	float output = kp * error + ki * integral + kd * derivative;
 
-	output = std::min(std::max(output, outMin), outMax);
+	output = std::max(outMin, std::min(output, outMax));
 
 	prevError = error;
 	return output;
 }
+
+void PID::reset() {
+	integral = 0.0f;
+	prevError = 0.0f;
+}
diff --git a/Core/Src/point.cpp b/Core/Src/point.cpp
index 43ddf1f..0018307 100644
--- a/Core/Src/point.cpp
+++ b/Core/Src/point.cpp
@@ -3,5 +3,4 @@
 #include "point.h"
 
 Point::Point(uint8_t id, StatePoint state, float x, float y, float theta, bool active) : id(id), state(state), x(x), y(y), theta(theta), active(active) {
-
 }
diff --git a/Core/Src/setup.cpp b/Core/Src/setup.cpp
index bea84f3..1fce036 100644
--- a/Core/Src/setup.cpp
+++ b/Core/Src/setup.cpp
@@ -7,6 +7,7 @@
 
 #include <setup.h>
 #include <modelec.h>
+#include "commSTM.h"
 
 DiffBot bot(Point(), 0.01f);