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

有问题

Browse Source
This commit is contained in:
ws 2025-06-14 01:07:21 +08:00
parent e531eb8d47
commit a3877e40fd
13 changed files with 228 additions and 116 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -20,3 +20,13 @@
[info] Log at : 2025/6/11|17:37:52|GMT+0800
[info] Log at : 2025/6/12|17:09:40|GMT+0800
[info] Log at : 2025/6/12|21:20:00|GMT+0800
[info] Log at : 2025/6/13|11:22:38|GMT+0800
[info] Log at : 2025/6/13|19:51:19|GMT+0800
[info] Log at : 2025/6/13|19:56:03|GMT+0800

6
MDK-ARM/.vscode/settings.json vendored Normal file
View File

@ -0,0 +1,6 @@
{
"files.associations": {
"djimotor.h": "c",
"user_math.h": "c"
}
}

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

@ -1,8 +1,8 @@
*** Using Compiler 'V5.06 update 7 (build 960)', folder: 'D:\keil\ARM\ARMCC\Bin'
Build target 'R1'
compiling ballTask.cpp...
compiling user_math.c...
linking...
Program Size: Code=29728 RO-data=1884 RW-data=284 ZI-data=33268
Program Size: Code=30000 RO-data=1884 RW-data=284 ZI-data=33268
FromELF: creating hex file...
"R1\R1.axf" - 0 Error(s), 0 Warning(s).
Build Time Elapsed: 00:00:06
Build Time Elapsed: 00:00:05

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

@ -1 +1 @@
2025/6/9 20:40:44
2025/6/13 23:04:00

26
MDK-ARM/R1.uvoptx
View File

@ -145,7 +145,7 @@
<SetRegEntry>
<Number>0</Number>
<Key>ST-LINKIII-KEIL_SWO</Key>
<Name>-U-O142 -O2254 -SF10000 -C0 -A0 -I0 -HNlocalhost -HP7184 -P1 -N00("ARM CoreSight SW-DP (ARM Core") -D00(2BA01477) -L00(0) -TO131090 -TC10000000 -TT10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO7 -FD20000000 -FC800 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name>
<Name>-U00160029510000164E574E32 -O2254 -SF10000 -C0 -A0 -I0 -HNlocalhost -HP7184 -P1 -N00("ARM CoreSight SW-DP (ARM Core") -D00(2BA01477) -L00(0) -TO131090 -TC10000000 -TT10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC800 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name>
</SetRegEntry>
</TargetDriverDllRegistry>
<Breakpoint/>
@ -175,6 +175,26 @@
<WinNumber>1</WinNumber>
<ItemText>goangle,0x0A</ItemText>
</Ww>
<Ww>
<count>5</count>
<WinNumber>1</WinNumber>
<ItemText>ball,0x0A</ItemText>
</Ww>
<Ww>
<count>6</count>
<WinNumber>1</WinNumber>
<ItemText>jy,0x0A</ItemText>
</Ww>
<Ww>
<count>7</count>
<WinNumber>1</WinNumber>
<ItemText>angle1,0x0A</ItemText>
</Ww>
<Ww>
<count>8</count>
<WinNumber>1</WinNumber>
<ItemText>angle2,0x0A</ItemText>
</Ww>
</WatchWindow1>
<Tracepoint>
<THDelay>0</THDelay>
@ -934,7 +954,7 @@
<Group>
<GroupName>User/device</GroupName>
<tvExp>1</tvExp>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
@ -1026,7 +1046,7 @@
<Group>
<GroupName>User/module</GroupName>
<tvExp>1</tvExp>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>

62
User/lib/user_math.c
View File

@ -113,23 +113,53 @@ 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)
// fp32 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;
// }
fp32 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;
}
}
// 使用取模运算将角度限制在 [-180, 180]
relative_angle = (relative_angle + 180) % 360 - 180;
return relative_angle;
}
fp32 motor_ecd_to_angle_change(uint32_t ecd, uint16_t offset_ecd)
{
int32_t relative_ecd = ecd - offset_ecd;
if (relative_ecd > 4095)
{
while(relative_ecd>4095)
{
relative_ecd -= 8191;
}
}
else if (relative_ecd < -4096)
{
while(relative_ecd<-4096)
{
relative_ecd += 8191;
}
}
return relative_ecd / (float)CAN_MOTOR_ENC_RES * 360.0f;
}

7
User/lib/user_math.h
View File

@ -14,10 +14,13 @@ extern "C" {
#include <float.h>
#include <math.h>
#include <stdbool.h>
#include "struct_typedef.h"
#define M_DEG2RAD_MULT (0.01745329251f)
#define M_RAD2DEG_MULT (57.2957795131f)
#define MOTOR_ECD_TO_RAD 0.000766990394f
#define CAN_MOTOR_ENC_RES (8191) /* 电机编码器分辨率 */
#ifndef M_PI
#define M_PI 3.14159265358979323846f
@ -102,7 +105,9 @@ 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);
fp32 motor_angle_change(uint32_t ecd, uint16_t offset_ecd);
fp32 motor_ecd_to_angle_change(uint32_t ecd, uint16_t offset_ecd);
#ifdef __cplusplus
}

112
User/module/ball.cpp
View File

@ -4,13 +4,22 @@
#include "detect.h"
#include "userTask.h"
#include "user_math.h"
#include "shoot.hpp"
//int jy=1;
extern RC_ctrl_t rc_ctrl;
extern int key;
extern int16_t M2006_result;
// C键 sw[5]👆 1800 中1000 👇200
// D键 sw[6]👆 200 👇1800
#define M_SPEED 4000
#define I_ANGLE1 185
#define I_ANGLE2 -75
#define O_ANGLE1 340
#define O_ANGLE2 -235
#if HANDLING3 == 1
//三摩擦轮运球！！！
@ -18,7 +27,7 @@ extern int key;
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};
const fp32 Ball:: Extend_angle_PID[3]= { 50, 0, 0.1};
//摩擦轮转速
int speedm=0;
@ -42,17 +51,17 @@ Ball ::Ball()
speedSet[i] = 0;
}
//两个伸缩6020
//两个伸缩6020 左207 右208
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[0],PID_POSITION,Extend_speed_PID,20000, 2000);
PID_init(&extend_angle_pid[0],PID_POSITION,Extend_angle_PID,10000, 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);
PID_init(&extend_speed_pid[1],PID_POSITION,Extend_speed_PID,20000, 2000);
PID_init(&extend_angle_pid[1],PID_POSITION,Extend_angle_PID,10000, 1000);
e_result[0] = 0;
e_result[1] = 0;
@ -65,21 +74,62 @@ Ball ::Ball()
}
// 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);
// }
//左0 右1
//real_round 外 176 240 内 89 326
// void Ball ::Extend_mcontrol(int angle1,int angle2)
// {
// int16_t delta[2];
// angleSet[0] = angle1/2;
// angleSet[1] = angle2/2;
// fp32 angle_get[2];
// 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[0], angle_get[0] , 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], angle_get[1] , angleSet[1]);
// e_result[1] = PID_calc(&extend_speed_pid[1], Extern_Motor[1]->speed_rpm, delta[1]);
// }
void Ball ::Extend_mcontrol(int angle1,int angle2)
{
int16_t delta[2];
angleSet[0] = angle1;
angleSet[1] = angle2;
angleSet[0] = angle1/2;
angleSet[1] = angle2/2;
delta[0] = PID_calc(&extend_angle_pid[0], Extern_Motor[0]->total_angle , angleSet[0]);
fp32 angle_get[2];
angle_get[0] = motor_ecd_to_angle_change(Extern_Motor[0]->ecd, angleSet[0]);
angle_get[1] = motor_ecd_to_angle_change(Extern_Motor[1]->ecd, angleSet[1]);
delta[0] = PID_calc(&extend_angle_pid[0], angle_get[0] , 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]);
delta[1] = PID_calc(&extend_angle_pid[1], angle_get[1] , angleSet[1]);
e_result[1] = PID_calc(&extend_speed_pid[1], Extern_Motor[1]->speed_rpm, delta[1]);
}
void Ball ::Extend_control(int angle)
{
int16_t delta[2];
@ -92,19 +142,6 @@ void Ball ::Extend_control(int angle)
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]);
}
@ -136,17 +173,20 @@ void Ball::ballStop(void)
}
int ball_have = 0;
void Ball::ballTake(void)
void Ball::Move_Extend(void)
{
ball_have=HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin);//有球 1 无球 0
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET);//确保闭合气缸
speedm=2000;
if(ball_have==1)
{
speedm=0;
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);//确保闭合气缸
}
if(rc_ctrl.sw[6] == 200) // 左拨杆上
{
Extend_mcontrol(I_ANGLE1, I_ANGLE2); // 内伸
}
else if(rc_ctrl.sw[6] == 1800) // 左拨杆下
{
Extend_mcontrol(O_ANGLE1, O_ANGLE2); // 外伸
}
else
{
Extend_mcontrol(O_ANGLE1, O_ANGLE2); // 外伸
}
}
@ -223,8 +263,8 @@ 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);
CAN_cmd_1FF(M2006_result,0,e_result[0],e_result[1],&hcan1);
osDelay(1);
}

3
User/module/ball.hpp
View File

@ -10,6 +10,7 @@
#include "pid.h"
#include "filter.h"
// 定义状态枚举
typedef enum {
BALL_IDLE, // 空闲状态
@ -53,7 +54,7 @@ public:
void ballDown(void);
void Send_control(void);
void ballStop(void);
void ballTake(void);
void Move_Extend(void);
void Extend_control(int angle);
void Extend_mcontrol(int angle1,int angle2);

11
User/module/shoot.cpp
View File

@ -28,6 +28,7 @@ const fp32 Shoot:: M2006_angle_PID[3] = { 25, 0, 0.1};
#define Tigger_DO -5
#define Tigger_ZERO 130
int16_t M2006_result =0;
float knob_increment;
Shoot::Shoot()
@ -65,8 +66,9 @@ void Shoot::trigger_control()
int delta = 0;
delta = PID_calc(&angle_pid,trigger_Motor->total_angle,t_posSet); // 计算位置环PID
result = PID_calc(&speed_pid,trigger_Motor->speed_rpm,delta); // 计算速度环PID
M2006_result=result;
// result = PID_calc(&speed_pid,trigger_Motor->speed_rpm,t_speedSet);
CAN_cmd_1FF(result,0,0,0,&hcan1);
// CAN_cmd_1FF(result,0,0,0,&hcan1);
}
@ -143,7 +145,7 @@ void Shoot::rc_mode()
// 计算旋钮增量
if (current_knob_value >= 200 && current_knob_value <= 1800) {
knob_increment = (current_knob_value - last_knob_value) / 50.0f; // 每80单位对应一个增量
knob_increment = (current_knob_value - last_knob_value) / 20.0f; // 每80单位对应一个增量
} else {
knob_increment = 0; // 如果旋钮值超出范围，不产生增量
}
@ -168,6 +170,7 @@ void Shoot::shoot_control() {
if (currentState == SHOOT_READY) {
// 如果当前状态是准备发射，松开钩子发射
t_posSet = Tigger_ZERO; // 扳机扣动
BSP_Buzzer_Stop();
if (t_posSet >= 120) { // 假设120度为发射完成角度
currentState = SHOOT_IDLE; // 切换到空闲状态
is_hooked = false; // 重置钩子状态
@ -212,7 +215,7 @@ void Shoot :: shoot_Current()
case SHOOT_TOP:
t_posSet = Tigger_DO; // 扳机扣动钩住
osDelay(500); // 等待一段时间，确保扳机动作完成
osDelay(1000); // 等待一段时间，确保扳机动作完成
if (fd_tpos <1)
{
//判定为钩住
@ -226,6 +229,8 @@ void Shoot :: shoot_Current()
{
go1.Pos = fire_pos; // 下拉到中间挡位位置
if (feedback.fd_gopos >= fire_pos - 0.3f && feedback.fd_gopos <= fire_pos + 0.3f) {
BSP_Buzzer_Start();
BSP_Buzzer_Set(1,5000);
//currentState = SHOOT_WAIT; // 等待发射信号
//在拨杆切换时触发了
}

20
User/module/shoot.hpp
View File

@ -17,19 +17,19 @@ typedef enum {
SHOOT_RETURN // 自动返回状态
} ShootState_t;
// 定义状态枚举
typedef enum {
BALL_IDLE, // 空闲状态
BALL_FINISH, // 完成状态
//持球状态
BALL_TAKE,
BALL_LOSE
// // 定义状态枚举
// typedef enum {
// BALL_IDLE, // 空闲状态
// BALL_FINISH, // 完成状态
// //持球状态
// BALL_TAKE,
// BALL_LOSE
} BallState_t;
// } BallState_t;
typedef enum
{
UP1,
UP1=1,
MIDDLE1,
DOWN1,
SHOOT,
@ -79,7 +79,7 @@ public:
int16_t rc_key; //遥控器按键
int16_t trigger_key;
ShootState_t currentState; //状态机
volatile BallState_t ballStatus;//是否有球
//volatile BallState_t ballStatus;//是否有球
volatile uint32_t flag_thread;//接收传回的线程通知
float distance; //视觉距离

77
User/task/ballTask.cpp
View File

@ -15,8 +15,9 @@ Ball ball;
// D键 sw[6]👆 200 外伸 👇1800 归位 //三摩擦架子
// H键 sw[7]👆 200 👇1800 //取球
int angle1=34;
int angle2=23;
int angle1=330;
int angle2=-240;
int test111=0;
//检查光电
int abc=0;
@ -39,50 +40,44 @@ void FunctionBall(void *argument)
abc=HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin);
if(rc_ctrl.sw[6] == 200)
{
angle1=85;
}
if(rc_ctrl.sw[6]==1800)
{
angle1=10;
// if(rc_ctrl.sw[6] == 200)
// {
// angle1=85;
// }
// if(rc_ctrl.sw[6]==1800)
// {
// angle1=10;
}
// }
//运球
if(rc_ctrl.sw[5]==200)
{
//👇
ball.ballHadling(); // 处理摩擦轮转动
}
//下球
if(rc_ctrl.sw[5]==1800)
{
//👆
ball.ballDown();
}
//中间
if(rc_ctrl.sw[7]==200)
{
if(rc_ctrl.sw[5]==1000)
{
//中
ball.ballStop();
ball.ballHadling(); // 处理摩擦轮转动
}
}
if(rc_ctrl.sw[7]==1800)
{
//ball.ballTake();
}
// CAN_XiaoMi(1,&ball.JZ_xiaomi,&hcan2);
// //运球
// if(rc_ctrl.sw[5]==200)
// {
// //👇
// ball.ballHadling(); // 处理摩擦轮转动
// }
// //下球
// if(rc_ctrl.sw[5]==1800)
// {
// //👆
// ball.ballDown();
// }
// //中间
// if(rc_ctrl.sw[7]==200)
// {
// if(rc_ctrl.sw[5]==1000)
// {
// //中
// ball.ballStop();
// ball.ballHadling(); // 处理摩擦轮转动
// }
// }
//ball.Extend_control(test111);
ball.Move_Extend();
// ball.Extend_mcontrol(angle1,angle2); // 内伸
ball.Spin(speedm);
ball.Send_control();
ball.Extend_control(angle1);
// vofa[0] = -speedm; // 发送电机角度数据
// vofa[1] = ball.hand_Motor[0]->speed_rpm; // 发送电机角度数据

2
User/task/userTask.h
View File

@ -17,7 +17,7 @@ extern "C" {
#define TASK_FREQ_CTRL_SHOOT (500u)
#define TASK_FREQ_CTRL_COMMAND (500u)
#define TASK_FREQ_MONITOR (2u)
#define TASK_FREQ_CAN (1000u)
#define TASK_FREQ_CAN (1500u)
#define TASK_FREQ_AI (500u)
#define TASK_FREQ_BALL (500u)