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

记忆6020

Browse Source
This commit is contained in:
ws 2025-05-05 18:40:54 +08:00
parent 793e40985b
commit c362ffe265
10 changed files with 115 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
User/device/djiMotor.c
View File

@ -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;

4
User/device/djiMotor.h
View File

@ -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
/**

21
User/lib/user_math.c
View File

@ -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;
}

3
User/lib/user_math.h
View File

@ -15,6 +15,7 @@ extern "C" {
#include <math.h>
#include <stdbool.h>
#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

36
User/module/ball.cpp
View File

@ -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)
{

11
User/module/ball.hpp
View File

@ -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];
};

12
User/module/gimbal.cpp
View File

@ -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;
}

1
User/module/shoot.cpp
View File

@ -5,6 +5,7 @@
#include "FreeRTOS.h"
#include <cmsis_os2.h>
#include "calc_lib.h"
extern RC_ctrl_t rc_ctrl;
#define SHOOT_SPEED 40000
#define SHOOT_SPEED_BACK -2000

3
User/task/ballTask.cpp
View File

@ -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();

2
User/task/gimbalTask.cpp
View File

@ -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);