referee alignment fixed, safe mode when referee package lose connecti…

…on added

src/algo/inc/Swerve_Locomotion.h

@@ -19,7 +19,7 @@
 #define NUMBER_OF_MODULES 4
 
 #define SWERVE_OPTIMIZE
-#define POWER_CONTROL
+#define POWER_REGULATION
 
 typedef struct
 {

src/algo/src/Swerve_Locomotion.c

@@ -261,27 +261,37 @@ void Swerve_Drive(float x, float y, float omega)
     y *= SWERVE_MAX_SPEED;
     omega *= SWERVE_MAX_ANGLUAR_SPEED; // convert to rad/s
     #ifdef POWER_REGULATION
-        if(fabs(x) > 0.1f || fabs(y) > 0.1f || fabs(omega) > 0.1f)
+        if(Referee_System.Online_Flag)
         {
-            __MOVING_AVERAGE(g_robot_state.chassis_power_buffer,g_robot_state.chassis_power_index,
-            Referee_Robot_State.Chassis_Power,g_robot_state.chassis_power_count,g_robot_state.chassis_total_power,g_robot_state.chassis_avg_power);
-            if(g_robot_state.chassis_avg_power < (Referee_Robot_State.Chassis_Power_Max*0.7f))
-                g_robot_state.power_increment_ratio += 0.001f;
+            if(fabs(x) > 0.1f || fabs(y) > 0.1f || fabs(omega) > 0.1f)
+            {
+                __MOVING_AVERAGE(g_robot_state.chassis_power_buffer,g_robot_state.chassis_power_index,
+                Referee_Robot_State.Chassis_Power,g_robot_state.chassis_power_count,g_robot_state.chassis_total_power,g_robot_state.chassis_avg_power);
+                if(g_robot_state.chassis_avg_power < (Referee_Robot_State.Chassis_Power_Max*0.7f))
+                    g_robot_state.power_increment_ratio += 0.001f;
+                else
+                    g_robot_state.power_increment_ratio -= 0.001f;
+            }
             else
-                g_robot_state.power_increment_ratio -= 0.001f;
-        }
+            {
+                memset(g_robot_state.chassis_power_buffer,0,sizeof(g_robot_state.chassis_power_buffer));
+                g_robot_state.chassis_power_index = 0;
+                g_robot_state.chassis_power_count = 0;
+                g_robot_state.chassis_total_power = 0;
+                g_robot_state.power_increment_ratio = 1.0f;
+            }
+            __MAX_LIMIT(g_robot_state.power_increment_ratio, 0.8f, 5.0f);
+            x *= g_robot_state.power_increment_ratio; // convert to m/s
+            y *= g_robot_state.power_increment_ratio;
+            omega *= g_robot_state.power_increment_ratio; // convert to rad/s
+            }
         else
         {
-            memset(g_robot_state.chassis_power_buffer,0,sizeof(g_robot_state.chassis_power_buffer));
-            g_robot_state.chassis_power_index = 0;
-            g_robot_state.chassis_power_count = 0;
-            g_robot_state.chassis_total_power = 0;
-            g_robot_state.power_increment_ratio = 1.0f;
+            g_robot_state.power_increment_ratio = 1 + Referee_Robot_State.Manual_Level*0.1f;
+            x *= g_robot_state.power_increment_ratio; // convert to m/s
+            y *= g_robot_state.power_increment_ratio;
+            omega *= g_robot_state.power_increment_ratio; // convert to rad/s
         }
-        __MAX_LIMIT(g_robot_state.power_increment_ratio, 0.8f, 5.0f);
-        x *= g_robot_state.power_increment_ratio; // convert to m/s
-        y *= g_robot_state.power_increment_ratio;
-        omega *= g_robot_state.power_increment_ratio; // convert to rad/s
     #endif
     Chassis_Speeds_t desired_chassis_speeds = {.x = x, .y = y, .omega = omega};
     Set_Desired_States(Chassis_Speeds_To_Module_States(desired_chassis_speeds));

src/app/inc/launch_task.h

@@ -7,6 +7,8 @@
 #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 (12)
+#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)
 #define FEED_FREQUENCY_12 (12.0f / FEED_HOLE_NUM * 60.0f)
@@ -22,6 +24,9 @@ typedef struct
     uint8_t single_launch_finished_flag;
     uint8_t burst_launch_flag;
     uint8_t flywheel_enabled;
+
+    uint8_t calculated_heat;
+    uint8_t referee_update_count;
 } Launch_Target_t;
 
 void Launch_Task_Init(void);

src/app/src/chassis_task.c

@@ -6,7 +6,7 @@
 #include "imu_task.h"
 #include "Swerve_Locomotion.h"
 
-#define SPINTOP_RAMP_COEF (0.003f)
+#define SPINTOP_RAMP_COEF (0.03f)
 #define SPIN_TOP_OMEGA (1.0f)
 
 extern Robot_State_t g_robot_state;
@@ -23,7 +23,6 @@ void Chassis_Task_Init()
 
 void Chassis_Ctrl_Loop()
 {
-
     if (g_robot_state.enabled)
     {
         /*
@@ -34,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 + chassis_rad * SPIN_TOP_OMEGA);
+            float spin_coeff = chassis_rad * SPIN_TOP_OMEGA / (translation_speed*0.1f + chassis_rad * SPIN_TOP_OMEGA);
 
             // ramp up to target omega
             float target_omega = SPIN_TOP_OMEGA * spin_coeff;

src/app/src/debug_task.c

@@ -22,7 +22,7 @@ extern uint64_t t;
 extern Daemon_Instance_t *g_daemon_instances[3];
 extern Daemon_Instance_t *g_remote_daemon;
 extern Daemon_Instance_t *g_referee_daemon_instance_ptr;
-#define PRINT_RUNTIME_STATS
+//#define PRINT_RUNTIME_STATS
 #ifdef PRINT_RUNTIME_STATS
 char g_debug_buffer[1024*2] = {0};
 #endif
@@ -35,25 +35,24 @@ const char* bottom_border = "/***** End of Info *****/\r\n";
 void Debug_Task_Loop(void)
 {
 #ifdef DEBUG_ENABLED
-    // static uint32_t counter = 0;
-    // #ifdef PRINT_RUNTIME_STATS
-    // if (counter % 100 == 0) // Print every 100 cycles
-    // {
-    //     vTaskGetRunTimeStats(g_debug_buffer);
-    //     DEBUG_PRINTF(&huart6, "%s", top_border);
-    //     DEBUG_PRINTF(&huart6, "%s", g_debug_buffer);
-    //     DEBUG_PRINTF(&huart6, "%s", bottom_border);
-    // }
-    // #endif
-
+    static uint32_t counter = 0;
+    #ifdef PRINT_RUNTIME_STATS
+    if (counter % 100 == 0) // Print every 100 cycles
+    {
+        vTaskGetRunTimeStats(g_debug_buffer);
+        DEBUG_PRINTF(&huart6, "%s", top_border);
+        DEBUG_PRINTF(&huart6, "%s", g_debug_buffer);
+        DEBUG_PRINTF(&huart6, "%s", bottom_border);
+    }
+    #endif
+    DEBUG_PRINTF(&huart6, ">time:%.1f\n>ref:%f\n",(float) counter / 1000.0f * DEBUG_PERIOD,Referee_Robot_State.Chassis_Power);
     // DEBUG_PRINTF(&huart6, ">time:%.1f\n>yaw:%f\n>pitch:%f\n>roll:%f\n", (float) counter / 1000.0f * DEBUG_PERIOD, 
     //             g_imu.deg.yaw, g_imu.deg.pitch, g_imu.deg.roll);
     // DEBUG_PRINTF(&huart6, ">remote_daemon:%d\n", g_remote_daemon->counter);
-    // counter++;
-    // if (counter > 0xFFFFFFFF) {
-    //     counter = 0;
-    // }
+    counter++;
+    if (counter > 0xFFFFFFFF) {
+        counter = 0;
+    }
     //DEBUG_PRINTF(&huart6, ">ref:%f\n>act:%f\n",g_motor_feed->velocity_pid->ref,g_motor_feed->stats->current_vel_rpm);
-    DEBUG_PRINTF(&huart6, ">ref:%d\n",g_referee_daemon_instance_ptr->counter);
 #endif
 }

src/app/src/launch_task.c

@@ -4,6 +4,7 @@
 #include "remote.h"
 #include "imu_task.h"
 #include "user_math.h"
+#include "referee_system.h"
 
 extern Robot_State_t g_robot_state;
 extern Remote_t g_remote;
@@ -12,7 +13,6 @@ DJI_Motor_Handle_t *g_flywheel_left, *g_flywheel_right, *g_motor_feed;
 Launch_Target_t g_launch_target;
 
 void Feed_Angle_Calc(void);
-void _launch_set_flywheel_vel_based_on_level();
 
 void Launch_Task_Init() {
     Motor_Config_t flywheel_left_config = {
@@ -72,7 +72,7 @@ void Launch_Task_Init() {
 void Launch_Ctrl_Loop() {
     if (g_robot_state.enabled) {
         if (g_launch_target.flywheel_enabled) {
-            _launch_set_flywheel_vel_based_on_level();
+            g_launch_target.flywheel_velocity = FLYWHEEL_VELOCITY_30;
             DJI_Motor_Set_Velocity(g_flywheel_left,g_launch_target.flywheel_velocity);
             DJI_Motor_Set_Velocity(g_flywheel_right,g_launch_target.flywheel_velocity);
             Feed_Angle_Calc();
@@ -88,10 +88,6 @@ void Launch_Ctrl_Loop() {
     }
 }
 
-void _launch_set_flywheel_vel_based_on_level() {
-    g_launch_target.flywheel_velocity = FLYWHEEL_VELOCITY_30;
-}
-
 void Feed_Angle_Calc()
 {
     if (g_launch_target.single_launch_flag && !g_launch_target.single_launch_finished_flag) {

src/app/src/robot.c

@@ -13,6 +13,7 @@
 #include "referee_system.h"
 #include "buzzer.h"
 #include "ui.h"
+#include "user_math.h"
 
 extern DJI_Motor_Handle_t *g_yaw;
 
@@ -177,6 +178,7 @@ void Robot_Cmd_Loop()
             if (g_remote.keyboard.Z == 1 && g_key_prev.prev_Z == 0)
             {
                 Referee_Robot_State.Manual_Level++;
+                __MAX_LIMIT(Referee_Robot_State.Manual_Level,1,10);
             }
             g_key_prev.prev_B = g_remote.keyboard.B;
             g_key_prev.prev_G = g_remote.keyboard.G;

src/app/src/ui_task.c

@@ -2,7 +2,7 @@
 
 void UI_Task_Loop(void)
 {
-    ui_self_id = Referee_Robot_State.ID;
+    ui_self_id = 3;//Referee_Robot_State.ID;
     if (!g_robot_state.UI_enabled)
     {
         ui_remove_indicator_0();

src/devices/inc/referee_system.h

@@ -26,7 +26,7 @@
 //#define ROBOT_TYPE_SENTRY
 
 //Standard Default Configuration
-#define DEFAULT_STANDARD_POWER_MAX 200
+#define DEFAULT_STANDARD_POWER_MAX 45
 #define DEFAULT_STANDARD_LAUNCH_SPEED_MAX 30
 #define DEFAULT_STANDARD_HEAT_MAX 200
 #define DEFAULT_STANDARD_COOLING_RATE 10
@@ -248,7 +248,7 @@ typedef struct
 		uint32_t State;
 	}RFID;
 
-	uint16_t Info_Update_Frame;
+	uint8_t Info_Update_Frame;
 	uint8_t Online_Flag;
 } Referee_System_t;
 

src/devices/src/referee_system.c

@@ -41,6 +41,7 @@ void Referee_Set_Robot_State(void)
             Referee_Robot_State.Cooling_Rate = DEFAULT_STANDARD_COOLING_RATE+(Referee_Robot_State.Manual_Level-1)*5;
             Referee_Robot_State.Heat_Max = DEFAULT_STANDARD_HEAT_MAX+(Referee_Robot_State.Manual_Level-1)*50;
             Referee_Robot_State.Launch_Speed_Max = DEFAULT_STANDARD_LAUNCH_SPEED_MAX;
+            Referee_Robot_State.Chassis_Power = (DEFAULT_STANDARD_POWER_MAX-10)+(Referee_Robot_State.Manual_Level-1)*5;
             Referee_Robot_State.Chassis_Power_Max = DEFAULT_STANDARD_POWER_MAX+(Referee_Robot_State.Manual_Level-1)*5;
         #endif
 
@@ -70,7 +71,6 @@ void Referee_System_Init(UART_HandleTypeDef *huart)
 {
     Referee_System.huart = huart;
     Referee_Robot_State.Manual_Level = 1;
-    HAL_UART_Receive_DMA(huart, Referee_System.Buffer, REFEREE_BUFFER_LEN);
 
     g_referee_uart_instance_ptr = UART_Register(huart, Referee_System.Buffer, REFEREE_BUFFER_LEN, Referee_Get_Data);
 
@@ -91,6 +91,10 @@ void Referee_Get_Data(UART_Instance_t *uart_instance)
         {
             Daemon_Reload(g_referee_daemon_instance_ptr);
             Referee_System.Online_Flag = 1;
+            Referee_System.Info_Update_Frame++;
+            if(Referee_System.Info_Update_Frame > 100)
+                Referee_System.Info_Update_Frame = 0;
+
             switch (Referee_System.Buffer[n + 5] | Referee_System.Buffer[n + 6] << 8)
             {
             case REFEREE_GAME_STATUS:

src/ui/src/ui_indicator_0_11.c

@@ -7,7 +7,7 @@
 
 #define FRAME_ID 1
 #define GROUP_ID 0
-#define START_ID 11
+#define START_ID 12
 
 ui_string_frame_t ui_indicator_0_11;