final final tuning -leo

Makefile

@@ -135,6 +135,7 @@ src/devices/src/remote.c \
 src/devices/src/imu_task.c \
 src/devices/src/referee_system.c \
 src/devices/src/jetson_orin.c \
+src/devices/src/supercap.c \
 src/app/src/motor_task.c \
 src/app/src/chassis_task.c \
 src/app/src/gimbal_task.c \

src/algo/src/Swerve_Locomotion.c

@@ -244,7 +244,7 @@ void Swerve_Drive(float x, float y, float omega)
             {
                 __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))
+                if(g_robot_state.chassis_avg_power < (Referee_Robot_State.Chassis_Power_Max*(0.9f-Referee_Robot_State.Level*0.2f)))
                     g_robot_state.power_increment_ratio += 0.001f;
                 else
                     g_robot_state.power_increment_ratio -= 0.001f;

src/app/inc/launch_task.h

@@ -4,18 +4,18 @@
 #include <stdint.h>
 #include "dji_motor.h"
 
-#define FLYWHEEL_VELOCITY_23 (6000.0f * M3508_REDUCTION_RATIO)
+#define FLYWHEEL_VELOCITY_10 (3000.0f * M3508_REDUCTION_RATIO)
 #define FLYWHEEL_VELOCITY_30 (7000.0f * M3508_REDUCTION_RATIO)
 #define FEED_HOLE_NUM (6.0f)
-#define LAUNCH_FREQUENCY (15)
+#define LAUNCH_FREQUENCY (20)
 #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)
-#define FEED_FREQUENCY_18 (18.0f / FEED_HOLE_NUM * 60.0f)
+#define FEED_FREQUENCY_20 (20.0f / FEED_HOLE_NUM * 60.0f)
 #define FEED_FREQUENCY_30 (30.0f / FEED_HOLE_NUM * 60.0f)
 
-#define HEAT_CONTROL
+//#define HEAT_CONTROL
 
 typedef struct
 {

src/app/inc/robot_tasks.h

@@ -89,7 +89,7 @@ void Robot_Tasks_UI(void const *argument)
 {
     portTickType xLastWakeTime;
     xLastWakeTime = xTaskGetTickCount();
-    const TickType_t TimeIncrement = pdMS_TO_TICKS(100);
+    const TickType_t TimeIncrement = pdMS_TO_TICKS(1);
     while (1)
     {
         UI_Task_Loop();
@@ -116,7 +116,7 @@ void Robot_Tasks_Jetson_Orin(void const *argument)
     const TickType_t TimeIncrement = pdMS_TO_TICKS(JETSON_ORIN_PERIOD);
     while (1)
     {
-        //Jetson_Orin_Send_Data();
+        Jetson_Orin_Send_Data();
         vTaskDelayUntil(&xLastWakeTime, TimeIncrement);
     }
 }

src/app/src/launch_task.c

@@ -93,14 +93,10 @@ 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.Shooter_Heat_1;
+                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.burst_launch_flag) 
             {
@@ -111,31 +107,30 @@ void Feed_Angle_Calc()
                     {
                         g_launch_target.calculated_heat += 10;
                         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);
+
                     }
                 }
+                DJI_Motor_Set_Control_Mode(g_motor_feed, POSITION_CONTROL_TOTAL_ANGLE);
+                        DJI_Motor_Set_Angle(g_motor_feed,g_launch_target.feed_angle);
             }   
         }
-        else
-        {
-            if (g_launch_target.single_launch_flag && !g_launch_target.single_launch_finished_flag) {
-                g_launch_target.feed_angle = DJI_Motor_Get_Total_Angle(g_motor_feed) + FEED_1_PROJECTILE_ANGLE;
-                g_launch_target.single_launch_finished_flag = 1;
-            }
-            else if (g_launch_target.single_launch_flag && g_launch_target.single_launch_finished_flag) {
-                DJI_Motor_Set_Control_Mode(g_motor_feed, POSITION_CONTROL_TOTAL_ANGLE);
-                DJI_Motor_Set_Angle(g_motor_feed,g_launch_target.feed_angle);
-            }
-            else if (g_launch_target.burst_launch_flag) {
-                g_launch_target.feed_velocity = FEED_FREQUENCY_12;
-                DJI_Motor_Set_Control_Mode(g_motor_feed, VELOCITY_CONTROL);
-                DJI_Motor_Set_Velocity(g_motor_feed,g_launch_target.feed_velocity);
-            }
-            else {
-                DJI_Motor_Set_Control_Mode(g_motor_feed, VELOCITY_CONTROL);
-                DJI_Motor_Set_Velocity(g_motor_feed,0);
-            }
+    #else 
+        if (g_launch_target.single_launch_flag && !g_launch_target.single_launch_finished_flag) {
+            g_launch_target.feed_angle = DJI_Motor_Get_Total_Angle(g_motor_feed) + FEED_1_PROJECTILE_ANGLE;
+            g_launch_target.single_launch_finished_flag = 1;
+        }
+        else if (g_launch_target.single_launch_flag && g_launch_target.single_launch_finished_flag) {
+            DJI_Motor_Set_Control_Mode(g_motor_feed, POSITION_CONTROL_TOTAL_ANGLE);
+            DJI_Motor_Set_Angle(g_motor_feed,g_launch_target.feed_angle);
+        }
+        else if (g_launch_target.burst_launch_flag) {
+            g_launch_target.feed_velocity = FEED_FREQUENCY_20;
+            DJI_Motor_Set_Control_Mode(g_motor_feed, VELOCITY_CONTROL);
+            DJI_Motor_Set_Velocity(g_motor_feed,g_launch_target.feed_velocity);
+        }
+        else {
+            DJI_Motor_Set_Control_Mode(g_motor_feed, VELOCITY_CONTROL);
+            DJI_Motor_Set_Velocity(g_motor_feed,0);
         }
     #endif
 

src/app/src/motor_task.c

@@ -2,10 +2,19 @@
 #include "dji_motor.h"
 #include "dm_motor.h"
 #include "mf_motor.h"
+#include "supercap.h"
+
+extern Supercap_t g_supercap;
 
 void Motor_Task_Loop() {
-    DJI_Motor_Send();
-    MF_Motor_Send();
-    DM_Motor_Send();
+    // DJI_Motor_Send();
+    // MF_Motor_Send();
+    // DM_Motor_Send();
+
+    g_supercap.send_counter++;
+    if (g_supercap.send_counter >= 100) {
+        Supercap_Send();
+        g_supercap.send_counter = 0;
+    }
 }
 

src/app/src/robot.c

@@ -15,6 +15,7 @@
 #include "ui.h"
 #include "user_math.h"
 #include "Swerve_Locomotion.h"
+#include "supercap.h"
 
 extern DJI_Motor_Handle_t *g_yaw;
 
@@ -26,6 +27,7 @@ Robot_State_t g_robot_state = {0, 0};
 Key_Prev_t g_key_prev = {0};
 extern Launch_Target_t g_launch_target;
 extern Remote_t g_remote;
+extern Supercap_t g_supercap;
 
 uint8_t g_start_safely = 0;
 
@@ -49,7 +51,8 @@ void Robot_Init()
     Remote_Init(&huart3);
     CAN_Service_Init();
     Referee_System_Init(&huart1);
-    // Jetson_Orin_Init(&huart6);
+    Jetson_Orin_Init(&huart6);
+    Supercap_Init(&g_supercap);
     //   Initialize all tasks
     Robot_Tasks_Start();
 }
@@ -137,7 +140,7 @@ void Robot_Cmd_Loop()
 
             /* Launch control starts here */
             g_launch_target.heat_count++;
-            if (Referee_Robot_State.Shooter_Heat_1 < Referee_Robot_State.Heat_Max)
+            if (1)//Referee_Robot_State.Shooter_Heat_1 < (Referee_Robot_State.Heat_Max-10))
             {
                 if (g_remote.controller.wheel < -50.0f)
                 { // dial wheel forward single fire

src/devices/inc/motor.h

@@ -20,7 +20,7 @@ typedef enum Motor_Reversal_e
 typedef struct
 {
     uint8_t can_bus;
-    uint16_t tx_id; // ignore this for 
+    uint16_t tx_id; // ignore this for DJI motors
     uint16_t rx_id; // use can_id for general devices
     uint8_t speed_controller_id; // use speed_contrller_id for dji motors
     uint16_t offset;

src/devices/inc/supercap.h

@@ -0,0 +1,22 @@
+#ifndef __SUPERCAP_H
+#define __SUPERCAP_H
+
+#include <stdint.h>
+#include "bsp_can.h"
+#include "referee_system.h"
+
+typedef struct
+{
+    uint8_t can_bus;
+    uint16_t tx_id;
+    uint16_t rx_id;
+    uint8_t send_counter;
+
+    uint8_t supercap_percent;
+} Supercap_t;
+
+void Supercap_Init(Supercap_t *g_supercap);
+void Supercap_Decode(CAN_Instance_t *can_instance);
+void Supercap_Send(void);
+
+#endif // __SUPERCAP_H

src/devices/src/supercap.c

@@ -0,0 +1,27 @@
+#include "supercap.h"
+Supercap_t g_supercap;
+CAN_Instance_t *supercap_can_instance;
+
+void Supercap_Init(Supercap_t *g_supercap)
+{
+    // Initialize supercap
+    g_supercap->can_bus = 1;
+    g_supercap->tx_id = 0x166;
+    g_supercap->rx_id = 0x188;
+    supercap_can_instance = CAN_Device_Register(g_supercap->can_bus,g_supercap->tx_id,g_supercap->rx_id,Supercap_Decode);
+}
+
+void Supercap_Decode(CAN_Instance_t *can_instance)
+{
+    // Send supercap data
+    uint8_t *data = can_instance->rx_buffer;
+    g_supercap.supercap_percent = data[0];
+}
+
+void Supercap_Send(void)
+{
+    // Send supercap data
+    uint8_t *data = supercap_can_instance->tx_buffer;
+    data[0] = 45;//Referee_Robot_State.Chassis_Power_Max;
+    CAN_Transmit(supercap_can_instance);
+}