diff --git a/MDK-ARM/.vscode/c_cpp_properties.json b/MDK-ARM/.vscode/c_cpp_properties.json index 2492d5b..120b12d 100644 --- a/MDK-ARM/.vscode/c_cpp_properties.json +++ b/MDK-ARM/.vscode/c_cpp_properties.json @@ -3,26 +3,26 @@ { "name": "R1-shooter", "includePath": [ - "d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\Core\\Inc", - "d:\\Desktop\\r1\\r1_upper - xiaomi\\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 - xiaomi\\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 - xiaomi\\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 - xiaomi\\r1upper-1\\Drivers\\CMSIS\\Include", - "d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\User\\bsp", - "d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\User\\module", - "d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\User\\task", - "d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\User\\lib", - "d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\User\\device", + "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Core\\Inc", + "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Drivers\\STM32F4xx_HAL_Driver\\Inc", + "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Drivers\\STM32F4xx_HAL_Driver\\Inc\\Legacy", + "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\include", + "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\CMSIS_RTOS_V2", + "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\RVDS\\ARM_CM4F", + "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Drivers\\CMSIS\\Device\\ST\\STM32F4xx\\Include", + "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Drivers\\CMSIS\\Include", + "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\User\\bsp", + "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\User\\module", + "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\User\\task", + "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\User\\lib", + "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\User\\device", "D:\\keil\\ARM\\ARMCC\\include", "D:\\keil\\ARM\\ARMCC\\include\\rw", - "d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\MDK-ARM", - "d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\Core\\Src", - "d:\\Desktop\\r1\\r1_upper - xiaomi\\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 - xiaomi\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\MemMang" + "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\MDK-ARM", + "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Core\\Src", + "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Drivers\\STM32F4xx_HAL_Driver\\Src", + "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source", + "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\MemMang" ], "defines": [ "USE_HAL_DRIVER", diff --git a/MDK-ARM/.vscode/keil-assistant.log b/MDK-ARM/.vscode/keil-assistant.log index 5fad5b6..5d1bd7b 100644 --- a/MDK-ARM/.vscode/keil-assistant.log +++ b/MDK-ARM/.vscode/keil-assistant.log @@ -104,3 +104,5 @@ [info] Log at : 2025/5/22|16:11:31|GMT+0800 >>>>>>> 8fc2f50677ecabc32b37eac7a7df89d916fefef0 +[info] Log at : 2025/5/23|21:04:08|GMT+0800 + diff --git a/MDK-ARM/.vscode/uv4.log b/MDK-ARM/.vscode/uv4.log index 6fa1216..fb7ea63 100644 --- a/MDK-ARM/.vscode/uv4.log +++ b/MDK-ARM/.vscode/uv4.log @@ -1,8 +1,16 @@ *** Using Compiler 'V5.06 update 7 (build 960)', folder: 'D:\keil\ARM\ARMCC\Bin' 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... 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... "R1-shooter\R1-shooter.axf" - 0 Error(s), 0 Warning(s). -Build Time Elapsed: 00:00:03 +Build Time Elapsed: 00:00:05 diff --git a/MDK-ARM/.vscode/uv4.log.lock b/MDK-ARM/.vscode/uv4.log.lock index d44ea73..1473684 100644 --- a/MDK-ARM/.vscode/uv4.log.lock +++ b/MDK-ARM/.vscode/uv4.log.lock @@ -1 +1 @@ -2025/5/23 20:55:43 \ No newline at end of file +2025/5/23 21:17:13 \ No newline at end of file diff --git a/User/device/djiMotor.c b/User/device/djiMotor.c index f92fe83..ca9390a 100644 --- a/User/device/djiMotor.c +++ b/User/device/djiMotor.c @@ -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 //电机回传数据结构体 motor_measure_t motor_chassis[10]; -CAN_MotorFeedback_t motor_5065[2]; +CAN_MotorFeedback_t motor_5065[3]; #else static motor_measure_t motor_chassis[5]; #endif @@ -93,16 +100,25 @@ static uint8_t can_send_data_2ff[8]; static CAN_TxHeaderTypeDef tx_message_200; static uint8_t can_send_data_200[8]; +//can1 dji CAN_RxHeaderTypeDef dji_rx_header; uint8_t dji_rx_data[8]; +CAN_RawTx_t raw_tx; + +//can2 vesc xiaomi CAN_RxHeaderTypeDef rx_header; uint8_t rx_data[8]; static CAN_TxHeaderTypeDef vesc_tx_message; 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 数据处理函数 * @param[in] none @@ -168,15 +184,28 @@ void can2MotorEncode() // 存储消息到对应的电机结构体中 VescMotor_Decode(&motor_5065[0], &rx_header,rx_data); break; - case CAN_VESC5065_M2_MSG1: // 存储消息到对应的电机结构体中 VescMotor_Decode(&motor_5065[1], &rx_header,rx_data); + case CAN_VESC5065_M3_MSG1: + // 存储消息到对应的电机结构体中 + VescMotor_Decode(&motor_5065[2], &rx_header,rx_data); break; - + default: break; } + + switch (rx_data[0]) + { + case ID_xiaomi: + Parse_CAN_Response (rx_data,&MotorFeedback); + break ; + + default: + break; + } + } #if FREERTOS_DJI == 0 @@ -437,3 +466,69 @@ return DEVICE_OK; { 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); +} + diff --git a/User/device/djiMotor.h b/User/device/djiMotor.h index 5b91458..f18588f 100644 --- a/User/device/djiMotor.h +++ b/User/device/djiMotor.h @@ -10,7 +10,7 @@ extern "C"{ #include "device.h" #include "can.h" #include - +#include "string.h" typedef enum{ GM6020 = 0, @@ -44,6 +44,7 @@ enum{ GM6020_2 = 0x208, CAN_VESC5065_M1_MSG1 =0x901, //vesc的数据回传使用了扩展id，[0:7]为驱动器id，[8:15]为帧类型 CAN_VESC5065_M2_MSG1 =0x902, + CAN_VESC5065_M3_MSG1 =0x903, }; //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); + +//小米 + +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 } #endif diff --git a/User/module/ball.cpp b/User/module/ball.cpp index f1d4393..35c69c1 100644 --- a/User/module/ball.cpp +++ b/User/module/ball.cpp @@ -17,8 +17,6 @@ extern int key; //添加平移3508 得跑位置吧 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; @@ -29,23 +27,6 @@ Ball ::Ball() { 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 ++) { @@ -59,6 +40,13 @@ Ball ::Ball() 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; @@ -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) { @@ -228,9 +176,6 @@ void Ball::Send_control() CAN_cmd_200(result[MOTOR_1],result[MOTOR_2],result[MOTOR_3],0,&hcan1); osDelay(1); - CAN_cmd_1FF(0,0,e_result[0],e_result[1],&hcan1); - osDelay(2); - } #else diff --git a/User/module/ball.hpp b/User/module/ball.hpp index c7100da..3919c88 100644 --- a/User/module/ball.hpp +++ b/User/module/ball.hpp @@ -54,15 +54,13 @@ public: void Send_control(void); void ballStop(void); void ballTake(void); - void Extend_control(int angle); - + BallState_t currentState1; // 当前状态 int flag;//暂时还没用到 int ball_state ;//光电检测 - //伸缩6020 - int16_t e_result[2]; - motor_measure_t * Extern_Motor[2]; + //小米电机伸缩 + JZ_xiaomi_t JZ_xiaomi; int16_t result[MOTOR_NUM]; motor_measure_t *hand_Motor[MOTOR_NUM]; @@ -72,24 +70,17 @@ public: volatile uint32_t flag_thread;//接收传回的线程通知 private: - //滤波一下 - //三个摩擦轮 static const float M3508_speed_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_type_def speed_pid[MOTOR_NUM]; //位置环pid 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 speedSet[MOTOR_NUM]; }; diff --git a/User/task/ballTask.cpp b/User/task/ballTask.cpp index e27921b..5da663c 100644 --- a/User/task/ballTask.cpp +++ b/User/task/ballTask.cpp @@ -79,9 +79,8 @@ void FunctionBall(void *argument) } + CAN_XiaoMi(1,&ball.JZ_xiaomi,&hcan2); - - ball.Extend_control(angle1); ball.Spin(speedm); ball.Send_control();