RMUL version

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 \
@@ -154,6 +155,7 @@ src/ui/src/ui_indicator_0_7.c \
 src/ui/src/ui_indicator_0_8.c \
 src/ui/src/ui_indicator_0_9.c \
 src/ui/src/ui_indicator_0_10.c \
+src/ui/src/ui_indicator_0_11.c \
 src/ui/src/ui_indicator_1_0.c 
 
 # ASM sources

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/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                                                                      \
     {                                                                       \

src/algo/src/Swerve_Locomotion.c

@@ -6,8 +6,8 @@
 
 Swerve_Module_t g_swerve_fl, g_swerve_rl, g_swerve_rr, g_swerve_fr;
 Swerve_Module_t *swerve_modules[NUMBER_OF_MODULES] = {&g_swerve_fl, &g_swerve_rl, &g_swerve_rr, &g_swerve_fr};
+Module_State_t optimized_module_state;
 float last_swerve_angle[NUMBER_OF_MODULES] = {0.0f, 0.0f, 0.0f, 0.0f};
-Kalman_Filter_t power_KF = {.Prev_P = 1.0f, .Q = 0.0001, .R = 5.0f};
 
 /**
  * @brief Inverse kinematics matrix for a 4 module swerve, defined counterclockwise from the front left
@@ -42,14 +42,14 @@ void Set_Module_Output(Swerve_Module_t *swerve_module, Module_State_t desired_st
 void Swerve_Init()
 {
     // define constants for each module in an array [0] == fl, [1] == rl, [2] == rr, [3] == fr
-    int azimuth_can_bus_array[NUMBER_OF_MODULES] = {1, 1, 2, 2};
+    int azimuth_can_bus_array[NUMBER_OF_MODULES] = {2, 2, 2, 2};
     int azimuth_speed_controller_id_array[NUMBER_OF_MODULES] = {1, 2, 3, 4};
-    int azimuth_offset_array[NUMBER_OF_MODULES] = {1990, 730, 6060, 1362};
+    int azimuth_offset_array[NUMBER_OF_MODULES] = {2050, 1940, 1430, 8150};
     Motor_Reversal_t azimuth_motor_reversal_array[NUMBER_OF_MODULES] = {MOTOR_REVERSAL_REVERSED, MOTOR_REVERSAL_REVERSED, MOTOR_REVERSAL_REVERSED, MOTOR_REVERSAL_REVERSED};
 
-    int drive_can_bus_array[NUMBER_OF_MODULES] = {1, 1, 2, 2};
+    int drive_can_bus_array[NUMBER_OF_MODULES] = {1, 2, 2, 2};
     int drive_speed_controller_id_array[NUMBER_OF_MODULES] = {1, 2, 3, 4};
-    Motor_Reversal_t drive_motor_reversal_array[NUMBER_OF_MODULES] = {MOTOR_REVERSAL_REVERSED, MOTOR_REVERSAL_REVERSED, MOTOR_REVERSAL_NORMAL, MOTOR_REVERSAL_NORMAL};
+    Motor_Reversal_t drive_motor_reversal_array[NUMBER_OF_MODULES] = {MOTOR_REVERSAL_NORMAL, MOTOR_REVERSAL_NORMAL, MOTOR_REVERSAL_REVERSED, MOTOR_REVERSAL_REVERSED};
 
     // init common PID configuration for azimuth motors
     Motor_Config_t azimuth_motor_config = {
@@ -179,45 +179,22 @@ Module_State_t Optimize_Module_Angle(Module_State_t input_state, float measured_
 {
     Module_State_t optimized_module_state = {0};
     optimized_module_state.speed = input_state.speed;
-    float target_angle = fmodf(input_state.angle, 2.0f * PI); // get positive normalized target angle
-    if (target_angle < 0)
-    {
-        target_angle += 2.0f * PI;
-    }
+    optimized_module_state.angle = input_state.angle;
 
-    float curr_angle_normalized = fmodf(measured_angle, 2 * PI); // get normalized current angle
-    if (curr_angle_normalized < 0)
-    {
-        curr_angle_normalized += 2.0f * PI;
-    }
+    if(measured_angle > PI)
+        measured_angle -= 2*PI;
 
-    float delta = target_angle - curr_angle_normalized;          // calculate the angle delta
+    float angle_diff = optimized_module_state.angle - measured_angle;
 
-    // these conditions essentially create the step function for angles to "snap" too offset from the measured angle to avoid flipping
-    if (PI / 2.0f < fabsf(delta) && fabsf(delta) < 3 * PI / 2.0f)
-    {
-        float beta = PI - fabsf(delta);
-        beta *= (delta > 0 ? 1.0f : -1.0f);
-        target_angle = measured_angle - beta;
-        optimized_module_state.speed = -1.0f * input_state.speed; // invert velocity if optimal angle 180 deg from target
-    }
-    else if (fabsf(delta) >= 3.0f * PI / 2.0f)
-    {
-        if (delta < 0)
-        {
-            target_angle = measured_angle + (2.0f * PI + delta);
-        }
-        else
-        {
-            target_angle = measured_angle - (2.0f * PI - delta);
-        }
-    }
-    else
-    {
-        target_angle = measured_angle + delta;
-    }
+    if((angle_diff >= PI/2 && angle_diff <= 3*PI/2) || (angle_diff <= -PI/2 && angle_diff >= -3*PI/2))
+	{
+		optimized_module_state.angle += (angle_diff > 0) ? -PI:PI;
+		optimized_module_state.speed *= -1.0f;
+	}
+
+    if (optimized_module_state.angle < 0)
+        optimized_module_state.angle += 2.0f * PI;
 
-    optimized_module_state.angle = target_angle;
     return optimized_module_state;
 }
 
@@ -261,27 +238,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.9f-Referee_Robot_State.Level*0.2f)))
+                    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

@@ -4,15 +4,19 @@
 #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 (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
+
 typedef struct
 {
     float flywheel_velocity;
@@ -22,6 +26,10 @@ typedef struct
     uint8_t single_launch_finished_flag;
     uint8_t burst_launch_flag;
     uint8_t flywheel_enabled;
+
+    int16_t calculated_heat;
+    uint16_t heat_count;
+    uint16_t launch_freq_count;
 } Launch_Target_t;
 
 void Launch_Task_Init(void);

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;
@@ -34,6 +35,7 @@ typedef struct
   uint8_t prev_B;
   uint8_t prev_G;
   uint8_t prev_V;
+  uint8_t prev_Z;
   uint8_t prev_left_switch;
 } Key_Prev_t;
 

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/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.2f + chassis_rad * SPIN_TOP_OMEGA);
 
             // ramp up to target omega
             float target_omega = SPIN_TOP_OMEGA * spin_coeff;

src/app/src/debug_task.c

@@ -15,43 +15,46 @@
 extern Robot_State_t g_robot_state;
 extern DJI_Motor_Handle_t *g_yaw;
 extern IMU_t g_imu;
-extern Swerve_Module_t g_swerve_fl;
+extern Swerve_Module_t g_swerve_rl, g_swerve_fr;
 extern Remote_t g_remote; 
 extern Launch_Target_t g_launch_target;
-extern uint64_t t;
 extern Daemon_Instance_t *g_daemon_instances[3];
 extern Daemon_Instance_t *g_remote_daemon;
-#define PRINT_RUNTIME_STATS
+extern Daemon_Instance_t *g_referee_daemon_instance_ptr;
+extern float test_tmd;
+//#define PRINT_RUNTIME_STATS
 #ifdef PRINT_RUNTIME_STATS
 char g_debug_buffer[1024*2] = {0};
 #endif
 
 const char* top_border = "\r\n\r\n\r\n/***** System Info *****/\r\n";
 const char* bottom_border = "/***** End of Info *****/\r\n";
 
-#define DEBUG_ENABLED
+//#define DEBUG_ENABLED
 
 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;
-    // }
-    DEBUG_PRINTF(&huart6, ">ref:%f\n>act:%f\n",g_motor_feed->velocity_pid->ref,g_motor_feed->stats->current_vel_rpm);
+    counter++;
+    if (counter > 0xFFFFFFFF) {
+        counter = 0;
+    }
+
+//     DEBUG_PRINTF(&huart1, ">fr_ref_speed:%f\n>fr_actual_speed:%f\n",
+//     g_swerve_fr.drive_motor->velocity_pid->ref,g_swerve_fr.drive_motor->stats->current_vel_rpm);
 #endif
 }

src/app/src/gimbal_task.c

@@ -42,7 +42,7 @@ void Gimbal_Task_Init()
     Motor_Config_t yaw_motor_config = {
         .can_bus = 1,
         .speed_controller_id = 3,
-        .offset = 3690,
+        .offset = 2400,
         .control_mode = POSITION_VELOCITY_SERIES,
         .motor_reversal = MOTOR_REVERSAL_NORMAL,
         .use_external_feedback = 1,
@@ -67,7 +67,7 @@ void Gimbal_Task_Init()
     };
 
     Motor_Config_t pitch_motor_config = {
-        .can_bus = 2,
+        .can_bus = 1,
         .speed_controller_id = 2,
         .offset = 4460,
         .use_external_feedback = 1,