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-07-13 20:45:47 +08:00
commit 31e7b73045
14 changed files with 334 additions and 101 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -102,3 +102,53 @@
[info] Log at : 2025/7/4|09:10:46|GMT+0800 [info] Log at : 2025/7/4|09:10:46|GMT+0800
[info] Log at : 2025/7/6|22:01:26|GMT+0800
[info] Log at : 2025/7/6|22:02:13|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
[info] Log at : 2025/7/8|20:39:07|GMT+0800
[info] Log at : 2025/7/9|08:39:28|GMT+0800
[info] Log at : 2025/7/9|11:36:02|GMT+0800
[info] Log at : 2025/7/9|16:01:34|GMT+0800
[info] Log at : 2025/7/9|23:48:02|GMT+0800
[info] Log at : 2025/7/10|01:59:23|GMT+0800
[info] Log at : 2025/7/10|15:33:45|GMT+0800
[info] Log at : 2025/7/10|17:26:59|GMT+0800
[info] Log at : 2025/7/10|22:12:10|GMT+0800
[info] Log at : 2025/7/11|10:11:25|GMT+0800
[info] Log at : 2025/7/11|17:39:11|GMT+0800
[info] Log at : 2025/7/12|23:45:28|GMT+0800
[info] Log at : 2025/7/13|11:08:55|GMT+0800
[info] Log at : 2025/7/13|12:29:41|GMT+0800
[info] Log at : 2025/7/13|16:33:17|GMT+0800
[info] Log at : 2025/7/13|16:42:25|GMT+0800

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

@ -1,13 +1,18 @@
*** Using Compiler 'V5.06 update 7 (build 960)', folder: 'D:\keil\ARM\ARMCC\Bin' *** Using Compiler 'V5.06 update 7 (build 960)', folder: 'D:\keil\ARM\ARMCC\Bin'
Build target 'R1' Build target 'R1'
compiling shootTask.cpp... compiling userTask.c...
compiling nuc.c... compiling remote_control.c...
compiling initTask.c... compiling initTask.c...
compiling ball.cpp... compiling main.c...
compiling shootTask.cpp...
compiling djiMotor.c...
compiling nucTask.cpp... compiling nucTask.cpp...
compiling encodeCan.cpp...
compiling ballTask.cpp...
compiling ball.cpp...
compiling shoot.cpp... compiling shoot.cpp...
linking... linking...
Program Size: Code=32960 RO-data=1832 RW-data=268 ZI-data=32220 Program Size: Code=32032 RO-data=1832 RW-data=284 ZI-data=32268
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:02:09 Build Time Elapsed: 00:00:09

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

@ -1 +1 @@
2025/7/4 9:21:52 2025/7/13 17:07:34

3
MDK-ARM/R1.uvoptx
View File

@ -154,6 +154,7 @@
</SetRegEntry> </SetRegEntry>
</TargetDriverDllRegistry> </TargetDriverDllRegistry>
<Breakpoint/> <Breakpoint/>
<<<<<<< HEAD
<WatchWindow1> <WatchWindow1>
<Ww> <Ww>
<count>0</count> <count>0</count>
@ -196,6 +197,8 @@
<ItemText>error_code</ItemText> <ItemText>error_code</ItemText>
</Ww> </Ww>
</WatchWindow1> </WatchWindow1>
=======
>>>>>>> 上层测试
<MemoryWindow4> <MemoryWindow4>
<Mm> <Mm>
<WinNumber>4</WinNumber> <WinNumber>4</WinNumber>

7
User/bsp/TopDefine.h
View File

@ -16,7 +16,7 @@
#endif #endif
#define ONE_CONTROL 1 #define ONE_CONTROL 0
//是否使用大疆DT7遥控器 //是否使用大疆DT7遥控器
#ifndef DT7 #ifndef DT7
@ -28,7 +28,8 @@
//================任务通知,时间组================// //================任务通知,时间组================//
//事件组 //事件组
#define EVENT_RC (1<<1) #define EVENT_RC (1<<1)
#define EVENT_CAN (1<<2) #define EVENT_CAN1 (1<<2)
#define EVENT_CAN2 (1<<3)
//================任务通知================// //================任务通知================//
//运球 //运球
@ -42,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)

15
User/device/djiMotor.c
View File

@ -215,15 +215,13 @@ static osThreadId_t thread_alert;
void Dji_Motor_CB() void Dji_Motor_CB()
{ {
HAL_CAN_GetRxMessage(&hcan1, CAN_RX_FIFO0, &dji_rx_header, dji_rx_data); HAL_CAN_GetRxMessage(&hcan1, CAN_RX_FIFO0, &dji_rx_header, dji_rx_data);
osThreadFlagsSet(thread_alert, EVENT_CAN1);
osThreadFlagsSet(thread_alert, EVENT_CAN);
// osEventFlagsSet(eventReceive, EVENT_CAN);
} }
void can2_Motor_CB(void) void can2_Motor_CB(void)
{ {
HAL_CAN_GetRxMessage(&hcan2, CAN_RX_FIFO1, &rx_header, rx_data); HAL_CAN_GetRxMessage(&hcan2, CAN_RX_FIFO1, &rx_header, rx_data);
osThreadFlagsSet(thread_alert, EVENT_CAN2);
//osThreadFlagsSet(thread_alert, EVENT_CAN);
} }
/** /**
@ -234,9 +232,6 @@ void can2_Motor_CB(void)
void djiInit(void) void djiInit(void)
{ {
thread_alert = osThreadGetId(); thread_alert = osThreadGetId();
BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_RX_FIFO0_MSG_PENDING_CB, BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_RX_FIFO0_MSG_PENDING_CB,
Dji_Motor_CB); Dji_Motor_CB);
BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_RX_FIFO1_MSG_PENDING_CB, BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_RX_FIFO1_MSG_PENDING_CB,
@ -250,11 +245,9 @@ void djiInit(void)
*/ */
uint32_t waitNewDji() uint32_t waitNewDji()
{ {
// return osEventFlagsWait( // 等待CAN1或CAN2任意一个事件
// eventReceive, EVENT_CAN,osFlagsWaitAny, osWaitForever);
return osThreadFlagsWait( return osThreadFlagsWait(
EVENT_CAN, osFlagsWaitAll, osWaitForever); EVENT_CAN1 | EVENT_CAN2, osFlagsWaitAny, osWaitForever);
} }
#endif #endif

97
User/device/nuc.c
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[18];
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,42 +149,23 @@ 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[17] != TAIL) goto error;
instance.data[2] = nucbuf[5];
instance.data[1] = nucbuf[4]; instance.data[3] = nucbuf[8];
instance.data[0] = nucbuf[3]; instance.data[2] = nucbuf[7];
instance.data[1] = nucbuf[6];
instance.data[0] = nucbuf[5];
n->vision.x = instance.x[0]; n->vision.x = instance.x[0];
instance.data[7] = nucbuf[10]; instance.data[7] = nucbuf[16];
instance.data[6] = nucbuf[9]; instance.data[6] = nucbuf[15];
instance.data[5] = nucbuf[8]; instance.data[5] = nucbuf[14];
instance.data[4] = nucbuf[7]; instance.data[4] = nucbuf[13];
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++;

120
User/module/ball.cpp
View File

@ -11,14 +11,14 @@ extern int ball_exit;
// 外死点168 外163 中150 内127 限位124.8 // 外死点168 外163 中150 内127 限位124.8
// 伸缩 // 伸缩
//外死点168 外163 中150 内127 限位124.8 //外死点89 外85 中75 内49 限位46
#define I_ANGLE 127
#define O_ANGLE 163
#define WAIT_POS 150
#define IN 124.8
#define OUT 168
// PE11 气缸 #define I_ANGLE 49
#define O_ANGLE 85
#define WAIT_POS 75
#define HANGDING_POS 89
// PE11 气缸git stash apply
Ball ::Ball() Ball ::Ball()
{ {
@ -107,7 +107,7 @@ void Ball::Move_Extend()
} }
if (extern_key == OUT) if (extern_key == OUT)
{ {
xiaomi.position = O_ANGLE; xiaomi.position = HANGDING_POS;
} }
} }
@ -218,12 +218,13 @@ void Ball::ballHadling(void)
void Ball::ball_control() void Ball::ball_control()
{ {
hand_thread = osThreadFlagsGet(); // 获取任务通知标志位 hand_thread = osThreadFlagsGet(); // 获取任务通知标志位
ball_state = HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin); // 读取光电状态(有球 1，无球 0) ball_state = HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin); // 读取光电状态(有球 0，无球 1)
// 进攻 // 进攻
if (ready_key == SIDE) if (ready_key == SIDE)
{ {
osThreadFlagsClear(DEF_READY);
switch (rc_key) switch (rc_key)
{ {
case MIDDLE2: case MIDDLE2:
@ -244,10 +245,23 @@ 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);
haveball=0;//变为空球状态
currentState1 = BALL_IDLE;
Send_control(); Send_control();
} }
@ -257,11 +271,12 @@ void Ball::ball_control()
void Ball::ballDown(void) void Ball::ballDown(void)
{ {
osThreadFlagsClear(HANDING_FINISH);
switch (currentState1) switch (currentState1)
{ {
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;
} }
@ -280,7 +295,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);
@ -300,24 +315,89 @@ void Ball::ballDown(void)
} }
} }
// void Ball::Idle_control()
// {
// HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保爪气缸关闭
// HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
// osThreadFlagsClear(EXTEND_OK);
// if (ready_key == SIDE) // 检测是否准备好
// {
// xiaomi.position = WAIT_POS;
// if (feedback->position_deg >= WAIT_POS - 3)
// {
// // 只在READY_TELL未置位时发送，防止重复
// if ((osThreadFlagsGet() & READY_TELL) == 0)
// {
// osThreadFlagsSet(task_struct.thread.shoot, READY_TELL);
// }
// }
// }
// else
// {
// xiaomi.position = I_ANGLE; // 默认回到收回位置
// }
// // 拨杆回到中间挡位时，回位并重置状态机
// if (currentState1 == EXTEND_FINISH) // 转移后
// {
// xiaomi.position = I_ANGLE;
// currentState1 = BALL_IDLE;
// }
// if (currentState1 == BALL_FINISH) // 运球完成
// {
// xiaomi.position = O_ANGLE;
// currentState1 = BALL_IDLE;
// }
// else
// {
// currentState1 = BALL_IDLE;
// }
// // xiaomi.position = I_ANGLE;
// }
void Ball::Idle_control() void Ball::Idle_control()
{ {
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保爪气缸关闭 if(ball_state==1 && haveball==0)// 读取光电状态(有球 0，无球 1)
{
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET); // 确保爪气缸张开
}
if(ball_state==0)
{
haveball=1;//变为持球状态
osDelay(500);
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(EXTEND_OK);
if (ready_key == SIDE) // 检测是否准备好 if (ready_key == SIDE) // 检测是否准备好
{ {
xiaomi.position = WAIT_POS; if(hand_thread & HANDING_FINISH)
if (feedback->position_deg >= WAIT_POS - 3)
{ {
xiaomi.position=HANGDING_POS;//继续保持外伸
}
else
{
xiaomi.position = WAIT_POS;
if (feedback->position_deg >= WAIT_POS - 3)
{
// 只在READY_TELL未置位时发送，防止重复 // 只在READY_TELL未置位时发送，防止重复
if ((osThreadFlagsGet() & READY_TELL) == 0) if ((osThreadFlagsGet() & READY_TELL) == 0)
{ {
osThreadFlagsSet(task_struct.thread.shoot, READY_TELL); osThreadFlagsSet(task_struct.thread.shoot, READY_TELL);
} }
}
} }
} }
else else
{ {
@ -339,9 +419,11 @@ void Ball::Idle_control()
{ {
currentState1 = BALL_IDLE; currentState1 = BALL_IDLE;
} }
// xiaomi.position = I_ANGLE;
} }
int ball_state = 0; int ball_state = 0;
int last_ball_state = 0; // 上一次的光电状态 int last_ball_state = 0; // 上一次的光电状态
@ -353,8 +435,8 @@ void Ball::ballHadling(void)
case BALL_IDLE: case BALL_IDLE:
if (rc_key == DOWN2) if (rc_key == DOWN2)
{ {
xiaomi.position = O_ANGLE; // 外伸 xiaomi.position = HANGDING_POS; // 外伸
if (feedback->position_deg >= O_ANGLE - 1) // 确保伸缩电机到位 if (feedback->position_deg >= HANGDING_POS - 0.5f) // 确保伸缩电机到位
{ {
currentState1 = BALL_FORWARD; currentState1 = BALL_FORWARD;
} }
@ -399,7 +481,7 @@ void Ball::ballHadling(void)
case BALL_FINISH: case BALL_FINISH:
osDelay(50); // 延时 50ms osDelay(50); // 延时 50ms
// osThreadFlagsSet(task_struct.thread.ball, HANDING_FINISH); osThreadFlagsSet(task_struct.thread.ball, HANDING_FINISH);
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); // 确保下气缸关闭
break; break;

3
User/module/ball.hpp
View File

@ -74,11 +74,10 @@ public:
int16_t extern_key; int16_t extern_key;
int16_t ready_key; //准备按键 int16_t ready_key; //准备按键
//用于传接球,运球后通知 //用于传接球,运球后通知
volatile BallState_t ballStatus;//是否有球
volatile uint32_t hand_thread;//接收传回的线程通知 volatile uint32_t hand_thread;//接收传回的线程通知
private: private:
bool haveball;
}; };

106
User/module/shoot.cpp
View File

@ -15,7 +15,7 @@ extern RC_ctrl_t rc_ctrl;
NUC_t nuc_v; NUC_t nuc_v;
float vofa[8]; float vofa[8];
double test_distance; double test_distance=4.0;
// sw[7]👆 1694 中 1000 👇306 // sw[7]👆 1694 中 1000 👇306
// sw[2]👆 1694 👇306 // sw[2]👆 1694 👇306
@ -29,19 +29,22 @@ double test_distance;
const fp32 Shoot::M2006_speed_PID[3] = {5, 0, 0.01}; const fp32 Shoot::M2006_speed_PID[3] = {5, 0, 0.01};
const fp32 Shoot::M2006_angle_PID[3] = {15, 0.1, 0}; const fp32 Shoot::M2006_angle_PID[3] = {15, 0.1, 0};
#define TO_TOP 10.0f #define TO_TOP 13.0f
#define TO_BOTTOM 6.0f #define TO_BOTTOM 6.0f
#define INIT_POS -130 #define INIT_POS -120
#define TOP_POS -211 #define TOP_POS -211
#define BOTTOM_POS 0 #define BOTTOM_POS 0
#define WAIT_POS -170
#define CHANEGE_POS -205
#define GO_ERROR 1.0f #define GO_ERROR 1.0f
#define Tigger_DO 0 #define Tigger_DO -10
#define Tigger_ZERO 120 #define Tigger_ZERO 115
#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 +74,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,14 +86,27 @@ 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.67076341f * x * x + 20.212423f * x + -154.53966f + 1.0f;
// }
// float shoot_fitting(float x)
// {
// return 1.4255807f * x * x + 12.138447f * x + -136.35306f;
// }
float shoot_fitting(float x) float shoot_fitting(float x)
{ {
return 0.2006334f * x * x + 25.788123f * x + -169.32157 + 3.8f; return 1.2143736f * x * x + 14.733786f * x + -133.3627f;
} }
float pass_fitting(float x) float pass_fitting(float x)
{ {
return 1.1790172f * x * x + 15.983755f * x + -172.04664f + 1.6f; return 0.36807548f * x * x + 25.002169f * x + -199.2727f;
} }
void Shoot::distanceGet(const NUC_t &nuc_v) void Shoot::distanceGet(const NUC_t &nuc_v)
@ -100,8 +117,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)
@ -216,6 +238,7 @@ void Shoot::rc_mode()
#if ONE_CONTROL == 0 #if ONE_CONTROL == 0
float and1=0.0f;
void Shoot::shoot_control() void Shoot::shoot_control()
{ {
@ -225,8 +248,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)+and1;
//fire_pos = INIT_POS + knob_increment; // 根据旋钮值调整发射位置
switch (rc_key) switch (rc_key)
{ {
case DOWN1: case DOWN1:
@ -266,8 +291,8 @@ void Shoot::shoot_control()
break; break;
case PASS: case PASS:
// 实时可调蓄力位置 // 实时可调蓄力位置
fire_pos = INIT_POS + knob_increment; // 根据旋钮值调整发射位置 //fire_pos = INIT_POS + knob_increment; // 根据旋钮值调整发射位置
fire_pos =pass_fitting(pass_distance);
switch (rc_key) switch (rc_key)
{ {
case DOWN1: case DOWN1:
@ -311,7 +336,7 @@ void Shoot::shoot_control()
break; break;
} }
abs_limit_min_max_fp(&go1.Pos, -210.0f, 2.0f); abs_limit_min_max_fp(&go1.Pos, -210.0f, 8.0f);
// 发送数据到蓄力电机 // 发送数据到蓄力电机
GO_SendData(go1.Pos, limit_speed); GO_SendData(go1.Pos, limit_speed);
@ -390,6 +415,10 @@ void Shoot::RemoveError()
#if ONE_CONTROL #if ONE_CONTROL
//float and1=-1.5f;
float and1=0;
float and2=0;
void Shoot::shoot_control() void Shoot::shoot_control()
{ {
@ -404,7 +433,9 @@ void Shoot::shoot_control()
switch (mode_key) switch (mode_key)
{ {
case VSION: case VSION:
fire_pos = shoot_fitting(distance); // 视觉拟合的力 fire_pos = shoot_fitting(distance)+and1; // 视觉拟合的力
//fire_pos = INIT_POS + knob_increment; // 根据旋钮值调整发射位置
switch (rc_key) switch (rc_key)
{ {
case MIDDLE1: case MIDDLE1:
@ -420,8 +451,8 @@ void Shoot::shoot_control()
if (feedback.fd_gopos >= fire_pos - GO_ERROR && feedback.fd_gopos <= fire_pos + GO_ERROR) if (feedback.fd_gopos >= fire_pos - GO_ERROR && feedback.fd_gopos <= fire_pos + GO_ERROR)
{ {
shoot_wait = 1; shoot_wait = 1;
BSP_Buzzer_Start(); //BSP_Buzzer_Start();
BSP_Buzzer_Set(1, 500); //BSP_Buzzer_Set(1, 500);
} }
} }
@ -434,7 +465,7 @@ void Shoot::shoot_control()
if (feedback.fd_tpos >= Tigger_ZERO - 20) if (feedback.fd_tpos >= Tigger_ZERO - 20)
{ {
BSP_Buzzer_Stop(); //BSP_Buzzer_Stop();
currentState = SHOOT_IDLE; currentState = SHOOT_IDLE;
osThreadFlagsClear(EXTEND_OK); osThreadFlagsClear(EXTEND_OK);
osThreadFlagsClear(READY_TELL); // 蓄力标志位 osThreadFlagsClear(READY_TELL); // 蓄力标志位
@ -458,7 +489,7 @@ void Shoot::shoot_control()
switch (rc_key) switch (rc_key)
{ {
case MIDDLE1: case MIDDLE1:
fire_pos = pass_fitting(distance); fire_pos = pass_fitting(pass_distance)+and2;
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时，顶部蓄力流程
@ -473,8 +504,8 @@ void Shoot::shoot_control()
if (feedback.fd_gopos >= fire_pos - GO_ERROR && feedback.fd_gopos <= fire_pos + GO_ERROR) if (feedback.fd_gopos >= fire_pos - GO_ERROR && feedback.fd_gopos <= fire_pos + GO_ERROR)
{ {
shoot_wait = 1; shoot_wait = 1;
BSP_Buzzer_Start(); // BSP_Buzzer_Start();
BSP_Buzzer_Set(1, 500); // BSP_Buzzer_Set(1, 500);
} }
} }
// 没收到READY_TELL不做任何蓄力 // 没收到READY_TELL不做任何蓄力
@ -489,7 +520,7 @@ void Shoot::shoot_control()
if (feedback.fd_tpos >= Tigger_ZERO - 20) if (feedback.fd_tpos >= Tigger_ZERO - 20)
{ {
BSP_Buzzer_Stop(); //BSP_Buzzer_Stop();
currentState = SHOOT_IDLE; currentState = SHOOT_IDLE;
osThreadFlagsClear(EXTEND_OK); osThreadFlagsClear(EXTEND_OK);
osThreadFlagsClear(READY_TELL); // 蓄力标志位 osThreadFlagsClear(READY_TELL); // 蓄力标志位
@ -515,10 +546,28 @@ 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 < WAIT_POS +2.0f)
{
t_posSet = Tigger_ZERO; // 扳机松开
//osThreadFlagsSet(task_struct.thread.ball, DEF_READY);
// 只在READY_TELL未置位时发送，防止重复
if ((osThreadFlagsGet() & DEF_READY) == 0)
{
osThreadFlagsSet(task_struct.thread.ball, DEF_READY);
}
}
osThreadFlagsClear(EXTEND_OK);
osThreadFlagsClear(READY_TELL); // 蓄力标志位
osThreadFlagsClear(HANDING_FINISH);
currentState = SHOOT_IDLE;
} }
else else
{ {
@ -533,6 +582,7 @@ void Shoot::shoot_control()
trigger_control(); trigger_control();
} }
// 配合运球到发射 // 配合运球到发射
void Shoot ::ball_receive() void Shoot ::ball_receive()
{ {
@ -542,11 +592,17 @@ 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; // 切换到准备发射状态
} }
} }
@ -591,7 +647,7 @@ void Shoot::RemoveError()
control_pos = INIT_POS; control_pos = INIT_POS;
BSP_Buzzer_Stop(); BSP_Buzzer_Stop();
} }
limit_speed = 3.0f; // 慢慢送上去 limit_speed = 5.0f; // 慢慢送上去
go1.Pos = control_pos; go1.Pos = control_pos;
} }

2
User/module/shoot.hpp
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

9
User/task/ballTask.cpp
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);

2
User/task/encodeCan.cpp
View File

@ -26,7 +26,7 @@ void FunctionCan(void *argument)
task_struct.stack_water_mark.can = osThreadGetStackSpace(osThreadGetId()); task_struct.stack_water_mark.can = osThreadGetStackSpace(osThreadGetId());
#endif #endif
waitNewDji(); //waitNewDji();
djiMotorEncode(); djiMotorEncode();
can2MotorEncode(); can2MotorEncode();

2
User/task/userTask.h
View File

@ -14,7 +14,7 @@ extern "C" {
/* 所有任务都要define一个“任务运行频率”和“初始化延时” */ /* 所有任务都要define一个“任务运行频率”和“初始化延时” */
#define TASK_FREQ_SHOOT (500u) #define TASK_FREQ_SHOOT (500u)
#define TASK_FREQ_CAN (1500u) #define TASK_FREQ_CAN (500u)
#define TASK_FREQ_AI (500u) #define TASK_FREQ_AI (500u)
#define TASK_FREQ_BALL (500u) #define TASK_FREQ_BALL (500u)