123

CMakeLists.txt

@@ -62,8 +62,8 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
     User/component/pid.c
     User/component/user_math.c
     User/component/crc16.c
-
-/home/shentou/workspace/SuperCap/god-yuan-hero/User/component/PowerControl.c
+    User/component/PowerControl.c
+# /home/shentou/workspace/SuperCap/god-yuan-hero/User/component/PowerControl.c
     # User/device sources
     User/device/dr16.c
     User/device/AT9S_Pro.c
@@ -72,8 +72,8 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
     User/device/motor_dm.c
     User/device/motor_rm.c
     User/device/vofa.c
-
-/home/shentou/workspace/SuperCap/god-yuan-hero/User/device/supercap.c
+    User/device/supercap.c
+# /home/shentou/workspace/SuperCap/god-yuan-hero/User/device/supercap.c
     # User/module sources
     User/module/chassis.c
     User/module/config.c
@@ -90,7 +90,8 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
     User/task/init.c
     User/task/rc.c
     User/task/user_task.c
-/home/shentou/workspace/SuperCap/god-yuan-hero/User/task/supercap.c
+    User/task/supercap.c
+# /home/shentou/workspace/SuperCap/god-yuan-hero/User/task/supercap.c
 )
 
 # Add include paths

User/module/config.c

@@ -93,7 +93,46 @@ Config_RobotParam_t robot_config = {
       .max_current = 16000.0f 
     },
   },
-    
+    .track_param = {
+        .motor = {
+            {
+                .can = BSP_CAN_1,
+                .id = 0x205,
+                .module = MOTOR_M3508,
+                .reverse = false,
+                .gear = true,
+            },
+            {
+                .can = BSP_CAN_1,
+                .id = 0x206,
+                .module = MOTOR_M3508,
+                .reverse = true,
+                .gear = true,
+            },
+        },
+        .pid = {
+            {
+                .k = 0.001f,
+                .p = 1.0f,
+                .i = 0.0f,
+                .d = 0.0f,
+                .i_limit = 1.0f,
+                .out_limit = 1.0f,
+                .d_cutoff_freq = -1.0f,
+                .range = -1.0f,
+            },
+            {
+                .k = 0.001f,
+                .p = 1.0f,
+                .i = 0.0f,
+                .d = 0.0f,
+                .i_limit = 1.0f,
+                .out_limit = 1.0f,
+                .d_cutoff_freq = -1.0f,
+                .range = -1.0f,
+            },
+        },
+    },
     .gimbal_param = {
         .pid = {
             .yaw_omega = {
@@ -150,7 +189,7 @@ Config_RobotParam_t robot_config = {
             .gyro = 1000.0f,
         },
         .pit_motor ={
-            .can = BSP_CAN_1,
+            .can = BSP_CAN_2,
             .can_id = 0x2,
             .master_id = 0x12,
             .module = MOTOR_DM_J4310,

User/module/config.h

@@ -15,12 +15,14 @@ extern "C" {
 #include "module/shoot.h"
 #include "module/gimbal.h"
 #include "module/chassis.h"
+#include "module/track.h"
 #include "module/cmd.h"
 #include "component/PowerControl.h"
 typedef struct {
     Shoot_Params_t shoot_param;
 	Gimbal_Params_t gimbal_param;
     Chassis_Params_t chassis_param;
+    Track_Params_t track_param;
     CMD_Params_t cmd_param;
 } Config_RobotParam_t;
 

User/module/shoot_Master&Slave.c

@@ -32,7 +32,7 @@ void Task(void *argument) {
 
 /* Includes ----------------------------------------------------------------- */
 #include <string.h>
-#include "shoot.h"
+#include "shoot_Master&Slave.h"
 #include "bsp/mm.h"
 #include "bsp/time.h"
 #include "component/filter.h"

User/module/track.c

@@ -0,0 +1,57 @@
+/*
+ * 英雄履带控制
+ */
+ 
+
+/* Includes ----------------------------------------------------------------- */
+#include <math.h>
+#include <string.h>
+#include "track.h"
+#include "bsp/mm.h"
+#include "bsp/time.h"
+#include "component/filter.h"
+#include "component/user_math.h"
+/* Private typedef ---------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+
+/* Private macro ------------------------------------------------------------ */
+/* Private variables -------------------------------------------------------- */
+
+/* Private function  -------------------------------------------------------- */
+
+/* Exported functions ------------------------------------------------------- */
+uint8_t TRACK_Init(Track_t *t, Track_Params_t *param, float target_freq)
+{
+    if (t == NULL || param == NULL) {
+        return TRACK_ERR_NULL; // 参数错误
+    }
+    t->param = param;
+
+    for (int i=0; i<TRACK_MOTOR_NUM; i++) {
+    PID_Init(&t->pid[i], KPID_MODE_CALC_D, target_freq, &param->pid[i]);
+    }
+    return TRACK_OK;
+}
+
+uint8_t TRACK_Control(Track_t *t, Track_CMD_t *cmd)
+{
+    if (t == NULL || cmd == NULL) {
+        return TRACK_ERR_NULL; // 参数错误
+    }
+    t->timer.now = BSP_TIME_Get_us()/1000000.0f;
+    t->timer.dt = t->timer.now - t->timer.lask_wakeup/1000000.0f;
+    t->timer.lask_wakeup = BSP_TIME_Get_us();
+    
+    for (int i=0; i<TRACK_MOTOR_NUM; i++) {
+      t->output.iout[i] = PID_Calc(&t->pid[i], cmd->vel, t->motor.feedback.rotor_speed, 0.0f, t->timer.dt);
+      MOTOR_RM_SetOutput(&t->param->motor[i], t->output.iout[i]);
+    }
+    MOTOR_RM_Ctrl(&t->param->motor[0]);
+    return TRACK_OK;
+}
+
+
+
+
+
+

User/module/track.h

@@ -0,0 +1,64 @@
+/*
+ * far♂蛇模块
+ */
+
+#pragma once
+
+#include <stdbool.h>
+#include <stddef.h>
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "main.h"
+#include "component/pid.h"
+#include "device/motor_rm.h"
+/* Exported constants ------------------------------------------------------- */
+#define TRACK_MOTOR_NUM 2
+#define TRACK_CMD_RPM_MAX 8000.0f 
+#define TRACK_OK (0)        /* 运行正常 */
+#define TRACK_ERR_NULL (-1) /* 运行时发现NULL指针 */    
+#define TRACK_ERR_ERR (-2)  /* 运行时发现了其他错误 */
+#define TRACK_ERR_MODE (-3) /* 运行时配置了错误的Mode */
+#define TRACK_ERR_MOTOR (-4) /* 运行时配置了不存在的电机类型 */
+#define TRACK_ERR_MALLOC (-5) /* 内存分配失败 */
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+typedef struct {
+  float now;            /* 当前时间，单位秒 */
+  uint64_t lask_wakeup; /* 上次唤醒时间，单位微秒 */
+  float dt;             /* 两次唤醒间隔时间，单位秒 */
+} Track_Timer_t;
+
+typedef struct {
+  MOTOR_RM_Param_t motor[TRACK_MOTOR_NUM];
+  KPID_Params_t pid[TRACK_MOTOR_NUM];
+} Track_Params_t;
+
+typedef struct {
+  bool enable;
+  float vel;
+} Track_CMD_t;
+
+typedef struct {
+  Track_Params_t *param;
+  Track_Timer_t timer;
+
+  KPID_t pid[TRACK_MOTOR_NUM];
+
+  MOTOR_RM_t motor; 
+
+  struct {
+    float iout[TRACK_MOTOR_NUM];
+  }output;
+} Track_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+
+
+#ifdef __cplusplus
+}
+#endif
+
+
+