From c362ffe265b12a080291da222ffb6812e7de0cd5 Mon Sep 17 00:00:00 2001 From: ws <1621320660@qq.com> Date: Mon, 5 May 2025 18:40:54 +0800 Subject: [PATCH] =?UTF-8?q?=E8=AE=B0=E5=BF=866020?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/device/djiMotor.c | 39 +++++++++++++++++++++++++++++++-------- User/device/djiMotor.h | 4 +++- User/lib/user_math.c | 21 +++++++++++++++++++++ User/lib/user_math.h | 3 +++ User/module/ball.cpp | 36 +++++++++++++++++++++++++++++++++++- User/module/ball.hpp | 11 +++++++++++ User/module/gimbal.cpp | 12 ++++++------ User/module/shoot.cpp | 1 + User/task/ballTask.cpp | 3 +++ User/task/gimbalTask.cpp | 2 +- 10 files changed, 115 insertions(+), 17 deletions(-) diff --git a/User/device/djiMotor.c b/User/device/djiMotor.c index 39d30a7..9198d57 100644 --- a/User/device/djiMotor.c +++ b/User/device/djiMotor.c @@ -41,10 +41,23 @@ else if((ptr)->type == M2006) \ (ptr)->total_angle = (fp64)((ptr)->round_cnt * 8192 + (ptr)->ecd - (ptr)->offset_ecd ) * MOTOR_ECD_TO_ANGLE_2006; \ else \ - (ptr)->total_angle = (fp64)((ptr)->round_cnt * 8192 + (ptr)->ecd - (ptr)->offset_ecd ) * MOTOR_ECD_TO_ANGLE_6020; \ + (ptr)->total_angle = (fp64)((ptr)->round_cnt * 8192 + (ptr)->ecd - (ptr)->offset_ecd ) * MOTOR_ECD_TO_ANGLE_6020; \ (ptr)->real_round = (ptr)->total_angle / 360; \ (ptr)->real_angle = (ptr)->total_angle % 360; \ } + //单圈绝对值式解析 + #define get_6020_motor_measure(ptr, data) \ + { \ + (ptr)->last_ecd = (ptr)->ecd; \ + (ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]); \ + (ptr)->speed_rpm = (uint16_t)((data)[2] << 8 | (data)[3]); \ + (ptr)->given_current = (uint16_t)((data)[4] << 8 | (data)[5]); \ + (ptr)->temperate = (data)[6]; \ + (ptr)->real_round=(ptr)->ecd/ (float)CAN_MOTOR_ENC_RES * 360.0f; \ + (ptr)->ecd - (ptr)->last_ecd > 4096 ? ((ptr)->round_cnt--) : ((ptr)->round_cnt=(ptr)->round_cnt); \ + (ptr)->ecd - (ptr)->last_ecd < -4096 ? ((ptr)->round_cnt++) : ((ptr)->round_cnt=(ptr)->round_cnt); \ + (ptr)->total_angle = (fp64)((ptr)->round_cnt * 8192 + (ptr)->ecd - (ptr)->offset_ecd ) * MOTOR_ECD_TO_ANGLE_6020; \ + } /*(ptr)->real_angle = (ptr)->real_angle % 360; */ #define get_motor_offset(ptr, data) \ @@ -99,13 +112,11 @@ void djiMotorEncode() { switch (dji_rx_header.StdId) { - case 0x201: - case 0x202: - case 0x203: - case 0x204: - case 0x205: - case 0x206: - case 0x207: + case M3508_1: + case M3508_2: + case M3508_3: + case M3508_4: + case M2006_1: { static uint8_t i = 0; //get motor id @@ -119,6 +130,18 @@ void djiMotorEncode() } break; } + case GM6020_1: + { + if(motor_chassis[6].msg_cnt<=50) + { + motor_chassis[6].msg_cnt++; + get_motor_offset(&motor_chassis[6], dji_rx_data); + }else{ + get_6020_motor_measure(&motor_chassis[6], dji_rx_data); + } + break; + } + default: { break; diff --git a/User/device/djiMotor.h b/User/device/djiMotor.h index ce2ed5f..5b91458 100644 --- a/User/device/djiMotor.h +++ b/User/device/djiMotor.h @@ -15,7 +15,7 @@ extern "C"{ typedef enum{ GM6020 = 0, M3508 = 1, - M2006 = 2 + M2006 = 2, }motor_type_e; typedef union{ @@ -102,6 +102,8 @@ typedef struct #define MOTOR_ECD_TO_ANGLE_3508 (360.0 / 8191.0 / (3591.0/187.0)) #define MOTOR_ECD_TO_ANGLE_6020 (360.0 / 8191.0 ) #define MOTOR_ECD_TO_ANGLE_2006 (360.0 / 8191.0 / 36) +#define CAN_MOTOR_ENC_RES (8191) /* 电机编码器分辨率 */ +#define M_2PI 6.28318530717958647692f #if FREERTOS_DJI == 1 /** diff --git a/User/lib/user_math.c b/User/lib/user_math.c index b51d2b5..35a176b 100644 --- a/User/lib/user_math.c +++ b/User/lib/user_math.c @@ -112,3 +112,24 @@ inline float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm) { /* 不为裁判系统设定值时,计算转速 */ return 60.0f * (float)bullet_speed / (M_2PI * fric_radius); } + +float motor_angle_change(uint32_t angle, uint16_t offset_angle) +{ + int32_t relative_angle = angle - offset_angle; + if (relative_angle > 180) + { + while(relative_angle>180) + { + relative_angle -= 360; + } + } + else if (relative_angle < -180) + { + while(relative_angle<-180) + { + relative_angle += 360; + } + } + return relative_angle; + +} diff --git a/User/lib/user_math.h b/User/lib/user_math.h index c0677f2..15d6a9a 100644 --- a/User/lib/user_math.h +++ b/User/lib/user_math.h @@ -15,6 +15,7 @@ extern "C" { #include #include + #define M_DEG2RAD_MULT (0.01745329251f) #define M_RAD2DEG_MULT (57.2957795131f) @@ -101,6 +102,8 @@ void CircleReverse(float *origin); */ float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm); +float motor_angle_change(uint32_t ecd, uint16_t offset_ecd); + #ifdef __cplusplus } #endif diff --git a/User/module/ball.cpp b/User/module/ball.cpp index 74b4cf6..bb6772e 100644 --- a/User/module/ball.cpp +++ b/User/module/ball.cpp @@ -3,15 +3,21 @@ #include "remote_control.h" #include "detect.h" #include "userTask.h" - +#include "user_math.h" extern RC_ctrl_t rc_ctrl; extern int key; +#define START 0 +#define OUT 100 + #if HANDLING3 == 1 //三摩擦轮运球！！！ //添加平移3508 得跑位置吧 const fp32 Ball:: M3508_speed_PID[3] = {5, 0.01, 0}; +const fp32 Ball:: Extend_speed_PID[3] = {5, 0.01, 0}; +const fp32 Ball:: Extend_angle_PID[3]= { 5, 0.01, 0}; + int speedm=0; int speedm1=0; //PE11 气缸 @@ -19,6 +25,16 @@ int speedm1=0; Ball ::Ball() { detect_init(); + + //伸缩6020 + Extern_Motor = get_motor_point(6); + //Extern_Motor->type = GM6020; + PID_init(&extend_speed_pid,PID_POSITION,Extend_speed_PID,3000, 200); + PID_init(&extend_angle_pid,PID_POSITION,Extend_angle_PID,3000, 200); + +// e_result = 0; +// angleSet = 0; + //三摩擦轮 for(int i = 0;i < MOTOR_NUM;i ++) { hand_Motor[i] = get_motor_point(i); @@ -30,11 +46,29 @@ Ball ::Ball() result[i] = 0; speedSet[i] = 0; } + //状态机状态初始化 currentState1= BALL_IDLE; } +void Ball ::Extend_control(int angle) +{ + int16_t delta; + angleSet = angle/2; + + fp32 angle_get; + angle_get=motor_angle_change(Extern_Motor->real_round, angleSet); + + delta = PID_calc(&extend_angle_pid, angle_get, angleSet); + e_result = PID_calc(&extend_speed_pid, Extern_Motor->speed_rpm, delta); + + CAN_cmd_1FF(0,0,e_result,0,&hcan1); + osDelay(1); + +} + + void Ball ::Spin(float speed,float speed2) { diff --git a/User/module/ball.hpp b/User/module/ball.hpp index 2ea05e9..a6d8093 100644 --- a/User/module/ball.hpp +++ b/User/module/ball.hpp @@ -43,9 +43,14 @@ public: void Spin(float speed,float speed2); void ballHadling(void); void Send_control(void); + void Extend_control(int angle); BallState_t currentState1; // 当前状态 + //伸缩6020 + int16_t e_result; + motor_measure_t * Extern_Motor; + int16_t result[MOTOR_NUM]; motor_measure_t *hand_Motor[MOTOR_NUM]; //==========================公共变量==========================// @@ -58,11 +63,17 @@ 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; + pid_type_def extend_angle_pid; + float angleSet; float speedSet[MOTOR_NUM]; }; diff --git a/User/module/gimbal.cpp b/User/module/gimbal.cpp index c089503..c59a90e 100644 --- a/User/module/gimbal.cpp +++ b/User/module/gimbal.cpp @@ -19,13 +19,13 @@ const fp32 Gimbal:: Gimbal_angle_PID[3]= { 5, 0.01, 0}; #if GM6020ING ==1 Gimbal::Gimbal() { - GM6020_Motor = get_motor_point(6); - GM6020_Motor->type = GM6020; - PID_init(&speed_pid,PID_POSITION,Gimbal_speed_PID,16000, 6000); - PID_init(&angle_pid,PID_POSITION,Gimbal_angle_PID,5000, 2000); + // GM6020_Motor = get_motor_point(6); + // GM6020_Motor->type = GM6020; + // PID_init(&speed_pid,PID_POSITION,Gimbal_speed_PID,16000, 6000); + // PID_init(&angle_pid,PID_POSITION,Gimbal_angle_PID,5000, 2000); - result = 0; - angleSet = 0; + // result = 0; + // angleSet = 0; } diff --git a/User/module/shoot.cpp b/User/module/shoot.cpp index b4b8d14..42d418a 100644 --- a/User/module/shoot.cpp +++ b/User/module/shoot.cpp @@ -5,6 +5,7 @@ #include "FreeRTOS.h" #include #include "calc_lib.h" + extern RC_ctrl_t rc_ctrl; #define SHOOT_SPEED 40000 #define SHOOT_SPEED_BACK -2000 diff --git a/User/task/ballTask.cpp b/User/task/ballTask.cpp index 21b846f..5ee0f86 100644 --- a/User/task/ballTask.cpp +++ b/User/task/ballTask.cpp @@ -10,6 +10,8 @@ extern RC_ctrl_t rc_ctrl; Ball ball; float vofa[8]; +int out=0; + int abc=0; extern int speedm; extern int speedm1; @@ -29,6 +31,7 @@ void FunctionBall(void *argument) abc=HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin); //abc=HAL_GPIO_ReadPin(STOP_GPIO_Port, STOP_Pin); + ball.Extend_control(out); ball.ballHadling(); // 处理摩擦轮转动 ball.Spin(speedm,speedm1); ball.Send_control(); diff --git a/User/task/gimbalTask.cpp b/User/task/gimbalTask.cpp index 89165fe..7db5aa3 100644 --- a/User/task/gimbalTask.cpp +++ b/User/task/gimbalTask.cpp @@ -35,7 +35,7 @@ void FunctionGimbal(void *argument) if (osMessageQueueGet(task_struct.msgq.nuc, &nucData, NULL, 0) == osOK) { // 使用接收到的视觉数据调整云台 - gimbal.gimbalVision(nucData); + //gimbal.gimbalVision(nucData); } osDelay(1);