From 7ef7228b31f6639c106e30d41317ac25234da51f Mon Sep 17 00:00:00 2001
From: lulko <keklolov7@gmail.com>
Date: Thu, 17 Apr 2025 16:55:48 +0300
Subject: [PATCH] add pid

---
 controller/fw/embed/src/main.cpp | 30 ++++++++++++++++++++++++++++--
 1 file changed, 28 insertions(+), 2 deletions(-)

diff --git a/controller/fw/embed/src/main.cpp b/controller/fw/embed/src/main.cpp
index 4b0d974..8dd8e66 100644
--- a/controller/fw/embed/src/main.cpp
+++ b/controller/fw/embed/src/main.cpp
@@ -216,7 +216,7 @@ void send_motor_torque() {
 }
 
 
-void send_pid(uint8_t param_pid, float data){
+void send_pid(uint8_t param_pid){
   flash_rec = load_params();
   if (flash_rec == nullptr) { // Проверка на NULL
     return;
@@ -238,7 +238,7 @@ void send_pid(uint8_t param_pid, float data){
   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));
+  send_can_with_id_crc(id,param_pid,&data_send,sizeof(data_send));
 }
 
 void setup_id(uint8_t my_id) {
@@ -340,6 +340,18 @@ void listen_can(const CAN_message_t &msg) {
         setup_angle(motor_control_inputs.target_angle);
         break;
 
+      case REG_MOTOR_POSPID_Kp:
+        setup_pid_angle(pid_p,msg.buf[1]);
+        break;
+
+      case REG_MOTOR_POSPID_Ki:
+        setup_pid_angle(pid_i,msg.buf[1]);
+        break;
+
+      case REG_MOTOR_POSPID_Kd:
+        setup_pid_angle(pid_d,msg.buf[1]);
+        break;
+
       case MOTOR_ENABLED:
         if (msg.buf[1] == 1) {
           motor.enable();
@@ -378,6 +390,20 @@ void listen_can(const CAN_message_t &msg) {
       send_foc_state();
       break;
     
+    case REG_MOTOR_POSPID_Kp:
+      send_pid(pid_p);
+      break;
+
+    case REG_MOTOR_POSPID_Ki:
+      send_pid(pid_i);
+      break;
+
+    case REG_MOTOR_POSPID_Kd:
+      send_pid(pid_d);
+      break;
+
+
+
     default:
       break;
     }