From c5dcbb5a95e3f0d572d2b74f21e38a6209bd4d1b Mon Sep 17 00:00:00 2001
From: Leo Liu <65527170+CuboiLeo@users.noreply.github.com>
Date: Wed, 5 Jun 2024 23:49:56 -0400
Subject: [PATCH] power regulation tuned

---
 src/algo/inc/user_math.h   | 2 +-
 src/app/inc/robot.h        | 3 ++-
 src/app/src/chassis_task.c | 2 +-
 3 files changed, 4 insertions(+), 3 deletions(-)

diff --git a/src/algo/inc/user_math.h b/src/algo/inc/user_math.h
index b01cf45..6ffd307 100644
--- a/src/algo/inc/user_math.h
+++ b/src/algo/inc/user_math.h
@@ -39,7 +39,7 @@
         }                             \
     } while (0);
 
-#define BUFFER_SIZE 500
+#define BUFFER_SIZE (1000)
 #define __MOVING_AVERAGE(buffer, index, update_value, count, sum, average)  \
     do                                                                      \
     {                                                                       \
diff --git a/src/app/inc/robot.h b/src/app/inc/robot.h
index 64329ec..00c6059 100644
--- a/src/app/inc/robot.h
+++ b/src/app/inc/robot.h
@@ -2,6 +2,7 @@
 #define ROBOT_H
 
 #include <stdint.h>
+#include "user_math.h"
 
 typedef struct
 {
@@ -14,7 +15,7 @@ typedef struct
   float chassis_y_speed;
   float chassis_omega;
 
-  float chassis_power_buffer[500];
+  float chassis_power_buffer[BUFFER_SIZE];
   uint16_t chassis_power_index;
   uint16_t chassis_power_count;
   float chassis_avg_power;
diff --git a/src/app/src/chassis_task.c b/src/app/src/chassis_task.c
index b91968a..409cd25 100644
--- a/src/app/src/chassis_task.c
+++ b/src/app/src/chassis_task.c
@@ -33,7 +33,7 @@ void Chassis_Ctrl_Loop()
         if (g_robot_state.spintop_mode)
         {
             float translation_speed = sqrtf(powf(g_robot_state.chassis_x_speed, 2) + powf(g_robot_state.chassis_y_speed, 2));
-            float spin_coeff = chassis_rad * SPIN_TOP_OMEGA / (translation_speed*0.1f + chassis_rad * SPIN_TOP_OMEGA);
+            float spin_coeff = chassis_rad * SPIN_TOP_OMEGA / (translation_speed*0.2f + chassis_rad * SPIN_TOP_OMEGA);
 
             // ramp up to target omega
             float target_omega = SPIN_TOP_OMEGA * spin_coeff;