正赛了
This commit is contained in:
commit
31e7b73045
50
MDK-ARM/.vscode/keil-assistant.log
vendored
50
MDK-ARM/.vscode/keil-assistant.log
vendored
@ -102,3 +102,53 @@
|
||||
|
||||
[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
15
MDK-ARM/.vscode/uv4.log
vendored
@ -1,13 +1,18 @@
|
||||
*** Using Compiler 'V5.06 update 7 (build 960)', folder: 'D:\keil\ARM\ARMCC\Bin'
|
||||
Build target 'R1'
|
||||
compiling shootTask.cpp...
|
||||
compiling nuc.c...
|
||||
compiling userTask.c...
|
||||
compiling remote_control.c...
|
||||
compiling initTask.c...
|
||||
compiling ball.cpp...
|
||||
compiling main.c...
|
||||
compiling shootTask.cpp...
|
||||
compiling djiMotor.c...
|
||||
compiling nucTask.cpp...
|
||||
compiling encodeCan.cpp...
|
||||
compiling ballTask.cpp...
|
||||
compiling ball.cpp...
|
||||
compiling shoot.cpp...
|
||||
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...
|
||||
"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
2
MDK-ARM/.vscode/uv4.log.lock
vendored
@ -1 +1 @@
|
||||
2025/7/4 9:21:52
|
||||
2025/7/13 17:07:34
|
||||
@ -154,6 +154,7 @@
|
||||
</SetRegEntry>
|
||||
</TargetDriverDllRegistry>
|
||||
<Breakpoint/>
|
||||
<<<<<<< HEAD
|
||||
<WatchWindow1>
|
||||
<Ww>
|
||||
<count>0</count>
|
||||
@ -196,6 +197,8 @@
|
||||
<ItemText>error_code</ItemText>
|
||||
</Ww>
|
||||
</WatchWindow1>
|
||||
=======
|
||||
>>>>>>> 上层测试
|
||||
<MemoryWindow4>
|
||||
<Mm>
|
||||
<WinNumber>4</WinNumber>
|
||||
|
||||
@ -16,7 +16,7 @@
|
||||
#endif
|
||||
|
||||
|
||||
#define ONE_CONTROL 1
|
||||
#define ONE_CONTROL 0
|
||||
|
||||
//是否使用大疆DT7遥控器
|
||||
#ifndef DT7
|
||||
@ -28,7 +28,8 @@
|
||||
//================任务通知,时间组================//
|
||||
//事件组
|
||||
#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)
|
||||
//等待ok
|
||||
#define WAIT_OK (1<<4)
|
||||
//可以防守收回
|
||||
#define DEF_READY (1<<5)
|
||||
|
||||
//要发送ok了
|
||||
#define BALL_SEND (1<<6)
|
||||
|
||||
@ -215,15 +215,13 @@ static osThreadId_t thread_alert;
|
||||
void Dji_Motor_CB()
|
||||
{
|
||||
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)
|
||||
{
|
||||
HAL_CAN_GetRxMessage(&hcan2, CAN_RX_FIFO1, &rx_header, rx_data);
|
||||
|
||||
//osThreadFlagsSet(thread_alert, EVENT_CAN);
|
||||
osThreadFlagsSet(thread_alert, EVENT_CAN2);
|
||||
}
|
||||
|
||||
/**
|
||||
@ -234,9 +232,6 @@ void can2_Motor_CB(void)
|
||||
void djiInit(void)
|
||||
{
|
||||
thread_alert = osThreadGetId();
|
||||
|
||||
|
||||
|
||||
BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_RX_FIFO0_MSG_PENDING_CB,
|
||||
Dji_Motor_CB);
|
||||
BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_RX_FIFO1_MSG_PENDING_CB,
|
||||
@ -250,11 +245,9 @@ void djiInit(void)
|
||||
*/
|
||||
uint32_t waitNewDji()
|
||||
{
|
||||
// return osEventFlagsWait(
|
||||
// eventReceive, EVENT_CAN,osFlagsWaitAny, osWaitForever);
|
||||
// 等待CAN1或CAN2任意一个事件
|
||||
return osThreadFlagsWait(
|
||||
EVENT_CAN, osFlagsWaitAll, osWaitForever);
|
||||
|
||||
EVENT_CAN1 | EVENT_CAN2, osFlagsWaitAny, osWaitForever);
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
@ -6,7 +6,7 @@ static osThreadId_t thread_alert;
|
||||
|
||||
volatile uint32_t drop_message = 0;
|
||||
|
||||
uint8_t nucbuf[16];
|
||||
uint8_t nucbuf[18];
|
||||
uint8_t packet[32]; // 假设最大数据包长度为 32 字节
|
||||
|
||||
static void NUC_CBLTCallback(void)
|
||||
@ -82,6 +82,62 @@ int8_t NUC_SendPacket(void *data, uint16_t length) {
|
||||
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) {
|
||||
if (n == NULL) return DEVICE_ERR_NULL;
|
||||
union {
|
||||
@ -93,43 +149,24 @@ int8_t NUC_RawParse(NUC_t *n) {
|
||||
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];
|
||||
if (nucbuf[17] != TAIL) goto error;
|
||||
|
||||
instance.data[3] = nucbuf[8];
|
||||
instance.data[2] = nucbuf[7];
|
||||
instance.data[1] = nucbuf[6];
|
||||
instance.data[0] = nucbuf[5];
|
||||
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];
|
||||
instance.data[7] = nucbuf[16];
|
||||
instance.data[6] = nucbuf[15];
|
||||
instance.data[5] = nucbuf[14];
|
||||
instance.data[4] = nucbuf[13];
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
error:
|
||||
drop_message++;
|
||||
return DEVICE_ERR;
|
||||
|
||||
@ -11,14 +11,14 @@ extern int ball_exit;
|
||||
|
||||
// 外死点168 外163 中150 内127 限位124.8
|
||||
// 伸缩
|
||||
//外死点168 外163 中150 内127 限位124.8
|
||||
#define I_ANGLE 127
|
||||
#define O_ANGLE 163
|
||||
#define WAIT_POS 150
|
||||
#define IN 124.8
|
||||
#define OUT 168
|
||||
//外死点89 外85 中75 内49 限位46
|
||||
|
||||
// PE11 气缸
|
||||
#define I_ANGLE 49
|
||||
#define O_ANGLE 85
|
||||
#define WAIT_POS 75
|
||||
#define HANGDING_POS 89
|
||||
|
||||
// PE11 气缸git stash apply
|
||||
|
||||
Ball ::Ball()
|
||||
{
|
||||
@ -107,7 +107,7 @@ void Ball::Move_Extend()
|
||||
}
|
||||
if (extern_key == OUT)
|
||||
{
|
||||
xiaomi.position = O_ANGLE;
|
||||
xiaomi.position = HANGDING_POS;
|
||||
}
|
||||
}
|
||||
|
||||
@ -218,12 +218,13 @@ void Ball::ballHadling(void)
|
||||
void Ball::ball_control()
|
||||
{
|
||||
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)
|
||||
{
|
||||
osThreadFlagsClear(DEF_READY);
|
||||
switch (rc_key)
|
||||
{
|
||||
case MIDDLE2:
|
||||
@ -244,10 +245,23 @@ void Ball::ball_control()
|
||||
// 防守
|
||||
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(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
|
||||
|
||||
osThreadFlagsClear(EXTEND_OK);
|
||||
osThreadFlagsClear(READY_TELL); // 蓄力标志位
|
||||
osThreadFlagsClear(HANDING_FINISH);
|
||||
|
||||
haveball=0;//变为空球状态
|
||||
|
||||
currentState1 = BALL_IDLE;
|
||||
|
||||
Send_control();
|
||||
}
|
||||
|
||||
@ -257,11 +271,12 @@ void Ball::ball_control()
|
||||
|
||||
void Ball::ballDown(void)
|
||||
{
|
||||
osThreadFlagsClear(HANDING_FINISH);
|
||||
switch (currentState1)
|
||||
{
|
||||
case BALL_IDLE:
|
||||
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;
|
||||
}
|
||||
@ -280,7 +295,7 @@ void Ball::ballDown(void)
|
||||
|
||||
case EXTEND_OUT:
|
||||
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);
|
||||
@ -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()
|
||||
{
|
||||
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); // 确保下气缸关闭
|
||||
|
||||
osThreadFlagsClear(EXTEND_OK);
|
||||
|
||||
if (ready_key == SIDE) // 检测是否准备好
|
||||
{
|
||||
xiaomi.position = WAIT_POS;
|
||||
if (feedback->position_deg >= WAIT_POS - 3)
|
||||
if(hand_thread & HANDING_FINISH)
|
||||
{
|
||||
xiaomi.position=HANGDING_POS;//继续保持外伸
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
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
|
||||
{
|
||||
@ -339,9 +419,11 @@ void Ball::Idle_control()
|
||||
{
|
||||
currentState1 = BALL_IDLE;
|
||||
}
|
||||
// xiaomi.position = I_ANGLE;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
int ball_state = 0;
|
||||
int last_ball_state = 0; // 上一次的光电状态
|
||||
|
||||
@ -353,8 +435,8 @@ void Ball::ballHadling(void)
|
||||
case BALL_IDLE:
|
||||
if (rc_key == DOWN2)
|
||||
{
|
||||
xiaomi.position = O_ANGLE; // 外伸
|
||||
if (feedback->position_deg >= O_ANGLE - 1) // 确保伸缩电机到位
|
||||
xiaomi.position = HANGDING_POS; // 外伸
|
||||
if (feedback->position_deg >= HANGDING_POS - 0.5f) // 确保伸缩电机到位
|
||||
{
|
||||
currentState1 = BALL_FORWARD;
|
||||
}
|
||||
@ -399,7 +481,7 @@ void Ball::ballHadling(void)
|
||||
|
||||
case BALL_FINISH:
|
||||
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(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
|
||||
break;
|
||||
|
||||
@ -74,11 +74,10 @@ public:
|
||||
int16_t extern_key;
|
||||
int16_t ready_key; //准备按键
|
||||
//用于传接球,运球后通知
|
||||
volatile BallState_t ballStatus;//是否有球
|
||||
volatile uint32_t hand_thread;//接收传回的线程通知
|
||||
|
||||
private:
|
||||
|
||||
bool haveball;
|
||||
|
||||
|
||||
};
|
||||
|
||||
@ -15,7 +15,7 @@ extern RC_ctrl_t rc_ctrl;
|
||||
NUC_t nuc_v;
|
||||
float vofa[8];
|
||||
|
||||
double test_distance;
|
||||
double test_distance=4.0;
|
||||
|
||||
// sw[7]👆 1694 中 1000 👇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_angle_PID[3] = {15, 0.1, 0};
|
||||
|
||||
#define TO_TOP 10.0f
|
||||
#define TO_TOP 13.0f
|
||||
#define TO_BOTTOM 6.0f
|
||||
|
||||
#define INIT_POS -130
|
||||
#define INIT_POS -120
|
||||
#define TOP_POS -211
|
||||
#define BOTTOM_POS 0
|
||||
#define WAIT_POS -170
|
||||
#define CHANEGE_POS -205
|
||||
#define GO_ERROR 1.0f
|
||||
#define Tigger_DO 0
|
||||
#define Tigger_ZERO 120
|
||||
#define Tigger_DO -10
|
||||
#define Tigger_ZERO 115
|
||||
#define Tigger_ERROR 3
|
||||
|
||||
float knob_increment;
|
||||
double last_distance = 4.0f; // 4米做测试吧
|
||||
double last_pass = 4.0f; // 4米做测试吧
|
||||
|
||||
Shoot::Shoot()
|
||||
{
|
||||
@ -71,6 +74,7 @@ Shoot::Shoot()
|
||||
currentState = SHOOT_IDLE;
|
||||
|
||||
LowPassFilter2p_Init(&distance_filter, 500.0f, 80.0f); // 给distance 做滤波
|
||||
LowPassFilter2p_Init(&pass_filter, 500.0f, 80.0f); // 给distance 做滤波
|
||||
}
|
||||
|
||||
void Shoot::trigger_control()
|
||||
@ -82,14 +86,27 @@ void Shoot::trigger_control()
|
||||
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)
|
||||
{
|
||||
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)
|
||||
{
|
||||
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)
|
||||
@ -100,8 +117,13 @@ void Shoot::distanceGet(const NUC_t &nuc_v)
|
||||
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;
|
||||
pass_distance =last_pass;
|
||||
}
|
||||
|
||||
int Shoot::GO_SendData(float pos, float limit)
|
||||
@ -216,6 +238,7 @@ void Shoot::rc_mode()
|
||||
|
||||
#if ONE_CONTROL == 0
|
||||
|
||||
float and1=0.0f;
|
||||
void Shoot::shoot_control()
|
||||
{
|
||||
|
||||
@ -225,8 +248,10 @@ void Shoot::shoot_control()
|
||||
switch (mode_key)
|
||||
{
|
||||
case VSION:
|
||||
fire_pos = distance; // 视觉拟合的力
|
||||
// fire_pos =shoot_fitting(test_distance);
|
||||
//fire_pos = distance; // 视觉拟合的力
|
||||
fire_pos =shoot_fitting(distance)+and1;
|
||||
//fire_pos = INIT_POS + knob_increment; // 根据旋钮值调整发射位置
|
||||
|
||||
switch (rc_key)
|
||||
{
|
||||
case DOWN1:
|
||||
@ -266,8 +291,8 @@ void Shoot::shoot_control()
|
||||
break;
|
||||
case PASS:
|
||||
// 实时可调蓄力位置
|
||||
fire_pos = INIT_POS + knob_increment; // 根据旋钮值调整发射位置
|
||||
|
||||
//fire_pos = INIT_POS + knob_increment; // 根据旋钮值调整发射位置
|
||||
fire_pos =pass_fitting(pass_distance);
|
||||
switch (rc_key)
|
||||
{
|
||||
case DOWN1:
|
||||
@ -311,7 +336,7 @@ void Shoot::shoot_control()
|
||||
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);
|
||||
|
||||
@ -390,6 +415,10 @@ void Shoot::RemoveError()
|
||||
|
||||
#if ONE_CONTROL
|
||||
|
||||
//float and1=-1.5f;
|
||||
float and1=0;
|
||||
float and2=0;
|
||||
|
||||
void Shoot::shoot_control()
|
||||
{
|
||||
|
||||
@ -404,7 +433,9 @@ void Shoot::shoot_control()
|
||||
switch (mode_key)
|
||||
{
|
||||
case VSION:
|
||||
fire_pos = shoot_fitting(distance); // 视觉拟合的力
|
||||
fire_pos = shoot_fitting(distance)+and1; // 视觉拟合的力
|
||||
//fire_pos = INIT_POS + knob_increment; // 根据旋钮值调整发射位置
|
||||
|
||||
switch (rc_key)
|
||||
{
|
||||
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)
|
||||
{
|
||||
shoot_wait = 1;
|
||||
BSP_Buzzer_Start();
|
||||
BSP_Buzzer_Set(1, 500);
|
||||
//BSP_Buzzer_Start();
|
||||
//BSP_Buzzer_Set(1, 500);
|
||||
}
|
||||
}
|
||||
|
||||
@ -434,7 +465,7 @@ void Shoot::shoot_control()
|
||||
if (feedback.fd_tpos >= Tigger_ZERO - 20)
|
||||
{
|
||||
|
||||
BSP_Buzzer_Stop();
|
||||
//BSP_Buzzer_Stop();
|
||||
currentState = SHOOT_IDLE;
|
||||
osThreadFlagsClear(EXTEND_OK);
|
||||
osThreadFlagsClear(READY_TELL); // 蓄力标志位
|
||||
@ -458,7 +489,7 @@ void Shoot::shoot_control()
|
||||
switch (rc_key)
|
||||
{
|
||||
case MIDDLE1:
|
||||
fire_pos = pass_fitting(distance);
|
||||
fire_pos = pass_fitting(pass_distance)+and2;
|
||||
if ((shoot_thread & READY_TELL) && !(shoot_thread & 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)
|
||||
{
|
||||
shoot_wait = 1;
|
||||
BSP_Buzzer_Start();
|
||||
BSP_Buzzer_Set(1, 500);
|
||||
// BSP_Buzzer_Start();
|
||||
// BSP_Buzzer_Set(1, 500);
|
||||
}
|
||||
}
|
||||
// 没收到READY_TELL不做任何蓄力
|
||||
@ -489,7 +520,7 @@ void Shoot::shoot_control()
|
||||
if (feedback.fd_tpos >= Tigger_ZERO - 20)
|
||||
{
|
||||
|
||||
BSP_Buzzer_Stop();
|
||||
//BSP_Buzzer_Stop();
|
||||
currentState = SHOOT_IDLE;
|
||||
osThreadFlagsClear(EXTEND_OK);
|
||||
osThreadFlagsClear(READY_TELL); // 蓄力标志位
|
||||
@ -515,10 +546,28 @@ void Shoot::shoot_control()
|
||||
}
|
||||
else if (ready_key == DEFENSE)
|
||||
{
|
||||
control_pos = TOP_POS - 2.0f; //-209
|
||||
|
||||
control_pos = WAIT_POS; //-209
|
||||
go1.Pos = control_pos;
|
||||
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
|
||||
{
|
||||
@ -533,6 +582,7 @@ void Shoot::shoot_control()
|
||||
trigger_control();
|
||||
}
|
||||
|
||||
|
||||
// 配合运球到发射
|
||||
void Shoot ::ball_receive()
|
||||
{
|
||||
@ -542,11 +592,17 @@ void Shoot ::ball_receive()
|
||||
// 初始状态：钩子移动到顶部，钩住拉簧
|
||||
if (shoot_thread & READY_TELL) // 如果收到等待通知
|
||||
{
|
||||
control_pos = TOP_POS;
|
||||
limit_speed = TO_TOP; // 快速上去
|
||||
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)
|
||||
{
|
||||
|
||||
currentState = GO_TOP; // 切换到准备发射状态
|
||||
}
|
||||
}
|
||||
@ -591,7 +647,7 @@ void Shoot::RemoveError()
|
||||
control_pos = INIT_POS;
|
||||
BSP_Buzzer_Stop();
|
||||
}
|
||||
limit_speed = 3.0f; // 慢慢送上去
|
||||
limit_speed = 5.0f; // 慢慢送上去
|
||||
go1.Pos = control_pos;
|
||||
}
|
||||
|
||||
|
||||
@ -87,6 +87,7 @@ public:
|
||||
|
||||
//滤波器
|
||||
LowPassFilter2p_t distance_filter; // 用于滤波视觉距离
|
||||
LowPassFilter2p_t pass_filter;
|
||||
|
||||
//==========================公共变量==========================
|
||||
int16_t rc_key; //遥控器按键
|
||||
@ -97,6 +98,7 @@ public:
|
||||
|
||||
volatile uint32_t shoot_thread;//接收传回的线程通知
|
||||
fp32 distance; //视觉距离
|
||||
fp32 pass_distance; //视觉距离
|
||||
|
||||
private:
|
||||
//扳机2006
|
||||
|
||||
@ -22,7 +22,7 @@ void FunctionBall(void *argument)
|
||||
|
||||
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_BALL;
|
||||
|
||||
osDelay(6000);//等待极致控制板启动
|
||||
osDelay(6000);//等待极致控制板启动
|
||||
XiaomiWait_init(1,&hcan2); //小米电机初始化
|
||||
|
||||
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(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;
|
||||
// CAN_XiaoMi(1,&ball.xiaomi,&hcan2);
|
||||
|
||||
|
||||
@ -26,7 +26,7 @@ void FunctionCan(void *argument)
|
||||
task_struct.stack_water_mark.can = osThreadGetStackSpace(osThreadGetId());
|
||||
|
||||
#endif
|
||||
waitNewDji();
|
||||
//waitNewDji();
|
||||
djiMotorEncode();
|
||||
can2MotorEncode();
|
||||
|
||||
|
||||
@ -14,7 +14,7 @@ extern "C" {
|
||||
|
||||
/* 所有任务都要define一个“任务运行频率”和“初始化延时” */
|
||||
#define TASK_FREQ_SHOOT (500u)
|
||||
#define TASK_FREQ_CAN (1500u)
|
||||
#define TASK_FREQ_CAN (500u)
|
||||
#define TASK_FREQ_AI (500u)
|
||||
#define TASK_FREQ_BALL (500u)
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user