From 60fdeb9b438f6537aaa3c3ee480a8733d3a79b69 Mon Sep 17 00:00:00 2001
From: Leo Liu <65527170+CuboiLeo@users.noreply.github.com>
Date: Wed, 29 May 2024 23:36:26 -0400
Subject: [PATCH] barrel heat regulation added, still need some final
 adjustments

---
 src/app/inc/launch_task.h |  2 +-
 src/app/src/debug_task.c  |  1 +
 src/app/src/launch_task.c | 14 +++++++-------
 src/app/src/robot.c       |  1 +
 4 files changed, 10 insertions(+), 8 deletions(-)

diff --git a/src/app/inc/launch_task.h b/src/app/inc/launch_task.h
index c8c4e44..c054aab 100644
--- a/src/app/inc/launch_task.h
+++ b/src/app/inc/launch_task.h
@@ -7,7 +7,7 @@
 #define FLYWHEEL_VELOCITY_23 (6000.0f * M3508_REDUCTION_RATIO)
 #define FLYWHEEL_VELOCITY_30 (7000.0f * M3508_REDUCTION_RATIO)
 #define FEED_HOLE_NUM (6.0f)
-#define LAUNCH_FREQUENCY (10)
+#define LAUNCH_FREQUENCY (15)
 #define LAUNCH_PERIOD (1000.0f/LAUNCH_FREQUENCY)
 #define FEED_1_PROJECTILE_ANGLE (2.0f*PI/FEED_HOLE_NUM)
 #define FEED_FREQUENCY_6 (6.0f / FEED_HOLE_NUM * 60.0f)
diff --git a/src/app/src/debug_task.c b/src/app/src/debug_task.c
index df2610a..ff39cbb 100644
--- a/src/app/src/debug_task.c
+++ b/src/app/src/debug_task.c
@@ -54,5 +54,6 @@ void Debug_Task_Loop(void)
         counter = 0;
     }
     DEBUG_PRINTF(&huart6, ">calc:%d\n>ref:%d\n",g_launch_target.calculated_heat,Referee_Robot_State.Shooter_Heat_1);
+    DEBUG_PRINTF(&huart6, ">feed_angle:%f\n>total_angle:%f\n",g_launch_target.feed_angle,g_motor_feed->stats->total_angle_rad);
 #endif
 }
\ No newline at end of file
diff --git a/src/app/src/launch_task.c b/src/app/src/launch_task.c
index 40e0014..cbd46a6 100644
--- a/src/app/src/launch_task.c
+++ b/src/app/src/launch_task.c
@@ -93,12 +93,12 @@ void Feed_Angle_Calc()
     #ifdef HEAT_CONTROL
         if (Referee_System.Online_Flag)
         {
+            // if (g_launch_target.heat_count*2 % 100 == 0)
+            // {
+            //     g_launch_target.calculated_heat -= Referee_Robot_State.Cooling_Rate/10;
+            //     __MAX_LIMIT(g_launch_target.calculated_heat,0,Referee_Robot_State.Heat_Max);
+            // }
             if (g_launch_target.heat_count*2 % 100 == 0)
-            {
-                g_launch_target.calculated_heat -= Referee_Robot_State.Cooling_Rate/10;
-                __MAX_LIMIT(g_launch_target.calculated_heat,0,Referee_Robot_State.Heat_Max);
-            }
-            if (g_launch_target.heat_count*2 % 1000 == 0)
             {
                 g_launch_target.calculated_heat = Referee_Robot_State.Shooter_Heat_1;
             }
@@ -107,10 +107,10 @@ void Feed_Angle_Calc()
                 if (g_launch_target.launch_freq_count*2 > LAUNCH_PERIOD)
                 {
                     g_launch_target.launch_freq_count = 0;
-                    if((Referee_Robot_State.Heat_Max-g_launch_target.calculated_heat) > 10)
+                    if((Referee_Robot_State.Heat_Max-g_launch_target.calculated_heat) > 20)
                     {
                         g_launch_target.calculated_heat += 10;
-                        g_launch_target.feed_angle = DJI_Motor_Get_Total_Angle(g_motor_feed) + FEED_1_PROJECTILE_ANGLE;
+                        g_launch_target.feed_angle += FEED_1_PROJECTILE_ANGLE;
                         DJI_Motor_Set_Control_Mode(g_motor_feed, POSITION_CONTROL_TOTAL_ANGLE);
                         DJI_Motor_Set_Angle(g_motor_feed,g_launch_target.feed_angle);
                     }
diff --git a/src/app/src/robot.c b/src/app/src/robot.c
index 60b6351..99d3165 100644
--- a/src/app/src/robot.c
+++ b/src/app/src/robot.c
@@ -155,6 +155,7 @@ void Robot_Cmd_Loop()
                     g_launch_target.single_launch_finished_flag = 0;
                     g_launch_target.burst_launch_flag = 0;
                     g_launch_target.launch_freq_count = 0;
+                    g_launch_target.feed_angle = DJI_Motor_Get_Total_Angle(g_motor_feed);
                 }
             }
             else