Compare commits

..

No commits in common. "31e7b730453020ba6bc162944ec838a61750975f" and "bf3d376b607fa9e307e55079835d2043ce4a7859" have entirely different histories.

14 changed files with 101 additions and 334 deletions

View File

@ -102,53 +102,3 @@
[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

View File

@ -1,18 +1,13 @@
*** 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 userTask.c...
compiling remote_control.c...
compiling initTask.c...
compiling main.c...
compiling shootTask.cpp... compiling shootTask.cpp...
compiling djiMotor.c... compiling nuc.c...
compiling nucTask.cpp... compiling initTask.c...
compiling encodeCan.cpp...
compiling ballTask.cpp...
compiling ball.cpp... compiling ball.cpp...
compiling nucTask.cpp...
compiling shoot.cpp... compiling shoot.cpp...
linking... linking...
Program Size: Code=32032 RO-data=1832 RW-data=284 ZI-data=32268 Program Size: Code=32960 RO-data=1832 RW-data=268 ZI-data=32220
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:02:09

View File

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

View File

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

View File

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

@ -215,13 +215,15 @@ 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);
} }
/** /**
@ -232,6 +234,9 @@ 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,
@ -245,9 +250,11 @@ void djiInit(void)
*/ */
uint32_t waitNewDji() uint32_t waitNewDji()
{ {
// 等待CAN1或CAN2任意一个事件 // return osEventFlagsWait(
// eventReceive, EVENT_CAN,osFlagsWaitAny, osWaitForever);
return osThreadFlagsWait( return osThreadFlagsWait(
EVENT_CAN1 | EVENT_CAN2, osFlagsWaitAny, osWaitForever); EVENT_CAN, osFlagsWaitAll, osWaitForever);
} }
#endif #endif

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[18]; uint8_t nucbuf[16];
uint8_t packet[32]; // 假设最大数据包长度为 32 字节 uint8_t packet[32]; // 假设最大数据包长度为 32 字节
static void NUC_CBLTCallback(void) static void NUC_CBLTCallback(void)
@ -82,62 +82,6 @@ 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 {
@ -149,24 +93,43 @@ 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;
if (nucbuf[17] != TAIL) goto error; instance.data[3] = nucbuf[6];
instance.data[2] = nucbuf[5];
instance.data[3] = nucbuf[8]; instance.data[1] = nucbuf[4];
instance.data[2] = nucbuf[7]; instance.data[0] = nucbuf[3];
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[16]; instance.data[7] = nucbuf[10];
instance.data[6] = nucbuf[15]; instance.data[6] = nucbuf[9];
instance.data[5] = nucbuf[14]; instance.data[5] = nucbuf[8];
instance.data[4] = nucbuf[13]; instance.data[4] = nucbuf[7];
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

@ -11,14 +11,14 @@ extern int ball_exit;
// 外死点168 外163 中150 内127 限位124.8 // 外死点168 外163 中150 内127 限位124.8
// 伸缩 // 伸缩
//外死点89 外85 中75 内49 限位46 //外死点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
#define I_ANGLE 49 // PE11 气缸
#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 = HANGDING_POS; xiaomi.position = O_ANGLE;
} }
} }
@ -218,13 +218,12 @@ 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); // 读取光电状态(有球 0无球 1) ball_state = HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin); // 读取光电状态(有球 1无球 0)
// 进攻 // 进攻
if (ready_key == SIDE) if (ready_key == SIDE)
{ {
osThreadFlagsClear(DEF_READY);
switch (rc_key) switch (rc_key)
{ {
case MIDDLE2: case MIDDLE2:
@ -245,23 +244,10 @@ void Ball::ball_control()
// 防守 // 防守
else if(ready_key == DEF) else if(ready_key == DEF)
{ {
if(hand_thread & DEF_READY) xiaomi.position = I_ANGLE; // 保持收回
{
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();
} }
@ -271,12 +257,11 @@ 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 - 1 && feedback->position_deg <= I_ANGLE + 1) if (feedback->position_deg >= I_ANGLE - 0.8 && feedback->position_deg <= I_ANGLE + 0.8)
{ {
currentState1 = EXTEND_DOWN; currentState1 = EXTEND_DOWN;
} }
@ -295,7 +280,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 - 1 && feedback->position_deg <= O_ANGLE + 1) if (feedback->position_deg >= O_ANGLE - 0.2 && feedback->position_deg <= O_ANGLE + 0.2)
{ {
osThreadFlagsSet(task_struct.thread.shoot, EXTEND_OK); osThreadFlagsSet(task_struct.thread.shoot, EXTEND_OK);
@ -315,89 +300,24 @@ 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()
{ {
if(ball_state==1 && haveball==0)// 读取光电状态(有球 0无球 1) HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保爪气缸关闭
{
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) // 检测是否准备好
{ {
if(hand_thread & HANDING_FINISH) xiaomi.position = WAIT_POS;
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
{ {
@ -419,11 +339,9 @@ 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; // 上一次的光电状态
@ -435,8 +353,8 @@ void Ball::ballHadling(void)
case BALL_IDLE: case BALL_IDLE:
if (rc_key == DOWN2) if (rc_key == DOWN2)
{ {
xiaomi.position = HANGDING_POS; // 外伸 xiaomi.position = O_ANGLE; // 外伸
if (feedback->position_deg >= HANGDING_POS - 0.5f) // 确保伸缩电机到位 if (feedback->position_deg >= O_ANGLE - 1) // 确保伸缩电机到位
{ {
currentState1 = BALL_FORWARD; currentState1 = BALL_FORWARD;
} }
@ -481,7 +399,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;

View File

@ -74,10 +74,11 @@ 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;
}; };

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=4.0; double test_distance;
// sw[7]👆 1694 中 1000 👇306 // sw[7]👆 1694 中 1000 👇306
// sw[2]👆 1694 👇306 // sw[2]👆 1694 👇306
@ -29,22 +29,19 @@ double test_distance=4.0;
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 13.0f #define TO_TOP 10.0f
#define TO_BOTTOM 6.0f #define TO_BOTTOM 6.0f
#define INIT_POS -120 #define INIT_POS -130
#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 -10 #define Tigger_DO 0
#define Tigger_ZERO 115 #define Tigger_ZERO 120
#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()
{ {
@ -74,7 +71,6 @@ 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()
@ -86,27 +82,14 @@ 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 1.2143736f * x * x + 14.733786f * x + -133.3627f; return 0.2006334f * x * x + 25.788123f * x + -169.32157 + 3.8f;
} }
float pass_fitting(float x) float pass_fitting(float x)
{ {
return 0.36807548f * x * x + 25.002169f * x + -199.2727f; return 1.1790172f * x * x + 15.983755f * x + -172.04664f + 1.6f;
} }
void Shoot::distanceGet(const NUC_t &nuc_v) void Shoot::distanceGet(const NUC_t &nuc_v)
@ -117,13 +100,8 @@ 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)
@ -238,7 +216,6 @@ void Shoot::rc_mode()
#if ONE_CONTROL == 0 #if ONE_CONTROL == 0
float and1=0.0f;
void Shoot::shoot_control() void Shoot::shoot_control()
{ {
@ -248,10 +225,8 @@ void Shoot::shoot_control()
switch (mode_key) switch (mode_key)
{ {
case VSION: case VSION:
//fire_pos = distance; // 视觉拟合的力 fire_pos = distance; // 视觉拟合的力
fire_pos =shoot_fitting(distance)+and1; // fire_pos =shoot_fitting(test_distance);
//fire_pos = INIT_POS + knob_increment; // 根据旋钮值调整发射位置
switch (rc_key) switch (rc_key)
{ {
case DOWN1: case DOWN1:
@ -291,8 +266,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:
@ -336,7 +311,7 @@ void Shoot::shoot_control()
break; break;
} }
abs_limit_min_max_fp(&go1.Pos, -210.0f, 8.0f); abs_limit_min_max_fp(&go1.Pos, -210.0f, 2.0f);
// 发送数据到蓄力电机 // 发送数据到蓄力电机
GO_SendData(go1.Pos, limit_speed); GO_SendData(go1.Pos, limit_speed);
@ -415,10 +390,6 @@ 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()
{ {
@ -433,9 +404,7 @@ void Shoot::shoot_control()
switch (mode_key) switch (mode_key)
{ {
case VSION: case VSION:
fire_pos = shoot_fitting(distance)+and1; // 视觉拟合的力 fire_pos = shoot_fitting(distance); // 视觉拟合的力
//fire_pos = INIT_POS + knob_increment; // 根据旋钮值调整发射位置
switch (rc_key) switch (rc_key)
{ {
case MIDDLE1: case MIDDLE1:
@ -451,8 +420,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);
} }
} }
@ -465,7 +434,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); // 蓄力标志位
@ -489,7 +458,7 @@ void Shoot::shoot_control()
switch (rc_key) switch (rc_key)
{ {
case MIDDLE1: case MIDDLE1:
fire_pos = pass_fitting(pass_distance)+and2; fire_pos = pass_fitting(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时顶部蓄力流程
@ -504,8 +473,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不做任何蓄力
@ -520,7 +489,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); // 蓄力标志位
@ -546,28 +515,10 @@ 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; // 快速上去
if(feedback.fd_gopos < WAIT_POS +2.0f) t_posSet = Tigger_ZERO; // 扳机松开
{
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
{ {
@ -582,7 +533,6 @@ void Shoot::shoot_control()
trigger_control(); trigger_control();
} }
// 配合运球到发射 // 配合运球到发射
void Shoot ::ball_receive() void Shoot ::ball_receive()
{ {
@ -592,17 +542,11 @@ 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; // 切换到准备发射状态
} }
} }
@ -647,7 +591,7 @@ void Shoot::RemoveError()
control_pos = INIT_POS; control_pos = INIT_POS;
BSP_Buzzer_Stop(); BSP_Buzzer_Stop();
} }
limit_speed = 5.0f; // 慢慢送上去 limit_speed = 3.0f; // 慢慢送上去
go1.Pos = control_pos; go1.Pos = control_pos;
} }

View File

@ -87,7 +87,6 @@ public:
//滤波器 //滤波器
LowPassFilter2p_t distance_filter; // 用于滤波视觉距离 LowPassFilter2p_t distance_filter; // 用于滤波视觉距离
LowPassFilter2p_t pass_filter;
//==========================公共变量========================== //==========================公共变量==========================
int16_t rc_key; //遥控器按键 int16_t rc_key; //遥控器按键
@ -98,7 +97,6 @@ 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,13 +36,10 @@ 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);

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

View File

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