Merge pull request #4 from pepealessio/kd_fix

kd pid fix
pepealessio · Jul 4, 2021 · abb35fb · abb35fb
2 parents 4b0455f + 8f61530
commit abb35fb
Show file tree

Hide file tree

Showing 4 changed files with 17 additions and 16 deletions.
diff --git a/Desktop Controller/gui_robot.py b/Desktop Controller/gui_robot.py
@@ -449,16 +449,16 @@ def on_leave(btn):
     ws.rowconfigure(x, weight=1)
 
 # Socket connection
-target_name = "SBLGruppo2"
-targetBluetoothMacAddress = None
-devices = bluetooth.discover_devices()
-for bdaddr in devices:
-    if target_name == bluetooth.lookup_name(bdaddr):
-        targetBluetoothMacAddress = bdaddr
-        # print(targetBluetoothMacAddress)
-        break
-
-# targetBluetoothMacAddress = '24:6F:28:B4:F0:6E'
+# target_name = "SBLGruppo2"
+# targetBluetoothMacAddress = None
+# devices = bluetooth.discover_devices()
+# for bdaddr in devices:
+#     if target_name == bluetooth.lookup_name(bdaddr):
+#         targetBluetoothMacAddress = bdaddr
+#         # print(targetBluetoothMacAddress)
+#         break
+
+targetBluetoothMacAddress = '24:6F:28:B4:F0:6E'
 if targetBluetoothMacAddress is None:
     messagebox.showerror(title='SBR not found', message='Turn on the SBR and restart the application')
     exit(2)

diff --git a/STM32_code/mcu6050_angle/Core/Inc/supervisor.h b/STM32_code/mcu6050_angle/Core/Inc/supervisor.h
@@ -12,8 +12,9 @@
 
 /********************* DEFINE ******************************/
 
-#define FSM__MAX_VELOCITY 		(12100)
-#define FSM__ANGLE_OFFSET   	(-125) // W is +
+
+#define FSM__MAX_VELOCITY 		(10100)
+#define FSM__ANGLE_OFFSET   	(-130) // W is +
 #define FSM__ANGLE_VARIATION	(-105) // W is +
 
 // The control byte is like the following:

diff --git a/STM32_code/mcu6050_angle/Core/Src/main.c b/STM32_code/mcu6050_angle/Core/Src/main.c
@@ -47,9 +47,9 @@
 /* USER CODE BEGIN PD */
 
 // Equilibrium PID parameters
-#define KP_ANGLE 		(120)
-#define KI_ANGLE		(80)
-#define KD_ANGLE 		(90)
+#define KP_ANGLE 		(123)
+#define KI_ANGLE		(78)
+#define KD_ANGLE 		(50)
 
 #define DT 				(15)
 

diff --git a/STM32_code/mcu6050_angle/Core/Src/pid_controller.c b/STM32_code/mcu6050_angle/Core/Src/pid_controller.c
@@ -43,7 +43,7 @@ int64_t pid__evaluate_output(pid_controller_t *hpid, int64_t read_value)
 //	hpid->error_sum = __pid_costraint(hpid->error_sum, 10000);
 
 	// evaluate PID output like $kp*error + \int_{0}^{t}{error dt} + \frac{d error}{dt}$"
-	output = (hpid->kp * error) + (hpid->ki * hpid->error_sum * PID_CONTROLLER__DT / 100) + hpid->kd * ((read_value - hpid->target) / PID_CONTROLLER__DT);
+	output = (hpid->kp * error) + (hpid->ki * hpid->error_sum * PID_CONTROLLER__DT / 100) + hpid->kd * ((read_value - hpid->last_read_value) / PID_CONTROLLER__DT);
 
 	// update last read value used in derivative error
 	hpid->last_read_value = read_value;