RoboDuoduo/r1upper
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

小米移植可用

Browse Source
This commit is contained in:
ws 2025-05-23 21:18:49 +08:00
parent 46cc491963
commit 9688eca871
9 changed files with 175 additions and 102 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
MDK-ARM/.vscode/c_cpp_properties.json vendored
View File

@ -3,26 +3,26 @@
{ {
"name": "R1-shooter", "name": "R1-shooter",
"includePath": [ "includePath": [
"d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\Core\\Inc", "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Core\\Inc",
"d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\Drivers\\STM32F4xx_HAL_Driver\\Inc", "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Drivers\\STM32F4xx_HAL_Driver\\Inc",
"d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\Drivers\\STM32F4xx_HAL_Driver\\Inc\\Legacy", "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Drivers\\STM32F4xx_HAL_Driver\\Inc\\Legacy",
"d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\include", "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\include",
"d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\CMSIS_RTOS_V2", "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\CMSIS_RTOS_V2",
"d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\RVDS\\ARM_CM4F", "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\RVDS\\ARM_CM4F",
"d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\Drivers\\CMSIS\\Device\\ST\\STM32F4xx\\Include", "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Drivers\\CMSIS\\Device\\ST\\STM32F4xx\\Include",
"d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\Drivers\\CMSIS\\Include", "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Drivers\\CMSIS\\Include",
"d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\User\\bsp", "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\User\\bsp",
"d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\User\\module", "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\User\\module",
"d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\User\\task", "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\User\\task",
"d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\User\\lib", "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\User\\lib",
"d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\User\\device", "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\User\\device",
"D:\\keil\\ARM\\ARMCC\\include", "D:\\keil\\ARM\\ARMCC\\include",
"D:\\keil\\ARM\\ARMCC\\include\\rw", "D:\\keil\\ARM\\ARMCC\\include\\rw",
"d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\MDK-ARM", "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\MDK-ARM",
"d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\Core\\Src", "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Core\\Src",
"d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\Drivers\\STM32F4xx_HAL_Driver\\Src", "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Drivers\\STM32F4xx_HAL_Driver\\Src",
"d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source", "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source",
"d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\MemMang" "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\MemMang"
], ],
"defines": [ "defines": [
"USE_HAL_DRIVER", "USE_HAL_DRIVER",

2
MDK-ARM/.vscode/keil-assistant.log vendored
View File

@ -104,3 +104,5 @@
[info] Log at : 2025/5/22|16:11:31|GMT+0800 [info] Log at : 2025/5/22|16:11:31|GMT+0800
>>>>>>> 8fc2f50677ecabc32b37eac7a7df89d916fefef0 >>>>>>> 8fc2f50677ecabc32b37eac7a7df89d916fefef0
[info] Log at : 2025/5/23|21:04:08|GMT+0800

12
MDK-ARM/.vscode/uv4.log vendored
View File

@ -1,8 +1,16 @@
*** Using Compiler 'V5.06 update 7 (build 960)', folder: 'D:\keil\ARM\ARMCC\Bin' *** Using Compiler 'V5.06 update 7 (build 960)', folder: 'D:\keil\ARM\ARMCC\Bin'
Build target 'R1-shooter' Build target 'R1-shooter'
compiling motor.cpp...
compiling ball.cpp...
compiling djiMotor.c...
compiling gimbal.cpp...
compiling encodeCan.cpp...
compiling shoot.cpp...
compiling ballTask.cpp...
compiling shootTask.cpp...
compiling gimbalTask.cpp... compiling gimbalTask.cpp...
linking... linking...
Program Size: Code=28656 RO-data=1824 RW-data=252 ZI-data=23940 Program Size: Code=28808 RO-data=1800 RW-data=264 ZI-data=23712
FromELF: creating hex file... FromELF: creating hex file...
"R1-shooter\R1-shooter.axf" - 0 Error(s), 0 Warning(s). "R1-shooter\R1-shooter.axf" - 0 Error(s), 0 Warning(s).
Build Time Elapsed: 00:00:03 Build Time Elapsed: 00:00:05

2
MDK-ARM/.vscode/uv4.log.lock vendored
View File

@ -1 +1 @@
2025/5/23 20:55:43 2025/5/23 21:17:13

101
User/device/djiMotor.c
View File

@ -78,11 +78,18 @@
} }
//小米
#define ID_xiaomi 1
#define Pmax 1
#define speed_full_scale 200.0f
#define current_full_scale 4.0f
#if DEBUG == 1 #if DEBUG == 1
//电机回传数据结构体 //电机回传数据结构体
motor_measure_t motor_chassis[10]; motor_measure_t motor_chassis[10];
CAN_MotorFeedback_t motor_5065[2]; CAN_MotorFeedback_t motor_5065[3];
#else #else
static motor_measure_t motor_chassis[5]; static motor_measure_t motor_chassis[5];
#endif #endif
@ -93,16 +100,25 @@ static uint8_t can_send_data_2ff[8];
static CAN_TxHeaderTypeDef tx_message_200; static CAN_TxHeaderTypeDef tx_message_200;
static uint8_t can_send_data_200[8]; static uint8_t can_send_data_200[8];
//can1 dji
CAN_RxHeaderTypeDef dji_rx_header; CAN_RxHeaderTypeDef dji_rx_header;
uint8_t dji_rx_data[8]; uint8_t dji_rx_data[8];
CAN_RawTx_t raw_tx;
//can2 vesc xiaomi
CAN_RxHeaderTypeDef rx_header; CAN_RxHeaderTypeDef rx_header;
uint8_t rx_data[8]; uint8_t rx_data[8];
static CAN_TxHeaderTypeDef vesc_tx_message; static CAN_TxHeaderTypeDef vesc_tx_message;
static uint8_t vesc_send_data[4]; static uint8_t vesc_send_data[4];
//小米
uint8_t can_send_data[8];
// 定义 CAN 传输头部信息
static CAN_TxHeaderTypeDef tx_message;
MotorFeedback_t MotorFeedback;
CAN_RawTx_t raw_tx;
/** /**
* @brief 数据处理函数 * @brief 数据处理函数
* @param[in] none * @param[in] none
@ -168,15 +184,28 @@ void can2MotorEncode()
// 存储消息到对应的电机结构体中 // 存储消息到对应的电机结构体中
VescMotor_Decode(&motor_5065[0], &rx_header,rx_data); VescMotor_Decode(&motor_5065[0], &rx_header,rx_data);
break; break;
case CAN_VESC5065_M2_MSG1: case CAN_VESC5065_M2_MSG1:
// 存储消息到对应的电机结构体中 // 存储消息到对应的电机结构体中
VescMotor_Decode(&motor_5065[1], &rx_header,rx_data); VescMotor_Decode(&motor_5065[1], &rx_header,rx_data);
case CAN_VESC5065_M3_MSG1:
// 存储消息到对应的电机结构体中
VescMotor_Decode(&motor_5065[2], &rx_header,rx_data);
break; break;
default: default:
break; break;
} }
switch (rx_data[0])
{
case ID_xiaomi:
Parse_CAN_Response (rx_data,&MotorFeedback);
break ;
default:
break;
}
} }
#if FREERTOS_DJI == 0 #if FREERTOS_DJI == 0
@ -437,3 +466,69 @@ return DEVICE_OK;
{ {
return &motor_5065[i]; return &motor_5065[i];
} }
//小米
void Parse_CAN_Response(uint8_t* rx_data, MotorFeedback_t* feedback) {
// 1. 解析位置
int16_t pos_code = (rx_data[1] << 8) | rx_data[2];
feedback->position_deg = ((int16_t)(pos_code - 0x8000) / 32768.0f) * 360.0f * Pmax;
// 2. 解析速度
int16_t speed_code = ((rx_data[3] << 4) | (rx_data[4] >> 4)) - 0x800;
feedback->speed_rads = (speed_code / 2048.0f) * speed_full_scale;
// 3. 解析电流
int16_t current_code = (((rx_data[4] & 0x0F) << 8) | rx_data[5]) - 0x800;
feedback->current_A = (current_code / 2048.0f) * current_full_scale;
// 记录电机ID
feedback->can_id = rx_data[0];
}
void CAN_XiaoMi(int id,JZ_xiaomi_t *JZ_xiaomi,CAN_HandleTypeDef*hcan)
{
uint32_t send_mail_box;
static int is_open=0; //是否开启电机，根据手册，第一次使用需要ff开启
tx_message.StdId = id;
tx_message.IDE = CAN_ID_STD;
tx_message.RTR = CAN_RTR_DATA;
tx_message.DLC = 0x08;
if(is_open){
uint16_t pos_code = (uint16_t)((JZ_xiaomi->position / (Pmax * 360.0f)) * 32768.0f + 32768.0f);
uint16_t speed_code = (uint16_t)((JZ_xiaomi->speed / 200.0f) * 2048.0f + 2048.0f);
uint16_t current_code = (uint16_t)((JZ_xiaomi->K_C / 4.0f) * 2048.0f + 2048.0f);
can_send_data[0] = (pos_code >> 8) & 0xFF; // 位置高8位
can_send_data[1] = pos_code & 0xFF; // 位置低8位
can_send_data[2] = (speed_code >> 4) & 0xFF; // 速度高8位（取12位中的高8）
can_send_data[3] = ((speed_code & 0x0F) << 4) | ((JZ_xiaomi->K_P >> 8) & 0x0F); // 速度低4位 + Kp高4位
can_send_data[4] = JZ_xiaomi->K_P & 0xFF; // Kp低8位
can_send_data[5] = (JZ_xiaomi->K_D >> 4) & 0xFF; // Kd高8位（取12位中的高8）
can_send_data[6] = ((JZ_xiaomi->K_D & 0x0F) << 4) | ((current_code >> 8) & 0x0F); // Kd低4位 + 电流高4位
can_send_data[7] = current_code & 0xFF; // 电流低8位
}
else
{
can_send_data[0] = 0xFF;
can_send_data[1] = 0xFF;
can_send_data[2] = 0xFF;
can_send_data[3] = 0xFF;
can_send_data[4] = 0xFF;
can_send_data[5] = 0xFF;
can_send_data[6] = 0xFF;
can_send_data[7] = 0xFC;
is_open=1;
}
HAL_CAN_AddTxMessage(hcan, &tx_message, can_send_data, &send_mail_box);
}

35
User/device/djiMotor.h
View File

@ -10,7 +10,7 @@ extern "C"{
#include "device.h" #include "device.h"
#include "can.h" #include "can.h"
#include <math.h> #include <math.h>
#include "string.h"
typedef enum{ typedef enum{
GM6020 = 0, GM6020 = 0,
@ -44,6 +44,7 @@ enum{
GM6020_2 = 0x208, GM6020_2 = 0x208,
CAN_VESC5065_M1_MSG1 =0x901, //vesc的数据回传使用了扩展id，[0:7]为驱动器id，[8:15]为帧类型 CAN_VESC5065_M1_MSG1 =0x901, //vesc的数据回传使用了扩展id，[0:7]为驱动器id，[8:15]为帧类型
CAN_VESC5065_M2_MSG1 =0x902, CAN_VESC5065_M2_MSG1 =0x902,
CAN_VESC5065_M3_MSG1 =0x903,
}; };
//vesc指令 //vesc指令
@ -170,6 +171,38 @@ void CAN_VESC_HEAD(uint8_t controller_id);
int8_t CAN_VESC_Control(int id,int speed ,CAN_HandleTypeDef*hcan); int8_t CAN_VESC_Control(int id,int speed ,CAN_HandleTypeDef*hcan);
//小米
typedef struct
{
int16_t position;
int16_t speed;
int16_t K_P;
int16_t K_D;
int16_t K_C;
int16_t Pmax;//决定最大角度值，+-1 -> 最大+-360°
}JZ_xiaomi_t;
typedef struct {
uint8_t can_id;
float position_deg;
float speed_rads;
float current_A;
} MotorFeedback_t;
void Parse_CAN_Response(uint8_t* rx_data, MotorFeedback_t* feedback) ;
void CAN_XiaoMi(int id,JZ_xiaomi_t *JZ_xiaomi,CAN_HandleTypeDef*hcan);
#ifdef __cplusplus #ifdef __cplusplus
} }
#endif #endif

69
User/module/ball.cpp
View File

@ -17,8 +17,6 @@ extern int key;
//添加平移3508 得跑位置吧 //添加平移3508 得跑位置吧
const fp32 Ball:: M3508_speed_PID[3] = {15, 0.03, 0}; const fp32 Ball:: M3508_speed_PID[3] = {15, 0.03, 0};
const fp32 Ball:: Extend_speed_PID[3] = { 25, 0, 0.};
const fp32 Ball:: Extend_angle_PID[3]= { 60, 0, 0.1};
//摩擦轮转速 //摩擦轮转速
int speedm=0; int speedm=0;
@ -29,23 +27,6 @@ Ball ::Ball()
{ {
detect_init(); detect_init();
//两个伸缩6020
Extern_Motor[0] = get_motor_point(6);
Extern_Motor[1] = get_motor_point(7);
Extern_Motor[0]->type = GM6020;
Extern_Motor[1]->type = GM6020;
PID_init(&extend_speed_pid[0],PID_POSITION,Extend_speed_PID,25000, 2000);
PID_init(&extend_angle_pid[0],PID_POSITION,Extend_angle_PID,25000, 1000);
PID_init(&extend_speed_pid[1],PID_POSITION,Extend_speed_PID,25000, 2000);
PID_init(&extend_angle_pid[1],PID_POSITION,Extend_angle_PID,25000, 1000);
e_result[0] = 0;
e_result[1] = 0;
angleSet[0] = 0;
angleSet[1] = 0;
//三摩擦轮 //三摩擦轮
for(int i = 0;i < MOTOR_NUM;i ++) for(int i = 0;i < MOTOR_NUM;i ++)
{ {
@ -59,6 +40,13 @@ Ball ::Ball()
speedSet[i] = 0; speedSet[i] = 0;
} }
//小米电机初始化
JZ_xiaomi.position = 0; //
JZ_xiaomi.speed = 20; //
JZ_xiaomi.K_P = 100; //
JZ_xiaomi.K_D =20; //
JZ_xiaomi.K_C = 2 ;
JZ_xiaomi.Pmax =1;
//状态机状态初始化 //状态机状态初始化
currentState1= BALL_IDLE; currentState1= BALL_IDLE;
@ -66,46 +54,6 @@ Ball ::Ball()
} }
void Ball ::Extend_control(int angle)
{
int16_t delta[2];
angleSet[0] = angle;
angleSet[1] = -angle;
delta[0] = PID_calc(&extend_angle_pid[0],Extern_Motor[0]->total_angle , angleSet[0]);
e_result[0] = PID_calc(&extend_speed_pid[0], Extern_Motor[0]->speed_rpm, delta[0]);
delta[1] = PID_calc(&extend_angle_pid[1], Extern_Motor[1]->total_angle , angleSet[1]);
e_result[1] = PID_calc(&extend_speed_pid[1], Extern_Motor[1]->speed_rpm, delta[1]);
// delta[1] = PID_calc(&extend_angle_pid, angle_get[1], angleSet[1]);
// e_result[1] = PID_calc(&extend_speed_pid, Extern_Motor[1]->speed_rpm, delta[1]);
// angle_get[0]=motor_angle_change(Extern_Motor[0]->real_round, angleSet[0]);
// angle_get[1]=motor_angle_change(Extern_Motor[1]->real_round, angleSet[1]);
// delta[0] = PID_calc(&extend_angle_pid, angle_get[0], angleSet[0]);
// e_result[0] = PID_calc(&extend_speed_pid, Extern_Motor[0]->speed_rpm, delta[0]);
// delta[1] = PID_calc(&extend_angle_pid, angle_get[1], angleSet[1]);
// e_result[1] = PID_calc(&extend_speed_pid, Extern_Motor[1]->speed_rpm, delta[1]);
}
// void Ball ::Extend_control(int angle)
// {
// int16_t delta;
// angleSet = angle;
// delta = PID_calc(&extend_angle_pid, Extern_Motor->total_angle, angleSet);
// e_result = PID_calc(&extend_speed_pid, Extern_Motor->speed_rpm, delta);
// }
void Ball ::Spin(float speed) void Ball ::Spin(float speed)
{ {
@ -228,9 +176,6 @@ void Ball::Send_control()
CAN_cmd_200(result[MOTOR_1],result[MOTOR_2],result[MOTOR_3],0,&hcan1); CAN_cmd_200(result[MOTOR_1],result[MOTOR_2],result[MOTOR_3],0,&hcan1);
osDelay(1); osDelay(1);
CAN_cmd_1FF(0,0,e_result[0],e_result[1],&hcan1);
osDelay(2);
} }
#else #else

13
User/module/ball.hpp
View File

@ -54,15 +54,13 @@ public:
void Send_control(void); void Send_control(void);
void ballStop(void); void ballStop(void);
void ballTake(void); void ballTake(void);
void Extend_control(int angle);
BallState_t currentState1; // 当前状态 BallState_t currentState1; // 当前状态
int flag;//暂时还没用到 int flag;//暂时还没用到
int ball_state ;//光电检测 int ball_state ;//光电检测
//伸缩6020 //小米电机伸缩
int16_t e_result[2]; JZ_xiaomi_t JZ_xiaomi;
motor_measure_t * Extern_Motor[2];
int16_t result[MOTOR_NUM]; int16_t result[MOTOR_NUM];
motor_measure_t *hand_Motor[MOTOR_NUM]; motor_measure_t *hand_Motor[MOTOR_NUM];
@ -72,24 +70,17 @@ public:
volatile uint32_t flag_thread;//接收传回的线程通知 volatile uint32_t flag_thread;//接收传回的线程通知
private: private:
//滤波一下
//三个摩擦轮 //三个摩擦轮
static const float M3508_speed_PID[3]; static const float M3508_speed_PID[3];
static const float M3508_angle_PID[3]; static const float M3508_angle_PID[3];
static const float Extend_speed_PID[3];
static const float Extend_angle_PID[3];
//电机速度pid结构体 //电机速度pid结构体
pid_type_def speed_pid[MOTOR_NUM]; pid_type_def speed_pid[MOTOR_NUM];
//位置环pid //位置环pid
pid_type_def angle_pid[MOTOR_NUM]; pid_type_def angle_pid[MOTOR_NUM];
pid_type_def extend_speed_pid[2];
pid_type_def extend_angle_pid[2];
float angleSet[2]; float angleSet[2];
float speedSet[MOTOR_NUM]; float speedSet[MOTOR_NUM];
}; };

3
User/task/ballTask.cpp
View File

@ -79,9 +79,8 @@ void FunctionBall(void *argument)
} }
CAN_XiaoMi(1,&ball.JZ_xiaomi,&hcan2);
ball.Extend_control(angle1);
ball.Spin(speedm); ball.Spin(speedm);
ball.Send_control(); ball.Send_control();