draft usage

src/app/src/chassis_task.c

@@ -5,25 +5,32 @@
 #include "remote.h"
 #include "imu_task.h"
 #include "Swerve_Locomotion.h"
+#include "unitree_motor.h"
 
 extern Robot_State_t g_robot_state;
 extern Remote_t g_remote;
 extern IMU_t g_imu;
 
 void Chassis_Task_Init()
 {
-    Swerve_Init();
+    // Swerve_Init();
+    Unitree_Motor_Config_t unitree_motor_config = {
+        .motor_id = 0,
+        .k_pos = 0.0f,
+        .k_vel = 0.0f,
+        .huartx = &huart1,
+    };
 }
 
 void Chassis_Ctrl_Loop()
 {
 
     if (g_robot_state.enabled)
     {
-        Swerve_Drive(g_robot_state.chassis_x_speed, g_robot_state.chassis_y_speed, g_robot_state.chassis_omega);
+
     }
     else
     {
-        Swerve_Disable();
+        //Unitree_Motor_Disable();
     }
 }

src/app/src/robot.c

@@ -47,8 +47,8 @@ void Robot_Init()
     Launch_Task_Init();
     Remote_Init(&huart3);
     CAN_Service_Init();
-    Referee_System_Init(&huart1);
-    Jetson_Orin_Init(&huart6);
+    // Referee_System_Init(&huart1);
+    // Jetson_Orin_Init(&huart6);
     //  Initialize all tasks
     Robot_Tasks_Start();
 }

src/devices/inc/unitree_motor.h

@@ -178,6 +178,16 @@ typedef struct _Unitree_Motor
     uint8_t motor_id;
 } Unitree_Motor_t;
 
+
+typedef struct _Unitree_Motor_Config
+{
+    uint8_t motor_id;
+    float k_pos;
+    float k_vel;
+    UART_HandleTypeDef *huartx;
+} Unitree_Motor_Config_t;
+
+void Unitree_Motor_Init(Unitree_Motor_Config_t *config);
 #pragma pack()
 
 #endif // UNITREE_MOTOR_H

src/devices/src/unitree_motor.c

@@ -36,22 +36,22 @@ int _Unitree_Motor_Transform_Cmd(Unitree_Motor_Cmd_Package_t *cmd);
  *
  * @return Unitree_Motor_t* Motor instance
  */
-Unitree_Motor_t *Unitree_Motor_Init(UART_HandleTypeDef *huartx, uint8_t motor_id, float k_pos, float k_vel)
+Unitree_Motor_t *Unitree_Motor_Init(Unitree_Motor_Config_t *config)
 {
     if (g_unitree_motor_service_initialzed == 0)
     {
-        g_unitree_motor_uart_instance_ptr = UART_Register(huartx, g_unitree_motor_statsx_buffer,
+        g_unitree_motor_uart_instance_ptr = UART_Register(config->huartx, g_unitree_motor_statsx_buffer,
                                                           sizeof(g_unitree_motor_statsx_buffer), Unitree_Motor_Callback);
         uint16_t reload_value = UNITREE_TIMEOUT_MS / DAEMON_PERIOD;
         uint16_t initial_counter = reload_value;
         g_unitree_motor_daemon_ptr = Daemon_Register(reload_value, initial_counter, Unitree_Motor_Timeout_Callback);
     }
     Unitree_Motor_t *motor = (Unitree_Motor_t *)malloc(sizeof(Unitree_Motor_t));
-    motor->motor_id = motor_id;
-    motor->cmd.id = motor_id;
+    motor->motor_id = config->motor_id;
+    motor->cmd.id = config->motor_id;
     motor->cmd.mode = UNITREE_IDLE_MODE;
-    motor->cmd.k_pos = k_pos;
-    motor->cmd.k_vel = k_vel;
+    motor->cmd.k_pos = config->k_pos;
+    motor->cmd.k_vel = config->k_vel;
     motor->uart_instance = g_unitree_motor_uart_instance_ptr;
 
     g_unitree_motors_ptr[g_unitree_motor_num++] = motor;