From e65857ca6fd55178a5d812b02dcd9f2a0e23c4b6 Mon Sep 17 00:00:00 2001
From: lulko <keklolov7@gmail.com>
Date: Thu, 17 Apr 2025 16:49:39 +0300
Subject: [PATCH] Added PID

---
 controller/fw/embed/include/flash.h        |  4 +-
 controller/fw/embed/src/main.cpp           | 60 ++++++++++++++++++++++
 controller/fw/embed/test/python_test_id.py |  3 +-
 3 files changed, 65 insertions(+), 2 deletions(-)

diff --git a/controller/fw/embed/include/flash.h b/controller/fw/embed/include/flash.h
index 70de7e8..f15b6ea 100644
--- a/controller/fw/embed/include/flash.h
+++ b/controller/fw/embed/include/flash.h
@@ -17,7 +17,9 @@ enum {
   foc_id    = 1,
   angl      = 2,
   vel       = 3,
-  kp_id     = 4
+  pid_p     = 4,  
+  pid_i,
+  pid_d
 };
 
 #define FLASH_RECORD_SIZE sizeof(FLASH_RECORD)    //size flash struct
diff --git a/controller/fw/embed/src/main.cpp b/controller/fw/embed/src/main.cpp
index 572cc52..4b0d974 100644
--- a/controller/fw/embed/src/main.cpp
+++ b/controller/fw/embed/src/main.cpp
@@ -215,6 +215,32 @@ void send_motor_torque() {
   Can.write(CAN_TX_msg);
 }
 
+
+void send_pid(uint8_t param_pid, float data){
+  flash_rec = load_params();
+  if (flash_rec == nullptr) { // Проверка на NULL
+    return;
+  }
+  uint8_t id = flash_rec[addr_id].value;
+  uint8_t d = flash_rec[param_pid].value;
+  uint8_t data_send = 0;
+  int l = 0;
+  while(d /= 10)
+    l++;
+  if(l >= 2)
+    data_send = (float)d;
+  
+  else if(l == 1)
+    data_send = (float)(d * 10);
+
+  else
+    data_send = (float)(d * 100);
+  if(param_pid == pid_p)param_pid = REG_MOTOR_POSPID_Kp;
+  else if(param_pid == pid_i)param_pid = REG_MOTOR_POSPID_Ki;
+  else if(param_pid == pid_d)param_pid = REG_MOTOR_POSPID_Kd;
+  send_can_with_id_crc(id,param_pid,&data,sizeof(data));
+}
+
 void setup_id(uint8_t my_id) {
   write_param(addr_id,my_id);
   // send_id();
@@ -227,6 +253,40 @@ void setup_angle(float target_angle) {
   motor.move(target_angle);
 }
 
+void setup_pid_angle(uint8_t param_pid, float data){
+  switch (param_pid)
+  {
+  case pid_p:
+    motor.P_angle.P = data;
+    break;
+
+  case pid_i:
+    motor.P_angle.I = data;
+    break;
+
+  case pid_d:
+    motor.P_angle.D = data;
+    break;
+
+  default:
+    break;
+  }
+  uint8_t check = uint8_t(data);
+  uint8_t data_save = 0;
+  if(check != 0)
+    if(check /= 10)
+      data_save = check;
+
+    else 
+    data_save = (uint8_t)(data * 10);
+
+  else
+    data_save = (uint8_t)(data * 100);
+
+  write_param(param_pid,data_save);
+}
+
+
 void listen_can(const CAN_message_t &msg) {
   msg_id = msg.id;
 
diff --git a/controller/fw/embed/test/python_test_id.py b/controller/fw/embed/test/python_test_id.py
index 4788dc1..b7a3084 100644
--- a/controller/fw/embed/test/python_test_id.py
+++ b/controller/fw/embed/test/python_test_id.py
@@ -75,7 +75,7 @@ send_can_message(bus, (OLD_DEVICE_ID << 4) | REG_WRITE, packet_write)
 
 time.sleep(0.5)
 
-# ======= 2. Запрашиваем текущий ID (используем новы й адрес) =======
+# ======= 2. Запрашиваем текущий ID (используем новый адрес) =======
 
 # Теперь для запроса используем **уже новый id**
 NEW_WITH_REG = (NEW_DEVICE_ID << 4) | REG_READ
@@ -99,6 +99,7 @@ if response:
         print("Ответ слишком короткий")
     else:
         received_crc = int.from_bytes(data[-2:], byteorder='little')
+        print("Полученный CRC: ", received_crc)
         # Расчет CRC по всему пакету без CRC
         calc_crc = validate_crc16(data[:-2])
         if received_crc == calc_crc: