可以小防护一下错误操作 还有bug

This commit is contained in:
ws 2025-07-08 20:37:05 +08:00
parent e17113c679
commit a939a4d8f3
10 changed files with 163 additions and 78 deletions

View File

@ -108,3 +108,15 @@
[info] Log at : 2025/7/6|23:12:04|GMT+0800 [info] Log at : 2025/7/6|23:12:04|GMT+0800
[info] Log at : 2025/7/7|04:53:38|GMT+0800
[info] Log at : 2025/7/7|08:44:07|GMT+0800
[info] Log at : 2025/7/7|14:20:11|GMT+0800
[info] Log at : 2025/7/8|02:49:12|GMT+0800
[info] Log at : 2025/7/8|13:15:42|GMT+0800
[info] Log at : 2025/7/8|18:00:22|GMT+0800

View File

@ -3,16 +3,16 @@ Build target 'R1'
compiling userTask.c... compiling userTask.c...
compiling remote_control.c... compiling remote_control.c...
compiling initTask.c... compiling initTask.c...
compiling nucTask.cpp...
compiling encodeCan.cpp... compiling encodeCan.cpp...
compiling ballTask.cpp...
compiling djiMotor.c...
compiling main.c... compiling main.c...
compiling shootTask.cpp... compiling shootTask.cpp...
compiling nucTask.cpp...
compiling djiMotor.c...
compiling ballTask.cpp...
compiling shoot.cpp... compiling shoot.cpp...
compiling ball.cpp... compiling ball.cpp...
linking... linking...
Program Size: Code=32720 RO-data=1832 RW-data=268 ZI-data=32220 Program Size: Code=32016 RO-data=1832 RW-data=276 ZI-data=32260
FromELF: creating hex file... FromELF: creating hex file...
"R1\R1.axf" - 0 Error(s), 0 Warning(s). "R1\R1.axf" - 0 Error(s), 0 Warning(s).
Build Time Elapsed: 00:00:09 Build Time Elapsed: 00:00:10

View File

@ -1 +1 @@
2025/7/6 23:29:24 2025/7/8 19:42:52

View File

@ -173,32 +173,17 @@
<Ww> <Ww>
<count>3</count> <count>3</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>ball_exit,0x0A</ItemText> <ItemText>nucbuf,0x10</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>4</count> <count>4</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>nucbuf</ItemText> <ItemText>drop_message,0x0A</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>5</count> <count>5</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>nuc_v</ItemText> <ItemText>cmd_fromnuc</ItemText>
</Ww>
<Ww>
<count>6</count>
<WinNumber>1</WinNumber>
<ItemText>test_exit,0x0A</ItemText>
</Ww>
<Ww>
<count>7</count>
<WinNumber>1</WinNumber>
<ItemText>ttttt1,0x0A</ItemText>
</Ww>
<Ww>
<count>8</count>
<WinNumber>1</WinNumber>
<ItemText>ball_exit,0x0A</ItemText>
</Ww> </Ww>
</WatchWindow1> </WatchWindow1>
<MemoryWindow4> <MemoryWindow4>
@ -967,7 +952,7 @@
<Group> <Group>
<GroupName>User/device</GroupName> <GroupName>User/device</GroupName>
<tvExp>0</tvExp> <tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel> <cbSel>0</cbSel>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
@ -1059,7 +1044,7 @@
<Group> <Group>
<GroupName>User/module</GroupName> <GroupName>User/module</GroupName>
<tvExp>1</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel> <cbSel>0</cbSel>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>

View File

@ -16,7 +16,7 @@
#endif #endif
#define ONE_CONTROL 1 #define ONE_CONTROL 0
//是否使用大疆DT7遥控器 //是否使用大疆DT7遥控器
#ifndef DT7 #ifndef DT7
@ -43,6 +43,8 @@
#define EXTEND_OK (1<<3) #define EXTEND_OK (1<<3)
//等待ok //等待ok
#define WAIT_OK (1<<4) #define WAIT_OK (1<<4)
//可以防守收回
#define DEF_READY (1<<5)
//要发送ok了 //要发送ok了
#define BALL_SEND (1<<6) #define BALL_SEND (1<<6)

View File

@ -6,7 +6,7 @@ static osThreadId_t thread_alert;
volatile uint32_t drop_message = 0; volatile uint32_t drop_message = 0;
uint8_t nucbuf[16]; uint8_t nucbuf[20];
uint8_t packet[32]; // 假设最大数据包长度为 32 字节 uint8_t packet[32]; // 假设最大数据包长度为 32 字节
static void NUC_CBLTCallback(void) static void NUC_CBLTCallback(void)
@ -82,6 +82,62 @@ int8_t NUC_SendPacket(void *data, uint16_t length) {
return DEVICE_OK; // 发送成功 return DEVICE_OK; // 发送成功
} }
// int8_t NUC_RawParse(NUC_t *n) {
// if (n == NULL) return DEVICE_ERR_NULL;
// union {
// float x[3];
// char data[12];
// } instance; // 方便在浮点数和字符数组之间进行数据转换
// // 校验数据包头
// if(nucbuf[0]!=HEAD) goto error; //发送ID不是底盘
// else
// {
// n->status_fromnuc = nucbuf[1];
// n->ctrl_status = nucbuf[2];
// switch (n->status_fromnuc) {
// case VISION:
// /* 协议格式
// 0xFF HEAD
// 0x07
// 控制帧
// 0x01 相机帧
// x fp32
// 0xFE TAIL
// */
// if (nucbuf[7] != TAIL) goto error;
// instance.data[3] = nucbuf[6];
// instance.data[2] = nucbuf[5];
// instance.data[1] = nucbuf[4];
// instance.data[0] = nucbuf[3];
// n->vision.x = instance.x[0];
// instance.data[7] = nucbuf[10];
// instance.data[6] = nucbuf[9];
// instance.data[5] = nucbuf[8];
// instance.data[4] = nucbuf[7];
// n->vision.y = instance.x[1];
// break;
// }
// return DEVICE_OK;
// }
// error:
// drop_message++;
// return DEVICE_ERR;
// }
/* 协议格式
0xFF HEAD
x fp32
y fp32
0xFE TAIL
*/
int8_t NUC_RawParse(NUC_t *n) { int8_t NUC_RawParse(NUC_t *n) {
if (n == NULL) return DEVICE_ERR_NULL; if (n == NULL) return DEVICE_ERR_NULL;
union { union {
@ -93,43 +149,24 @@ int8_t NUC_RawParse(NUC_t *n) {
if(nucbuf[0]!=HEAD) goto error; //发送ID不是底盘 if(nucbuf[0]!=HEAD) goto error; //发送ID不是底盘
else else
{ {
n->status_fromnuc = nucbuf[1];
n->ctrl_status = nucbuf[2];
switch (n->status_fromnuc) {
case VISION:
/* 协议格式
0xFF HEAD
0x07
0x01
x fp32
0xFE TAIL
*/
if (nucbuf[7] != TAIL) goto error;
instance.data[3] = nucbuf[6]; if (nucbuf[9] != TAIL) goto error;
instance.data[2] = nucbuf[5];
instance.data[1] = nucbuf[4]; instance.data[3] = nucbuf[4];
instance.data[0] = nucbuf[3]; instance.data[2] = nucbuf[3];
instance.data[1] = nucbuf[2];
instance.data[0] = nucbuf[1];
n->vision.x = instance.x[0]; n->vision.x = instance.x[0];
instance.data[7] = nucbuf[10]; instance.data[7] = nucbuf[8];
instance.data[6] = nucbuf[9]; instance.data[6] = nucbuf[7];
instance.data[5] = nucbuf[8]; instance.data[5] = nucbuf[6];
instance.data[4] = nucbuf[7]; instance.data[4] = nucbuf[5];
n->vision.y = instance.x[1]; n->vision.y = instance.x[1];
instance.data[11] = nucbuf[11];
instance.data[10] = nucbuf[12];
instance.data[9] = nucbuf[13];
instance.data[8] = nucbuf[14];
n->vision.z = instance.x[2];
break;
} }
return DEVICE_OK; return DEVICE_OK;
}
error: error:
drop_message++; drop_message++;
return DEVICE_ERR; return DEVICE_ERR;

View File

@ -10,10 +10,11 @@ extern RC_ctrl_t rc_ctrl;
extern int ball_exit; extern int ball_exit;
// 伸缩 // 伸缩
//外死点168 外163 中150 内127 限位124.8 //外死点132 外130 中119.12 内92 限位90
#define I_ANGLE 124.8
#define O_ANGLE 168 #define I_ANGLE 92
#define WAIT_POS 150 #define O_ANGLE 130
#define WAIT_POS 119
// PE11 气缸git stash apply // PE11 气缸git stash apply
@ -221,6 +222,7 @@ void Ball::ball_control()
// 进攻 // 进攻
if (ready_key == SIDE) if (ready_key == SIDE)
{ {
osThreadFlagsClear(DEF_READY);
switch (rc_key) switch (rc_key)
{ {
case MIDDLE2: case MIDDLE2:
@ -241,10 +243,20 @@ void Ball::ball_control()
// 防守 // 防守
else if(ready_key == DEF) else if(ready_key == DEF)
{ {
xiaomi.position = I_ANGLE; // 保持收回 if(hand_thread & DEF_READY)
{
xiaomi.position = I_ANGLE;
}
// 保持收回
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保爪气缸关闭 HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保爪气缸关闭
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭 HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
osThreadFlagsClear(EXTEND_OK);
osThreadFlagsClear(READY_TELL); // 蓄力标志位
osThreadFlagsClear(HANDING_FINISH);
Send_control(); Send_control();
} }
@ -259,7 +271,7 @@ void Ball::ballDown(void)
{ {
case BALL_IDLE: case BALL_IDLE:
xiaomi.position = I_ANGLE; // 保持收回 xiaomi.position = I_ANGLE; // 保持收回
if (feedback->position_deg >= I_ANGLE - 0.8 && feedback->position_deg <= I_ANGLE + 0.8) if (feedback->position_deg >= I_ANGLE - 1 && feedback->position_deg <= I_ANGLE + 1)
{ {
currentState1 = EXTEND_DOWN; currentState1 = EXTEND_DOWN;
} }
@ -278,7 +290,7 @@ void Ball::ballDown(void)
case EXTEND_OUT: case EXTEND_OUT:
xiaomi.position = O_ANGLE; // 保持伸出 xiaomi.position = O_ANGLE; // 保持伸出
if (feedback->position_deg >= O_ANGLE - 0.2 && feedback->position_deg <= O_ANGLE + 0.2) if (feedback->position_deg >= O_ANGLE - 1 && feedback->position_deg <= O_ANGLE + 1)
{ {
osThreadFlagsSet(task_struct.thread.shoot, EXTEND_OK); osThreadFlagsSet(task_struct.thread.shoot, EXTEND_OK);

View File

@ -32,16 +32,18 @@ const fp32 Shoot::M2006_angle_PID[3] = {15, 0.1, 0};
#define TO_TOP 10.0f #define TO_TOP 10.0f
#define TO_BOTTOM 6.0f #define TO_BOTTOM 6.0f
#define INIT_POS -130 #define INIT_POS -135
#define TOP_POS -211 #define TOP_POS -211
#define BOTTOM_POS 0 #define BOTTOM_POS 0
#define WAIT_POS -150
#define GO_ERROR 1.0f #define GO_ERROR 1.0f
#define Tigger_DO 0 #define Tigger_DO 0
#define Tigger_ZERO 120 #define Tigger_ZERO 138
#define Tigger_ERROR 3 #define Tigger_ERROR 3
float knob_increment; float knob_increment;
double last_distance = 4.0f; // 4米做测试吧 double last_distance = 4.0f; // 4米做测试吧
double last_pass = 4.0f; // 4米做测试吧
Shoot::Shoot() Shoot::Shoot()
{ {
@ -71,6 +73,7 @@ Shoot::Shoot()
currentState = SHOOT_IDLE; currentState = SHOOT_IDLE;
LowPassFilter2p_Init(&distance_filter, 500.0f, 80.0f); // 给distance 做滤波 LowPassFilter2p_Init(&distance_filter, 500.0f, 80.0f); // 给distance 做滤波
LowPassFilter2p_Init(&pass_filter, 500.0f, 80.0f); // 给distance 做滤波
} }
void Shoot::trigger_control() void Shoot::trigger_control()
@ -82,11 +85,17 @@ void Shoot::trigger_control()
CAN_cmd_200(result, 0, 0, 0, &hcan1); CAN_cmd_200(result, 0, 0, 0, &hcan1);
} }
// float shoot_fitting(float x)
// {
// return 0.2006334f * x * x + 25.788123f * x + -169.32157 + 3.8f-3.8f;
// }
float shoot_fitting(float x) float shoot_fitting(float x)
{ {
return 0.2006334f * x * x + 25.788123f * x + -169.32157 + 3.8f-3.8f; return 0.67076341f * x * x + 20.212423f * x + -154.53966f;
} }
float pass_fitting(float x) float pass_fitting(float x)
{ {
return 1.1790172f * x * x + 15.983755f * x + -172.04664f + 1.6f; return 1.1790172f * x * x + 15.983755f * x + -172.04664f + 1.6f;
@ -100,8 +109,13 @@ void Shoot::distanceGet(const NUC_t &nuc_v)
last_distance = LowPassFilter2p_Apply(&distance_filter, nuc_v.vision.x); last_distance = LowPassFilter2p_Apply(&distance_filter, nuc_v.vision.x);
} }
if(nuc_v.vision.y >= 0.0f && nuc_v.vision.y <= 7.5f)
{
last_pass = LowPassFilter2p_Apply(&pass_filter, nuc_v.vision.y);
}
// 否则不更新,保持上一次的值 // 否则不更新,保持上一次的值
distance = last_distance; distance = last_distance;
pass_distance =last_pass;
} }
int Shoot::GO_SendData(float pos, float limit) int Shoot::GO_SendData(float pos, float limit)
@ -225,8 +239,10 @@ void Shoot::shoot_control()
switch (mode_key) switch (mode_key)
{ {
case VSION: case VSION:
fire_pos = distance; // 视觉拟合的力 //fire_pos = distance; // 视觉拟合的力
// fire_pos =shoot_fitting(test_distance); fire_pos =shoot_fitting(distance);
//fire_pos = INIT_POS + knob_increment; // 根据旋钮值调整发射位置
switch (rc_key) switch (rc_key)
{ {
case DOWN1: case DOWN1:
@ -460,7 +476,8 @@ void Shoot::shoot_control()
switch (rc_key) switch (rc_key)
{ {
case MIDDLE1: case MIDDLE1:
fire_pos = pass_fitting(distance); //fire_pos = pass_fitting(distance);
fire_pos = pass_fitting(pass_distance);
if ((shoot_thread & READY_TELL) && !(shoot_thread & EXTEND_OK)) if ((shoot_thread & READY_TELL) && !(shoot_thread & EXTEND_OK))
{ {
// 只收到READY_TELL且未收到EXTEND_OK时顶部蓄力流程 // 只收到READY_TELL且未收到EXTEND_OK时顶部蓄力流程
@ -517,10 +534,21 @@ void Shoot::shoot_control()
} }
else if (ready_key == DEFENSE) else if (ready_key == DEFENSE)
{ {
control_pos = TOP_POS - 2.0f; //-209 control_pos = WAIT_POS; //-209
go1.Pos = control_pos; go1.Pos = control_pos;
limit_speed = TO_TOP; // 快速上去 limit_speed = TO_TOP; // 快速上去
t_posSet = Tigger_ZERO; // 扳机松开 if(feedback.fd_gopos < -149)
{
t_posSet = Tigger_ZERO; // 扳机松开
osThreadFlagsSet(task_struct.thread.ball, DEF_READY);
}
osThreadFlagsClear(EXTEND_OK);
osThreadFlagsClear(READY_TELL); // 蓄力标志位
osThreadFlagsClear(HANDING_FINISH);
} }
else else
{ {
@ -544,9 +572,13 @@ void Shoot ::ball_receive()
// 初始状态:钩子移动到顶部,钩住拉簧 // 初始状态:钩子移动到顶部,钩住拉簧
if (shoot_thread & READY_TELL) // 如果收到等待通知 if (shoot_thread & READY_TELL) // 如果收到等待通知
{ {
control_pos = TOP_POS;
limit_speed = TO_TOP; // 快速上去
t_posSet = Tigger_ZERO; t_posSet = Tigger_ZERO;
if(trigger_Motor->total_angle > Tigger_ZERO-5 )
{
control_pos = TOP_POS;
limit_speed = TO_TOP; // 快速上去
}
if (feedback.fd_gopos < -209) if (feedback.fd_gopos < -209)
{ {
currentState = GO_TOP; // 切换到准备发射状态 currentState = GO_TOP; // 切换到准备发射状态

View File

@ -87,6 +87,7 @@ public:
//滤波器 //滤波器
LowPassFilter2p_t distance_filter; // 用于滤波视觉距离 LowPassFilter2p_t distance_filter; // 用于滤波视觉距离
LowPassFilter2p_t pass_filter;
//==========================公共变量========================== //==========================公共变量==========================
int16_t rc_key; //遥控器按键 int16_t rc_key; //遥控器按键
@ -97,6 +98,7 @@ public:
volatile uint32_t shoot_thread;//接收传回的线程通知 volatile uint32_t shoot_thread;//接收传回的线程通知
fp32 distance; //视觉距离 fp32 distance; //视觉距离
fp32 pass_distance; //视觉距离
private: private:
//扳机2006 //扳机2006

View File

@ -22,7 +22,7 @@ void FunctionBall(void *argument)
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_BALL; const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_BALL;
osDelay(6000);//等待极致控制板启动 osDelay(6000);//等待极致控制板启动
XiaomiWait_init(1,&hcan2); //小米电机初始化 XiaomiWait_init(1,&hcan2); //小米电机初始化
uint32_t tick = osKernelGetTickCount(); uint32_t tick = osKernelGetTickCount();
@ -36,10 +36,13 @@ void FunctionBall(void *argument)
//abc=HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin); //abc=HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin);
abc=HAL_GPIO_ReadPin(BALL_GPIO_Port, BALL_Pin); // 0为到位 abc=HAL_GPIO_ReadPin(BALL_GPIO_Port, BALL_Pin); // 0为到位
ball.rc_mode(); // 遥控器模式 ball.rc_mode(); // 遥控器模式
ball.ball_control(); // 控制球的动作 ball.ball_control(); // 控制球的动作
// HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET); // 确保爪气缸关闭
// HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_SET); // 确保下气缸关闭
// ball.xiaomi.position = aaaa; // ball.xiaomi.position = aaaa;
// CAN_XiaoMi(1,&ball.xiaomi,&hcan2); // CAN_XiaoMi(1,&ball.xiaomi,&hcan2);