Compare commits
3 Commits
e420cea621
...
4db4582450
| Author | SHA1 | Date | |
|---|---|---|---|
| 4db4582450 | |||
| 4fb6b424a1 | |||
| 048319df42 |
8
MDK-ARM/.vscode/keil-assistant.log
vendored
8
MDK-ARM/.vscode/keil-assistant.log
vendored
@ -74,3 +74,11 @@
|
||||
|
||||
[info] Log at : 2025/6/25|19:23:57|GMT+0800
|
||||
|
||||
[info] Log at : 2025/6/27|11:41:11|GMT+0800
|
||||
|
||||
[info] Log at : 2025/6/27|15:32:00|GMT+0800
|
||||
|
||||
[info] Log at : 2025/6/27|18:43:18|GMT+0800
|
||||
|
||||
[info] Log at : 2025/6/28|01:19:54|GMT+0800
|
||||
|
||||
|
||||
4
MDK-ARM/.vscode/uv4.log
vendored
4
MDK-ARM/.vscode/uv4.log
vendored
@ -1,10 +1,8 @@
|
||||
*** Using Compiler 'V5.06 update 7 (build 960)', folder: 'D:\keil\ARM\ARMCC\Bin'
|
||||
Build target 'R1'
|
||||
compiling shootTask.cpp...
|
||||
compiling shoot.cpp...
|
||||
compiling ball.cpp...
|
||||
linking...
|
||||
Program Size: Code=31644 RO-data=1832 RW-data=268 ZI-data=32252
|
||||
Program Size: Code=31188 RO-data=1832 RW-data=268 ZI-data=32252
|
||||
FromELF: creating hex file...
|
||||
"R1\R1.axf" - 0 Error(s), 0 Warning(s).
|
||||
Build Time Elapsed: 00:00:06
|
||||
|
||||
2
MDK-ARM/.vscode/uv4.log.lock
vendored
2
MDK-ARM/.vscode/uv4.log.lock
vendored
@ -1 +1 @@
|
||||
2025/6/26 3:22:31
|
||||
2025/6/28 1:27:37
|
||||
@ -158,7 +158,7 @@
|
||||
<Ww>
|
||||
<count>0</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>rc_ctrl</ItemText>
|
||||
<ItemText>rc_ctrl,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>1</count>
|
||||
@ -173,17 +173,7 @@
|
||||
<Ww>
|
||||
<count>3</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>ball_exit,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>4</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>nucbuf</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>5</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>nuc_v</ItemText>
|
||||
<ItemText>nuc_v,0x0A</ItemText>
|
||||
</Ww>
|
||||
</WatchWindow1>
|
||||
<MemoryWindow4>
|
||||
@ -952,7 +942,7 @@
|
||||
|
||||
<Group>
|
||||
<GroupName>User/device</GroupName>
|
||||
<tvExp>1</tvExp>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<cbSel>0</cbSel>
|
||||
<RteFlg>0</RteFlg>
|
||||
|
||||
@ -36,8 +36,22 @@ r1上层
|
||||
+ 拨置👇发射清空掉落信号
|
||||
|
||||
+ 用一个攻守方档
|
||||
+ 初始移动到最上面 更待蓄力(不管攻方守方都在最上面等待)
|
||||
+ 攻方时拨下立马蓄力并伸出(可小角度)
|
||||
+ 守方时不动并保持缩回
|
||||
+ 👇 运球档正常运球
|
||||
+ 中 初始档直接缩回
|
||||
+ 👆 配合档 完成配合并伸出才能发射
|
||||
|
||||
+ 传球模式
|
||||
+ 自动
|
||||
+ 底盘的传球对准档拨下
|
||||
+ 我的蓄力进入传球拟合
|
||||
+ 继续拨下发射
|
||||
+ 手动
|
||||
+ 目前只能打固定距离
|
||||
+ 切相同传球档 自动蓄力到传球力度
|
||||
+ 发射档发射
|
||||
+ 图传多距离
|
||||
+ 传球档
|
||||
+ 旋钮+看图传点位调整
|
||||
|
||||
@ -16,10 +16,7 @@
|
||||
#endif
|
||||
|
||||
|
||||
#define ONE_CONTROL 1
|
||||
|
||||
|
||||
|
||||
#define ONE_CONTROL 0
|
||||
|
||||
//是否使用大疆DT7遥控器
|
||||
#ifndef DT7
|
||||
@ -43,6 +40,8 @@
|
||||
#define HANDING_FINISH (1<<2)
|
||||
//伸缩结束
|
||||
#define EXTEND_OK (1<<3)
|
||||
//等待ok
|
||||
#define WAIT_OK (1<<4)
|
||||
|
||||
//要发送ok了
|
||||
#define BALL_SEND (1<<6)
|
||||
|
||||
@ -9,105 +9,101 @@
|
||||
extern RC_ctrl_t rc_ctrl;
|
||||
extern int ball_exit;
|
||||
|
||||
//伸缩
|
||||
// 伸缩
|
||||
#define I_ANGLE 147
|
||||
#define O_ANGLE 187
|
||||
#define WAIT_POS 165
|
||||
#define WAIT_POS 170
|
||||
|
||||
//PE11 气缸
|
||||
// PE11 气缸
|
||||
|
||||
Ball ::Ball()
|
||||
{
|
||||
detect_init();
|
||||
{
|
||||
detect_init();
|
||||
|
||||
//小米电机
|
||||
feedback=get_CyberGear_point();
|
||||
// 小米电机
|
||||
feedback = get_CyberGear_point();
|
||||
|
||||
//小米电机初始化
|
||||
xiaomi.position = I_ANGLE; //
|
||||
xiaomi.speed = 25; //
|
||||
xiaomi.K_P = 80; // 位置增益
|
||||
xiaomi.K_D =20; // 位置阻尼
|
||||
xiaomi.K_C = 12 ; // 力矩
|
||||
xiaomi.Pmax =1; //好像没啥用
|
||||
// 小米电机初始化
|
||||
xiaomi.position = I_ANGLE; //
|
||||
xiaomi.speed = 25; //
|
||||
xiaomi.K_P = 80; // 位置增益
|
||||
xiaomi.K_D = 50; // 位置阻尼
|
||||
xiaomi.K_C = 12; // 力矩
|
||||
xiaomi.Pmax = 1; // 好像没啥用
|
||||
|
||||
// //小米电机初始化
|
||||
// xiaomi.position = 0; //
|
||||
// xiaomi.speed = 0; //
|
||||
// xiaomi.K_P = 0; //
|
||||
// xiaomi.K_D =0; //
|
||||
// xiaomi.K_C = 0 ;
|
||||
// xiaomi.position = 0; //
|
||||
// xiaomi.speed = 0; //
|
||||
// xiaomi.K_P = 0; //
|
||||
// xiaomi.K_D =0; //
|
||||
// xiaomi.K_C = 0 ;
|
||||
// xiaomi.Pmax =0;
|
||||
|
||||
//状态机状态初始化
|
||||
currentState1= BALL_IDLE;
|
||||
|
||||
// 状态机状态初始化
|
||||
currentState1 = BALL_IDLE;
|
||||
}
|
||||
|
||||
|
||||
// E键 sw[1] 👆 200 shoot 中 1000 stop sw[2]👇1800
|
||||
// E键 sw[1] 👆 200 shoot 中 1000 stop sw[2]👇1800
|
||||
// G键 sw[6]👆 200 中 1000 👇1800
|
||||
// sw[5] 👆 200 👇1800
|
||||
//左旋 sw[7] 200 --1800
|
||||
// 左旋 sw[7] 200 --1800
|
||||
void Ball::rc_mode()
|
||||
{
|
||||
if(rc_ctrl.sw[6]==200)
|
||||
if (rc_ctrl.sw[6] == 200)
|
||||
{
|
||||
rc_key=UP2;
|
||||
rc_key = UP2;
|
||||
}
|
||||
if(rc_ctrl.sw[6]==1000)
|
||||
if (rc_ctrl.sw[6] == 1000)
|
||||
{
|
||||
rc_key=MIDDLE2;
|
||||
rc_key = MIDDLE2;
|
||||
}
|
||||
if(rc_ctrl.sw[6]==1800)
|
||||
if (rc_ctrl.sw[6] == 1800)
|
||||
{
|
||||
rc_key=DOWN2;
|
||||
rc_key = DOWN2;
|
||||
}
|
||||
|
||||
if(rc_ctrl.sw[5]==200)
|
||||
if (rc_ctrl.sw[5] == 200)
|
||||
{
|
||||
extern_key=IN;
|
||||
extern_key = IN;
|
||||
}
|
||||
if(rc_ctrl.sw[5]==1800)
|
||||
if (rc_ctrl.sw[5] == 1800)
|
||||
{
|
||||
extern_key=OUT;
|
||||
extern_key = OUT;
|
||||
}
|
||||
|
||||
if(rc_ctrl.sw[5]==1800)
|
||||
if (rc_ctrl.sw[5] == 1800)
|
||||
{
|
||||
ready_key=SIDE;
|
||||
ready_key = SIDE;
|
||||
}
|
||||
if(rc_ctrl.sw[5]==200)
|
||||
if (rc_ctrl.sw[5] == 200)
|
||||
{
|
||||
ready_key=0; //默认不准备
|
||||
ready_key = 0; // 默认不准备
|
||||
}
|
||||
}
|
||||
|
||||
void Ball::Send_control()
|
||||
{
|
||||
|
||||
CAN_XiaoMi(1,&xiaomi,&hcan2);
|
||||
osDelay(1);
|
||||
|
||||
CAN_XiaoMi(1, &xiaomi, &hcan2);
|
||||
osDelay(1);
|
||||
}
|
||||
|
||||
#if ONE_CONTROL==0
|
||||
#if ONE_CONTROL == 0
|
||||
|
||||
void Ball::ballDown(void)
|
||||
{
|
||||
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET); // 打开气缸爪子
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
|
||||
|
||||
}
|
||||
|
||||
void Ball::Move_Extend()
|
||||
{
|
||||
if(extern_key==IN)
|
||||
if (extern_key == IN)
|
||||
{
|
||||
xiaomi.position = I_ANGLE;
|
||||
}
|
||||
if(extern_key==OUT)
|
||||
if (extern_key == OUT)
|
||||
{
|
||||
xiaomi.position = O_ANGLE;
|
||||
}
|
||||
@ -119,64 +115,60 @@ void Ball::ball_control()
|
||||
|
||||
Move_Extend();
|
||||
|
||||
switch (rc_key){
|
||||
case MIDDLE2:
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); //确保爪气缸关闭
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
|
||||
switch (rc_key)
|
||||
{
|
||||
case MIDDLE2:
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保爪气缸关闭
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
|
||||
if (currentState1 == BALL_FINISH)
|
||||
{
|
||||
currentState1 = BALL_IDLE;
|
||||
}
|
||||
else {
|
||||
currentState1 = BALL_IDLE; // 默认回到空闲状态
|
||||
else
|
||||
{
|
||||
currentState1 = BALL_IDLE; // 默认回到空闲状态
|
||||
}
|
||||
break;
|
||||
|
||||
case UP2:
|
||||
case UP2:
|
||||
ballDown();
|
||||
break;
|
||||
|
||||
case DOWN2:
|
||||
case DOWN2:
|
||||
ballHadling();
|
||||
//test_Handling();
|
||||
|
||||
break;
|
||||
|
||||
}
|
||||
|
||||
Send_control();
|
||||
|
||||
Send_control();
|
||||
}
|
||||
|
||||
|
||||
int ball_state = 0;
|
||||
int last_ball_state = 0; // 上一次的光电状态
|
||||
int ball_state = 0;
|
||||
int last_ball_state = 0; // 上一次的光电状态
|
||||
|
||||
void Ball::ballHadling(void)
|
||||
{
|
||||
|
||||
|
||||
switch (currentState1)
|
||||
{
|
||||
case BALL_IDLE:
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); //确保爪气缸关闭
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
|
||||
if (rc_key == DOWN2) // 检测按键是否被按下
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保爪气缸关闭
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
|
||||
if (rc_key == DOWN2) // 检测按键是否被按下
|
||||
{
|
||||
|
||||
|
||||
currentState1 = BALL_FORWARD;
|
||||
currentState1 = BALL_FORWARD;
|
||||
}
|
||||
break;
|
||||
|
||||
case BALL_FORWARD:
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET); // 打开气缸爪子
|
||||
osDelay(5);
|
||||
osDelay(5);
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_SET); // 打开下气缸
|
||||
currentState1 = BALL_DROP; // 切换到球下落状态
|
||||
currentState1 = BALL_DROP; // 切换到球下落状态
|
||||
break;
|
||||
|
||||
case BALL_DROP:
|
||||
osDelay(100); // 延时 100ms
|
||||
case BALL_DROP:
|
||||
osDelay(100); // 延时 100ms
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 关闭下气缸
|
||||
// 一直检测有球（ball_state == 0），等球离开
|
||||
if (ball_state == 1 && last_ball_state == 0) // 球离开
|
||||
@ -186,7 +178,7 @@ void Ball::ballHadling(void)
|
||||
last_ball_state = ball_state;
|
||||
break;
|
||||
|
||||
case BALL_FLAG:
|
||||
case BALL_FLAG:
|
||||
osDelay(10); // 延时 50ms
|
||||
// 等待球弹回再次检测到球
|
||||
if (ball_state == 0 && last_ball_state == 1) // 球弹回
|
||||
@ -197,18 +189,17 @@ void Ball::ballHadling(void)
|
||||
break;
|
||||
|
||||
case BALL_CLOSE:
|
||||
|
||||
osDelay(25);
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 闭合气缸爪子
|
||||
currentState1 = BALL_FINISH; // 切换到反转状态
|
||||
osDelay(100);
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 闭合气缸爪子
|
||||
currentState1 = BALL_FINISH; // 切换到反转状态
|
||||
break;
|
||||
|
||||
case BALL_FINISH:
|
||||
osDelay(50); // 延时 50ms
|
||||
osDelay(50); // 延时 50ms
|
||||
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); // 确保下气缸关闭
|
||||
|
||||
//currentState1 = BALL_IDLE; // 回到空闲状态
|
||||
// currentState1 = BALL_IDLE; // 回到空闲状态
|
||||
|
||||
break;
|
||||
|
||||
@ -216,11 +207,7 @@ void Ball::ballHadling(void)
|
||||
currentState1 = BALL_IDLE; // 默认回到空闲状态
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
@ -228,156 +215,155 @@ void Ball::ballHadling(void)
|
||||
|
||||
void Ball::ball_control()
|
||||
{
|
||||
hand_thread = osThreadFlagsGet(); // 获取任务通知标志位
|
||||
hand_thread = osThreadFlagsGet(); // 获取任务通知标志位
|
||||
ball_state = HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin); // 读取光电状态(有球 1，无球 0)
|
||||
|
||||
//进攻
|
||||
if(ready_key==SIDE)
|
||||
// 进攻
|
||||
if (ready_key == SIDE)
|
||||
{
|
||||
switch (rc_key){
|
||||
switch (rc_key)
|
||||
{
|
||||
case MIDDLE2:
|
||||
Idle_control();
|
||||
break;
|
||||
Idle_control();
|
||||
break;
|
||||
|
||||
case UP2:
|
||||
ballDown();
|
||||
break;
|
||||
|
||||
case DOWN2:
|
||||
ballHadling();
|
||||
break;
|
||||
case UP2:
|
||||
ballDown();
|
||||
break;
|
||||
|
||||
case DOWN2:
|
||||
ballHadling();
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
//防守
|
||||
// 防守
|
||||
else
|
||||
{
|
||||
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); // 确保下气缸关闭
|
||||
|
||||
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); // 确保下气缸关闭
|
||||
}
|
||||
|
||||
|
||||
|
||||
Send_control();
|
||||
|
||||
// 发送小米电机控制数据
|
||||
Send_control();
|
||||
}
|
||||
|
||||
|
||||
//任务通知来作全过程
|
||||
// 任务通知来作全过程
|
||||
|
||||
void Ball::ballDown(void)
|
||||
{
|
||||
switch(currentState1)
|
||||
switch (currentState1)
|
||||
{
|
||||
case BALL_IDLE:
|
||||
xiaomi.position = I_ANGLE; // 保持收回
|
||||
if(feedback->position_deg >= I_ANGLE-0.8 && feedback->position_deg <= I_ANGLE+0.8)
|
||||
{
|
||||
currentState1 = EXTEND_DOWN;
|
||||
}
|
||||
break;
|
||||
case BALL_IDLE:
|
||||
xiaomi.position = I_ANGLE; // 保持收回
|
||||
if (feedback->position_deg >= I_ANGLE - 0.8 && feedback->position_deg <= I_ANGLE + 0.8)
|
||||
{
|
||||
currentState1 = EXTEND_DOWN;
|
||||
}
|
||||
break;
|
||||
|
||||
case EXTEND_DOWN:
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET);
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);
|
||||
// 检测到球自由下落一次就切换状态
|
||||
if(ball_state == 1)
|
||||
{
|
||||
currentState1 = EXTEND_OUT;
|
||||
}
|
||||
break;
|
||||
case EXTEND_DOWN:
|
||||
osDelay(500); // 不放太快
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET);
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);
|
||||
// 检测到球自由下落一次就切换状态
|
||||
if (ball_state == 1)
|
||||
{
|
||||
currentState1 = EXTEND_OUT;
|
||||
}
|
||||
break;
|
||||
|
||||
case EXTEND_OUT:
|
||||
xiaomi.position = O_ANGLE; // 保持伸出
|
||||
if(feedback->position_deg >= O_ANGLE-0.2 && feedback->position_deg <= O_ANGLE+0.2)
|
||||
{
|
||||
osThreadFlagsSet(task_struct.thread.shoot, EXTEND_OK);
|
||||
currentState1 = EXTEND_FINISH; // 保持伸出，直到拨杆复位
|
||||
}
|
||||
break;
|
||||
case EXTEND_OUT:
|
||||
xiaomi.position = O_ANGLE; // 保持伸出
|
||||
if (feedback->position_deg >= O_ANGLE - 0.2 && feedback->position_deg <= O_ANGLE + 0.2)
|
||||
{
|
||||
|
||||
case EXTEND_FINISH:
|
||||
xiaomi.position = O_ANGLE; // 一直保持伸出
|
||||
// 等待拨杆复位（如切到MIDDLE2），Idle_control会负责回位
|
||||
break;
|
||||
osThreadFlagsSet(task_struct.thread.shoot, EXTEND_OK);
|
||||
currentState1 = EXTEND_FINISH; // 保持伸出，直到拨杆复位
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
currentState1 = BALL_IDLE;
|
||||
break;
|
||||
case EXTEND_FINISH:
|
||||
xiaomi.position = O_ANGLE; // 一直保持伸出
|
||||
// 等待拨杆复位（如切到MIDDLE2），Idle_control会负责回位
|
||||
break;
|
||||
|
||||
default:
|
||||
currentState1 = BALL_IDLE;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
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); // 确保下气缸关闭
|
||||
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) // 检测是否准备好
|
||||
if (ready_key == SIDE) // 检测是否准备好
|
||||
{
|
||||
xiaomi.position = WAIT_POS;
|
||||
if(feedback->position_deg >= WAIT_POS-3)
|
||||
xiaomi.position = WAIT_POS;
|
||||
if (feedback->position_deg >= WAIT_POS - 3)
|
||||
{
|
||||
osThreadFlagsSet(task_struct.thread.shoot, READY_TELL);
|
||||
// 只在READY_TELL未置位时发送，防止重复
|
||||
if ((osThreadFlagsGet() & READY_TELL) == 0)
|
||||
{
|
||||
osThreadFlagsSet(task_struct.thread.shoot, READY_TELL);
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
xiaomi.position = I_ANGLE; // 默认回到收回位置
|
||||
}
|
||||
|
||||
|
||||
// 拨杆回到中间挡位时，回位并重置状态机
|
||||
if(currentState1==EXTEND_FINISH)//转移后
|
||||
if (currentState1 == EXTEND_FINISH) // 转移后
|
||||
{
|
||||
xiaomi.position = I_ANGLE;
|
||||
currentState1 = BALL_IDLE;
|
||||
xiaomi.position = I_ANGLE;
|
||||
currentState1 = BALL_IDLE;
|
||||
}
|
||||
if(currentState1==BALL_FINISH)//运球完成
|
||||
if (currentState1 == BALL_FINISH) // 运球完成
|
||||
{
|
||||
xiaomi.position = O_ANGLE;
|
||||
currentState1 = BALL_IDLE;
|
||||
xiaomi.position = O_ANGLE;
|
||||
currentState1 = BALL_IDLE;
|
||||
}
|
||||
else
|
||||
{
|
||||
currentState1 = BALL_IDLE;
|
||||
currentState1 = BALL_IDLE;
|
||||
}
|
||||
//xiaomi.position = I_ANGLE;
|
||||
|
||||
// xiaomi.position = I_ANGLE;
|
||||
}
|
||||
|
||||
int ball_state = 0;
|
||||
int last_ball_state = 0; // 上一次的光电状态
|
||||
int ball_state = 0;
|
||||
int last_ball_state = 0; // 上一次的光电状态
|
||||
|
||||
void Ball::ballHadling(void)
|
||||
{
|
||||
|
||||
|
||||
switch (currentState1)
|
||||
{
|
||||
case BALL_IDLE:
|
||||
if (rc_key == DOWN2)
|
||||
{
|
||||
xiaomi.position = O_ANGLE;//外伸
|
||||
if(feedback->position_deg>= O_ANGLE-1)// 确保伸缩电机到位
|
||||
{
|
||||
xiaomi.position = O_ANGLE; // 外伸
|
||||
if (feedback->position_deg >= O_ANGLE - 1) // 确保伸缩电机到位
|
||||
{
|
||||
currentState1 = BALL_FORWARD;
|
||||
currentState1 = BALL_FORWARD;
|
||||
}
|
||||
|
||||
}
|
||||
break;
|
||||
|
||||
case BALL_FORWARD:
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET); // 打开气缸爪子
|
||||
osDelay(5);
|
||||
osDelay(5);
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_SET); // 打开下气缸
|
||||
currentState1 = BALL_DROP; // 切换到球下落状态
|
||||
currentState1 = BALL_DROP; // 切换到球下落状态
|
||||
break;
|
||||
|
||||
case BALL_DROP:
|
||||
osDelay(100); // 延时 100ms
|
||||
case BALL_DROP:
|
||||
osDelay(100); //不要动这里
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 关闭下气缸
|
||||
// 一直检测有球（ball_state == 0），等球离开
|
||||
if (ball_state == 1 && last_ball_state == 0) // 球离开
|
||||
@ -387,7 +373,7 @@ void Ball::ballHadling(void)
|
||||
last_ball_state = ball_state;
|
||||
break;
|
||||
|
||||
case BALL_FLAG:
|
||||
case BALL_FLAG:
|
||||
osDelay(10); // 延时 50ms
|
||||
// 等待球弹回再次检测到球
|
||||
if (ball_state == 0 && last_ball_state == 1) // 球弹回
|
||||
@ -399,26 +385,22 @@ void Ball::ballHadling(void)
|
||||
|
||||
case BALL_CLOSE:
|
||||
|
||||
osDelay(25);
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 闭合气缸爪子
|
||||
currentState1 = BALL_FINISH; // 切换到反转状态
|
||||
osDelay(100);
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 闭合气缸爪子
|
||||
currentState1 = BALL_FINISH; // 切换到反转状态
|
||||
break;
|
||||
|
||||
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); // 确保下气缸关闭
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
|
||||
break;
|
||||
|
||||
default:
|
||||
currentState1 = BALL_IDLE; // 默认回到空闲状态
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
@ -1,5 +1,6 @@
|
||||
#include "TopDefine.h"
|
||||
#include "shoot.hpp"
|
||||
#include "userTask.h"
|
||||
#include "main.h"
|
||||
#include "remote_control.h"
|
||||
#include "FreeRTOS.h"
|
||||
@ -12,17 +13,17 @@
|
||||
extern RC_ctrl_t rc_ctrl;
|
||||
NUC_t nuc_v;
|
||||
|
||||
//sw[7]👆 1694 中 1000 👇306
|
||||
//sw[2]👆 1694 👇306
|
||||
// sw[7]👆 1694 中 1000 👇306
|
||||
// sw[2]👆 1694 👇306
|
||||
|
||||
//E键 sw[1]👆 200 shoot 中 1000 stop 👇1800 error
|
||||
//F键 sw[0]👆 1800 开 👇200 关
|
||||
//B键 sw[3]👆 200 开 👇1800 关
|
||||
//左旋钮 sw[4] 👈 200 👉1800
|
||||
// E键 sw[1]👆 200 shoot 中 1000 stop 👇1800 error
|
||||
// F键 sw[0]👆 1800 开 👇200 关
|
||||
// B键 sw[3]👆 200 开 👇1800 关
|
||||
// 左旋钮 sw[4] 👈 200 👉1800
|
||||
|
||||
//尽量别动这组pid
|
||||
const fp32 Shoot:: M2006_speed_PID[3] = {5, 0, 0.01};
|
||||
const fp32 Shoot:: M2006_angle_PID[3] = { 15, 0.1, 0};
|
||||
// 尽量别动这组pid
|
||||
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_BOTTOM 5.0f
|
||||
@ -30,70 +31,64 @@ const fp32 Shoot:: M2006_angle_PID[3] = { 15, 0.1, 0};
|
||||
#define INIT_POS -130
|
||||
#define TOP_POS -211
|
||||
#define BOTTOM_POS 0
|
||||
#define GO_ERROR 1.0f
|
||||
#define Tigger_DO 0
|
||||
#define Tigger_ZERO 100
|
||||
#define GO_ERROR 1.0f
|
||||
#define Tigger_DO 0
|
||||
#define Tigger_ZERO 100
|
||||
#define Tigger_ERROR 3
|
||||
|
||||
|
||||
float knob_increment;
|
||||
|
||||
Shoot::Shoot()
|
||||
{
|
||||
//扳机初始化
|
||||
trigger_Motor= get_motor_point(0);//id 201
|
||||
trigger_Motor->type=M2006;
|
||||
PID_init(&speed_pid,PID_POSITION,M2006_speed_PID,5000, 2000);//pid初始化
|
||||
PID_init(&angle_pid,PID_POSITION,M2006_angle_PID,5000, 2000);//pid初始化
|
||||
t_speedSet = 0;
|
||||
// 扳机初始化
|
||||
trigger_Motor = get_motor_point(0); // id 201
|
||||
trigger_Motor->type = M2006;
|
||||
PID_init(&speed_pid, PID_POSITION, M2006_speed_PID, 5000, 2000); // pid初始化
|
||||
PID_init(&angle_pid, PID_POSITION, M2006_angle_PID, 5000, 2000); // pid初始化
|
||||
t_speedSet = 0;
|
||||
result = 0;
|
||||
|
||||
|
||||
go1.id =1,
|
||||
go1.id = 1,
|
||||
go1.mode = 1,
|
||||
go1.K_P = 1.0f,
|
||||
go1.K_W = 0.05,
|
||||
go1.Pos = 0,//上电先到一个舒服的位置
|
||||
go1.W = 0,
|
||||
go1.Pos = 0, // 上电先到一个舒服的位置
|
||||
go1.W = 0,
|
||||
go1.T = 0,
|
||||
|
||||
limit_speed=TO_TOP;//快速上去
|
||||
fire_pos = INIT_POS; // 发射位置
|
||||
limit_speed = TO_TOP; // 快速上去
|
||||
fire_pos = INIT_POS; // 发射位置
|
||||
|
||||
BSP_UART_RegisterCallback(BSP_UART_RS485, BSP_UART_RX_CPLT_CB, USART1_RxCompleteCallback);//注册串口回调函数，bsp层
|
||||
BSP_UART_RegisterCallback(BSP_UART_RS485, BSP_UART_RX_CPLT_CB, USART1_RxCompleteCallback); // 注册串口回调函数，bsp层
|
||||
|
||||
go1_data = get_GO_measure_point();//go1数据
|
||||
go1_data = get_GO_measure_point(); // go1数据
|
||||
|
||||
currentState= SHOOT_IDLE;
|
||||
|
||||
LowPassFilter2p_Init(&distance_filter,500.0f,80.0f); //给distance 做滤波
|
||||
currentState = SHOOT_IDLE;
|
||||
|
||||
LowPassFilter2p_Init(&distance_filter, 500.0f, 80.0f); // 给distance 做滤波
|
||||
}
|
||||
|
||||
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
|
||||
delta = PID_calc(&angle_pid, trigger_Motor->total_angle, t_posSet); // 计算位置环PID
|
||||
result = PID_calc(&speed_pid, trigger_Motor->speed_rpm, delta); // 计算速度环PID
|
||||
|
||||
|
||||
CAN_cmd_200(result,0,0,0,&hcan1);
|
||||
CAN_cmd_200(result, 0, 0, 0, &hcan1);
|
||||
}
|
||||
|
||||
|
||||
void Shoot :: distanceGet(const NUC_t &nuc_v)
|
||||
void Shoot ::distanceGet(const NUC_t &nuc_v)
|
||||
{
|
||||
|
||||
distance= LowPassFilter2p_Apply(&distance_filter, nuc_v.vision.x); // 对视觉距离进行滤波处理
|
||||
//distance=nuc_v.vision.x; // 获取自瞄数据
|
||||
|
||||
distance = LowPassFilter2p_Apply(&distance_filter, nuc_v.vision.x); // 对视觉距离进行滤波处理
|
||||
// distance=nuc_v.vision.x; // 获取自瞄数据
|
||||
}
|
||||
|
||||
int Shoot::GO_SendData(float pos,float limit)
|
||||
int Shoot::GO_SendData(float pos, float limit)
|
||||
{
|
||||
|
||||
|
||||
//// static int is_calibration=0;
|
||||
//// static fp32 error=0; //误差量
|
||||
//// static fp32 error=0; //误差量
|
||||
|
||||
// 读取参数
|
||||
// float tff = go1.T; // 前馈力矩
|
||||
@ -101,20 +96,25 @@ int Shoot::GO_SendData(float pos,float limit)
|
||||
// float kd = go1.K_W; // 速度阻尼
|
||||
// float q_desired = go1.Pos; // 期望位置（rad）
|
||||
float q_current = go1_data->Pos; // 当前角度位置（rad）
|
||||
// float dq_desired = go1.W; // 期望角速度（rad/s）
|
||||
// float dq_current = go1_data->W; // 当前角速度（rad/s）
|
||||
// float dq_desired = go1.W; // 期望角速度（rad/s）
|
||||
// float dq_current = go1_data->W; // 当前角速度（rad/s）
|
||||
|
||||
// 计算输出力矩 tau
|
||||
//// float tau = tff + kp * (q_desired - q_current) + kd * (dq_desired - dq_current);
|
||||
|
||||
/*限制最大输入来限制最大输出*/
|
||||
if (pos - q_current > limit) {
|
||||
go1.Pos = q_current + limit; // 限制位置
|
||||
}else if (pos - q_current < -limit) {
|
||||
go1.Pos = q_current - limit; // 限制位置
|
||||
}else {
|
||||
go1.Pos = pos; // 允许位置
|
||||
}
|
||||
/*限制最大输入来限制最大输出*/
|
||||
if (pos - q_current > limit)
|
||||
{
|
||||
go1.Pos = q_current + limit; // 限制位置
|
||||
}
|
||||
else if (pos - q_current < -limit)
|
||||
{
|
||||
go1.Pos = q_current - limit; // 限制位置
|
||||
}
|
||||
else
|
||||
{
|
||||
go1.Pos = pos; // 允许位置
|
||||
}
|
||||
|
||||
// 发送数据
|
||||
GO_M8010_send_data(&go1);
|
||||
@ -122,45 +122,45 @@ int Shoot::GO_SendData(float pos,float limit)
|
||||
return 0;
|
||||
}
|
||||
|
||||
// F键 sw[0]👆 1800 👇200
|
||||
// E键 sw[1] 👆 200 shoot 中 1000 stop sw[2]200 sw[2]👇1800
|
||||
// G键 sw[6]👆 1800 中 1000 👇200
|
||||
// F键 sw[0]👆 1800 👇200
|
||||
// E键 sw[1] 👆 200 shoot 中 1000 stop sw[2]200 sw[2]👇1800
|
||||
// G键 sw[6]👆 200 中 1000 👇1800
|
||||
// B键 sw[3]👆 200 开 中 1000 👇1800 关
|
||||
// sw[5] 👆 200 👇1800
|
||||
//左旋 sw[7] 200 --1800
|
||||
// 左旋 sw[7] 200 --1800
|
||||
void Shoot::rc_mode()
|
||||
{
|
||||
if(rc_ctrl.sw[1]==200)
|
||||
if (rc_ctrl.sw[1] == 200)
|
||||
{
|
||||
rc_key=UP1;
|
||||
rc_key = UP1;
|
||||
}
|
||||
if(rc_ctrl.sw[1]==1800 && rc_ctrl.sw[2]==200)
|
||||
if (rc_ctrl.sw[1] == 1800 && rc_ctrl.sw[2] == 200)
|
||||
{
|
||||
rc_key=MIDDLE1;
|
||||
rc_key = MIDDLE1;
|
||||
}
|
||||
if(rc_ctrl.sw[2]==1800 && rc_ctrl.sw[1]==1800)
|
||||
if (rc_ctrl.sw[2] == 1800 && rc_ctrl.sw[1] == 1800)
|
||||
{
|
||||
rc_key=DOWN1;
|
||||
rc_key = DOWN1;
|
||||
}
|
||||
if(rc_ctrl.sw[0]==1800)
|
||||
if (rc_ctrl.sw[0] == 1800)
|
||||
{
|
||||
mode_key=TEST;
|
||||
mode_key = TEST;
|
||||
}
|
||||
if(rc_ctrl.sw[0]==200)
|
||||
if (rc_ctrl.sw[0] == 200)
|
||||
{
|
||||
mode_key=VSION;
|
||||
mode_key = VSION;
|
||||
}
|
||||
// if(rc_ctrl.sw[5]==200)
|
||||
// {
|
||||
// mode_key=OUT;
|
||||
// mode_key=OUT;
|
||||
// }
|
||||
if(rc_ctrl.sw[5]==1800)
|
||||
if (rc_ctrl.sw[5] == 1800)
|
||||
{
|
||||
ready_key=OFFENSIVE;
|
||||
ready_key = OFFENSIVE;
|
||||
}
|
||||
else if(rc_ctrl.sw[5]==200)
|
||||
else if (rc_ctrl.sw[5] == 200)
|
||||
{
|
||||
ready_key=DEFENSE;
|
||||
ready_key = DEFENSE;
|
||||
}
|
||||
|
||||
// //旋钮增量
|
||||
@ -175,303 +175,122 @@ void Shoot::rc_mode()
|
||||
// }
|
||||
|
||||
// 旋钮物理范围
|
||||
const int knob_min = 200;
|
||||
const int knob_max = 1800;
|
||||
const int knob_min = 200;
|
||||
const int knob_max = 1800;
|
||||
|
||||
// 目标映射范围
|
||||
const float map_min = 130.0f;
|
||||
const float map_max = -60.0f;
|
||||
// 目标映射范围
|
||||
const float map_min = 130.0f;
|
||||
const float map_max = -60.0f;
|
||||
|
||||
int current_knob_value = rc_ctrl.sw[7];
|
||||
|
||||
// 限制范围
|
||||
if (current_knob_value < knob_min) current_knob_value = knob_min;
|
||||
if (current_knob_value > knob_max) current_knob_value = knob_max;
|
||||
|
||||
// 线性映射
|
||||
knob_increment = map_min + (map_max - map_min) * (current_knob_value - knob_min) / (knob_max - knob_min);
|
||||
int current_knob_value = rc_ctrl.sw[7];
|
||||
|
||||
// 限制范围
|
||||
if (current_knob_value < knob_min)
|
||||
current_knob_value = knob_min;
|
||||
if (current_knob_value > knob_max)
|
||||
current_knob_value = knob_max;
|
||||
|
||||
// 线性映射
|
||||
knob_increment = map_min + (map_max - map_min) * (current_knob_value - knob_min) / (knob_max - knob_min);
|
||||
}
|
||||
|
||||
#if ONE_CONTROL == 0
|
||||
|
||||
|
||||
#if ONE_CONTROL==0
|
||||
|
||||
void Shoot::shoot_control() {
|
||||
|
||||
//方便调试
|
||||
feedback.fd_gopos = go1_data->Pos;
|
||||
feedback.fd_tpos = trigger_Motor->total_angle;
|
||||
switch (mode_key)
|
||||
{
|
||||
case VSION:
|
||||
fire_pos = distance; // 视觉拟合的力
|
||||
switch (rc_key) {
|
||||
case DOWN1:
|
||||
|
||||
control_pos = INIT_POS; // 默认中间挡位位置
|
||||
go1.Pos = fire_pos; // 设置蓄力电机位置
|
||||
//t_posSet = Tigger_ZERO; // 扳机松开
|
||||
|
||||
if (currentState == SHOOT_READY) {
|
||||
// 如果当前状态是准备发射，松开钩子发射
|
||||
t_posSet = Tigger_ZERO; // 扳机扣动
|
||||
BSP_Buzzer_Stop();
|
||||
if (t_posSet >= Tigger_ZERO-20) { // 假设120度为发射完成角度
|
||||
currentState = SHOOT_IDLE; // 切换到空闲状态
|
||||
is_hooked = false; // 重置钩子状态
|
||||
}
|
||||
} else {
|
||||
currentState = SHOOT_IDLE; // 默认回到空闲状态
|
||||
}
|
||||
break;
|
||||
|
||||
case MIDDLE1:
|
||||
shoot_Current();
|
||||
break;
|
||||
|
||||
case UP1:
|
||||
RemoveError();
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
break;
|
||||
case TEST:
|
||||
//实时可调蓄力位置
|
||||
fire_pos = INIT_POS + knob_increment; // 根据旋钮值调整发射位置
|
||||
|
||||
switch (rc_key) {
|
||||
case DOWN1:
|
||||
|
||||
control_pos = INIT_POS; // 默认中间挡位位置
|
||||
go1.Pos = fire_pos; // 设置蓄力电机位置
|
||||
//t_posSet = Tigger_ZERO; // 扳机松开
|
||||
|
||||
if (currentState == SHOOT_READY) {
|
||||
// 如果当前状态是准备发射，松开钩子发射
|
||||
t_posSet = Tigger_ZERO; // 扳机扣动
|
||||
BSP_Buzzer_Stop();
|
||||
if (t_posSet >= Tigger_ZERO-20) { // 假设120度为发射完成角度
|
||||
currentState = SHOOT_IDLE; // 切换到空闲状态
|
||||
is_hooked = false; // 重置钩子状态
|
||||
}
|
||||
} else {
|
||||
currentState = SHOOT_IDLE; // 默认回到空闲状态
|
||||
}
|
||||
break;
|
||||
|
||||
case MIDDLE1:
|
||||
shoot_Current();
|
||||
break;
|
||||
|
||||
case UP1:
|
||||
RemoveError();
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
|
||||
abs_limit_min_max_fp(&go1.Pos ,-210.0f,2.0f);
|
||||
// 发送数据到蓄力电机
|
||||
GO_SendData(go1.Pos,limit_speed);
|
||||
|
||||
// 控制扳机电机
|
||||
trigger_control();
|
||||
}
|
||||
|
||||
void Shoot :: shoot_Current()
|
||||
void Shoot::shoot_control()
|
||||
{
|
||||
|
||||
switch (currentState) {
|
||||
case SHOOT_IDLE:
|
||||
// 初始状态：钩子移动到顶部，钩住拉簧
|
||||
control_pos = TOP_POS; // 顶部位置
|
||||
limit_speed=TO_TOP;//快速上去
|
||||
go1.Pos = control_pos;
|
||||
t_posSet = Tigger_ZERO; // 扳机松开
|
||||
if (feedback.fd_gopos <-209) {
|
||||
currentState = SHOOT_TOP; // 切换到准备发射状态
|
||||
}
|
||||
break;
|
||||
// 方便调试
|
||||
feedback.fd_gopos = go1_data->Pos;
|
||||
feedback.fd_tpos = trigger_Motor->total_angle;
|
||||
switch (mode_key)
|
||||
{
|
||||
case VSION:
|
||||
fire_pos = distance; // 视觉拟合的力
|
||||
switch (rc_key)
|
||||
{
|
||||
case DOWN1:
|
||||
|
||||
case SHOOT_TOP:
|
||||
t_posSet = Tigger_DO; // 扳机扣动钩住
|
||||
if (trigger_Motor->total_angle<Tigger_DO+1.0f && trigger_Motor->total_angle>Tigger_DO-1.0f)
|
||||
{
|
||||
//判定为钩住
|
||||
is_hooked = true; // 标记钩子已钩住
|
||||
currentState = SHOOT_READY; // 切换到准备发射状态
|
||||
}
|
||||
break;
|
||||
control_pos = INIT_POS; // 默认中间挡位位置
|
||||
go1.Pos = fire_pos; // 设置蓄力电机位置
|
||||
// t_posSet = Tigger_ZERO; // 扳机松开
|
||||
|
||||
case SHOOT_READY:
|
||||
if (is_hooked)
|
||||
if (currentState == SHOOT_READY)
|
||||
{
|
||||
go1.Pos = fire_pos; // 下拉到中间挡位位置
|
||||
limit_speed=TO_BOTTOM;//慢速下来
|
||||
if (feedback.fd_gopos >= fire_pos - GO_ERROR && feedback.fd_gopos <= fire_pos + GO_ERROR) {
|
||||
BSP_Buzzer_Start();
|
||||
BSP_Buzzer_Set(1,5000);
|
||||
//currentState = SHOOT_WAIT; // 等待发射信号
|
||||
//在拨杆切换时触发了
|
||||
// 如果当前状态是准备发射，松开钩子发射
|
||||
t_posSet = Tigger_ZERO; // 扳机扣动
|
||||
BSP_Buzzer_Stop();
|
||||
if (t_posSet >= Tigger_ZERO - 20)
|
||||
{ // 假设120度为发射完成角度
|
||||
currentState = SHOOT_IDLE; // 切换到空闲状态
|
||||
is_hooked = false; // 重置钩子状态
|
||||
}
|
||||
}
|
||||
break;
|
||||
else
|
||||
{
|
||||
currentState = SHOOT_IDLE; // 默认回到空闲状态
|
||||
}
|
||||
break;
|
||||
|
||||
case MIDDLE1:
|
||||
shoot_Current();
|
||||
break;
|
||||
|
||||
case UP1:
|
||||
RemoveError();
|
||||
break;
|
||||
|
||||
default:
|
||||
currentState = SHOOT_IDLE; // 默认回到空闲状态
|
||||
break;
|
||||
}
|
||||
}
|
||||
break;
|
||||
case TEST:
|
||||
// 实时可调蓄力位置
|
||||
fire_pos = INIT_POS + knob_increment; // 根据旋钮值调整发射位置
|
||||
|
||||
void Shoot::RemoveError() {
|
||||
currentState = SHOOT_IDLE;
|
||||
control_pos = TOP_POS+1.0f;
|
||||
if (feedback.fd_gopos >= control_pos - 0.5f&& feedback.fd_gopos<= control_pos + 0.5f)
|
||||
{
|
||||
t_posSet = Tigger_ZERO;
|
||||
is_hooked = false;
|
||||
}
|
||||
if(trigger_Motor->total_angle>=Tigger_ZERO-10)
|
||||
{
|
||||
//认为完全松开
|
||||
control_pos=INIT_POS;
|
||||
BSP_Buzzer_Stop();
|
||||
|
||||
}
|
||||
limit_speed=3.0f;//慢慢送上去
|
||||
go1.Pos = control_pos;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
#if ONE_CONTROL
|
||||
|
||||
|
||||
void Shoot::shoot_control() {
|
||||
|
||||
//方便调试 反馈信息
|
||||
feedback.fd_gopos = go1_data->Pos;
|
||||
feedback.fd_tpos = trigger_Motor->total_angle;
|
||||
|
||||
|
||||
shoot_thread = osThreadFlagsGet(); // 获取任务通知标志位
|
||||
|
||||
if(ready_key==OFFENSIVE){
|
||||
switch (mode_key)
|
||||
{
|
||||
case VSION:
|
||||
fire_pos = distance; // 视觉拟合的力
|
||||
switch (rc_key) {
|
||||
case MIDDLE1:
|
||||
ball_receive(); // 执行接收球的操作
|
||||
|
||||
if(shoot_thread & EXTEND_OK) // 如果收到伸缩完成的通知
|
||||
switch (rc_key)
|
||||
{
|
||||
go1.Pos = fire_pos; // 设置蓄力电机位置
|
||||
if(feedback.fd_gopos >= fire_pos - GO_ERROR && feedback.fd_gopos <= fire_pos + GO_ERROR)
|
||||
case DOWN1:
|
||||
|
||||
control_pos = INIT_POS; // 默认中间挡位位置
|
||||
go1.Pos = fire_pos; // 设置蓄力电机位置
|
||||
// t_posSet = Tigger_ZERO; // 扳机松开
|
||||
|
||||
if (currentState == SHOOT_READY)
|
||||
{
|
||||
BSP_Buzzer_Start();
|
||||
BSP_Buzzer_Set(1,5000);
|
||||
// 如果当前状态是准备发射，松开钩子发射
|
||||
t_posSet = Tigger_ZERO; // 扳机扣动
|
||||
BSP_Buzzer_Stop();
|
||||
if (t_posSet >= Tigger_ZERO - 20)
|
||||
{ // 假设120度为发射完成角度
|
||||
currentState = SHOOT_IDLE; // 切换到空闲状态
|
||||
is_hooked = false; // 重置钩子状态
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case DOWN1:
|
||||
if(shoot_thread & EXTEND_OK)
|
||||
{
|
||||
BSP_Buzzer_Stop();
|
||||
t_posSet = Tigger_ZERO; // 扳机扣动 射出
|
||||
currentState = SHOOT_IDLE; // 默认回到空闲状态
|
||||
osThreadFlagsClear(EXTEND_OK);
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case UP1:
|
||||
RemoveError();
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
break;
|
||||
|
||||
//无自瞄拟合测试档
|
||||
case TEST:
|
||||
switch (rc_key) {
|
||||
case MIDDLE1:
|
||||
|
||||
if(shoot_thread & EXTEND_OK) // 如果收到伸缩完成的通知
|
||||
{
|
||||
fire_pos = INIT_POS + knob_increment; // 根据旋钮值调整发射位置
|
||||
go1.Pos = fire_pos; // 设置蓄力电机位置
|
||||
if(feedback.fd_gopos >= fire_pos - GO_ERROR && feedback.fd_gopos <= fire_pos + GO_ERROR)
|
||||
else
|
||||
{
|
||||
BSP_Buzzer_Start();
|
||||
BSP_Buzzer_Set(1,5000);
|
||||
currentState = SHOOT_IDLE; // 默认回到空闲状态
|
||||
}
|
||||
}
|
||||
if(shoot_thread & READY_TELL) // 如果收到等待配合的通知
|
||||
{
|
||||
ball_receive(); // 执行接收球的操作
|
||||
}
|
||||
else
|
||||
{
|
||||
go1.Pos = TOP_POS-3.0f; //-208
|
||||
limit_speed=TO_TOP;//快速上去
|
||||
t_posSet = Tigger_ZERO; // 扳机松开
|
||||
}
|
||||
break;
|
||||
break;
|
||||
|
||||
case DOWN1:
|
||||
if(shoot_thread & EXTEND_OK)
|
||||
{
|
||||
BSP_Buzzer_Stop();
|
||||
t_posSet = Tigger_ZERO; // 扳机扣动 射出
|
||||
currentState = SHOOT_IDLE; // 默认回到空闲状态
|
||||
osThreadFlagsClear(EXTEND_OK);
|
||||
}
|
||||
case MIDDLE1:
|
||||
shoot_Current();
|
||||
break;
|
||||
|
||||
break;
|
||||
case UP1:
|
||||
RemoveError();
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
case UP1:
|
||||
RemoveError();
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
else if(ready_key==DEFENSE)//攻守档总控
|
||||
{
|
||||
go1.Pos = TOP_POS-3.0f; //-208
|
||||
limit_speed=TO_TOP;//快速上去
|
||||
t_posSet = Tigger_ZERO; // 扳机松开
|
||||
|
||||
}
|
||||
|
||||
abs_limit_min_max_fp(&go1.Pos ,-210.0f,2.0f);
|
||||
abs_limit_min_max_fp(&go1.Pos, -210.0f, 2.0f);
|
||||
// 发送数据到蓄力电机
|
||||
GO_SendData(go1.Pos, limit_speed);
|
||||
|
||||
@ -479,64 +298,270 @@ else if(ready_key==DEFENSE)//攻守档总控
|
||||
trigger_control();
|
||||
}
|
||||
|
||||
//配合运球到发射
|
||||
void Shoot ::shoot_Current()
|
||||
{
|
||||
|
||||
switch (currentState)
|
||||
{
|
||||
case SHOOT_IDLE:
|
||||
// 初始状态：钩子移动到顶部，钩住拉簧
|
||||
control_pos = TOP_POS; // 顶部位置
|
||||
limit_speed = TO_TOP; // 快速上去
|
||||
go1.Pos = control_pos;
|
||||
t_posSet = Tigger_ZERO; // 扳机松开
|
||||
if (feedback.fd_gopos < -209)
|
||||
{
|
||||
currentState = SHOOT_TOP; // 切换到准备发射状态
|
||||
}
|
||||
break;
|
||||
|
||||
case SHOOT_TOP:
|
||||
t_posSet = Tigger_DO; // 扳机扣动钩住
|
||||
if (trigger_Motor->total_angle < Tigger_DO + 1.0f && trigger_Motor->total_angle > Tigger_DO - 1.0f)
|
||||
{
|
||||
// 判定为钩住
|
||||
is_hooked = true; // 标记钩子已钩住
|
||||
currentState = SHOOT_READY; // 切换到准备发射状态
|
||||
}
|
||||
break;
|
||||
|
||||
case SHOOT_READY:
|
||||
if (is_hooked)
|
||||
{
|
||||
go1.Pos = fire_pos; // 下拉到中间挡位位置
|
||||
limit_speed = TO_BOTTOM; // 慢速下来
|
||||
if (feedback.fd_gopos >= fire_pos - GO_ERROR && feedback.fd_gopos <= fire_pos + GO_ERROR)
|
||||
{
|
||||
BSP_Buzzer_Start();
|
||||
BSP_Buzzer_Set(1, 5000);
|
||||
// currentState = SHOOT_WAIT; // 等待发射信号
|
||||
// 在拨杆切换时触发了
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
currentState = SHOOT_IDLE; // 默认回到空闲状态
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void Shoot::RemoveError()
|
||||
{
|
||||
currentState = SHOOT_IDLE;
|
||||
control_pos = TOP_POS + 1.0f;
|
||||
if (feedback.fd_gopos >= control_pos - 0.5f && feedback.fd_gopos <= control_pos + 0.5f)
|
||||
{
|
||||
t_posSet = Tigger_ZERO;
|
||||
is_hooked = false;
|
||||
}
|
||||
if (trigger_Motor->total_angle >= Tigger_ZERO - 10)
|
||||
{
|
||||
// 认为完全松开
|
||||
control_pos = INIT_POS;
|
||||
BSP_Buzzer_Stop();
|
||||
}
|
||||
limit_speed = 3.0f; // 慢慢送上去
|
||||
go1.Pos = control_pos;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
#if ONE_CONTROL
|
||||
|
||||
int asd = 0;
|
||||
|
||||
void Shoot::shoot_control()
|
||||
{
|
||||
|
||||
// 方便调试 反馈信息
|
||||
feedback.fd_gopos = go1_data->Pos;
|
||||
feedback.fd_tpos = trigger_Motor->total_angle;
|
||||
|
||||
shoot_thread = osThreadFlagsGet(); // 获取任务通知标志位
|
||||
|
||||
if (ready_key == OFFENSIVE)
|
||||
{
|
||||
switch (mode_key)
|
||||
{
|
||||
case VSION:
|
||||
fire_pos = distance; // 视觉拟合的力
|
||||
switch (rc_key)
|
||||
{
|
||||
case MIDDLE1:
|
||||
if ((shoot_thread & READY_TELL) && !(shoot_thread & EXTEND_OK))
|
||||
{
|
||||
// 只收到READY_TELL且未收到EXTEND_OK时，顶部蓄力流程
|
||||
ball_receive();
|
||||
}
|
||||
else if (shoot_thread & EXTEND_OK)
|
||||
{
|
||||
go1.Pos = fire_pos;
|
||||
limit_speed = TO_BOTTOM;
|
||||
if (feedback.fd_gopos >= fire_pos - GO_ERROR && feedback.fd_gopos <= fire_pos + GO_ERROR)
|
||||
{
|
||||
BSP_Buzzer_Start();
|
||||
BSP_Buzzer_Set(1, 5000);
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case DOWN1:
|
||||
if (shoot_thread & EXTEND_OK)
|
||||
{
|
||||
t_posSet = Tigger_ZERO;
|
||||
if (feedback.fd_tpos >= Tigger_ZERO - 20)
|
||||
{
|
||||
BSP_Buzzer_Stop();
|
||||
currentState = SHOOT_IDLE;
|
||||
osThreadFlagsClear(EXTEND_OK);
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case UP1:
|
||||
RemoveError();
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
// 无自瞄拟合测试档
|
||||
case TEST:
|
||||
switch (rc_key)
|
||||
{
|
||||
case MIDDLE1:
|
||||
if ((shoot_thread & READY_TELL) && !(shoot_thread & EXTEND_OK))
|
||||
{
|
||||
// 只收到READY_TELL且未收到EXTEND_OK时，顶部蓄力流程
|
||||
ball_receive(); // ball_receive内部写go1.Pos
|
||||
}
|
||||
else if (shoot_thread & EXTEND_OK)
|
||||
{
|
||||
// 只收到EXTEND_OK时，允许调节蓄力位置
|
||||
fire_pos = INIT_POS + knob_increment;
|
||||
go1.Pos = fire_pos;
|
||||
limit_speed = TO_BOTTOM;
|
||||
if (feedback.fd_gopos >= fire_pos - GO_ERROR && feedback.fd_gopos <= fire_pos + GO_ERROR)
|
||||
{
|
||||
BSP_Buzzer_Start();
|
||||
BSP_Buzzer_Set(1, 5000);
|
||||
}
|
||||
}
|
||||
// 没收到READY_TELL不做任何蓄力
|
||||
break;
|
||||
|
||||
case DOWN1:
|
||||
if (shoot_thread & EXTEND_OK)
|
||||
{
|
||||
t_posSet = Tigger_ZERO;
|
||||
if (feedback.fd_tpos >= Tigger_ZERO - 20)
|
||||
{
|
||||
BSP_Buzzer_Stop();
|
||||
currentState = SHOOT_IDLE;
|
||||
osThreadFlagsClear(EXTEND_OK);
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case UP1:
|
||||
RemoveError();
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
else if (ready_key == DEFENSE)
|
||||
{
|
||||
control_pos = TOP_POS - 2.0f; //-209
|
||||
go1.Pos = control_pos;
|
||||
limit_speed = TO_TOP; // 快速上去
|
||||
t_posSet = Tigger_ZERO; // 扳机松开
|
||||
}
|
||||
else
|
||||
{
|
||||
t_posSet = Tigger_DO; // 防止debug时重复
|
||||
}
|
||||
|
||||
abs_limit_min_max_fp(&go1.Pos, -210.0f, 2.0f);
|
||||
// 发送数据到蓄力电机
|
||||
GO_SendData(go1.Pos, limit_speed);
|
||||
|
||||
// 控制扳机电机
|
||||
trigger_control();
|
||||
}
|
||||
|
||||
// 配合运球到发射
|
||||
void Shoot ::ball_receive()
|
||||
{
|
||||
switch (currentState)
|
||||
switch (currentState)
|
||||
{
|
||||
case SHOOT_IDLE:
|
||||
// 初始状态：钩子移动到顶部，钩住拉簧
|
||||
control_pos = TOP_POS;
|
||||
limit_speed=TO_TOP;//快速上去
|
||||
case SHOOT_IDLE:
|
||||
// 初始状态：钩子移动到顶部，钩住拉簧
|
||||
if (shoot_thread & READY_TELL) // 如果收到等待通知
|
||||
{
|
||||
control_pos = TOP_POS;
|
||||
limit_speed = TO_TOP; // 快速上去
|
||||
t_posSet = Tigger_ZERO;
|
||||
if(feedback.fd_gopos <-209)
|
||||
if (feedback.fd_gopos < -209)
|
||||
{
|
||||
currentState = GO_TOP; // 切换到准备发射状态
|
||||
}
|
||||
break;
|
||||
}
|
||||
break;
|
||||
|
||||
case GO_TOP:
|
||||
t_posSet = Tigger_DO;
|
||||
if (trigger_Motor->total_angle<Tigger_DO+1.0f && trigger_Motor->total_angle>Tigger_DO-1.0f)
|
||||
{
|
||||
|
||||
currentState = BAKC; // 切换到准备发射状态
|
||||
}
|
||||
break;
|
||||
case BAKC:
|
||||
control_pos = BOTTOM_POS;
|
||||
limit_speed=TO_BOTTOM;//慢速下来
|
||||
if(feedback.fd_gopos >= BOTTOM_POS - 1.0f && feedback.fd_gopos <= BOTTOM_POS + 1.0f)
|
||||
{
|
||||
case GO_TOP:
|
||||
t_posSet = Tigger_DO;
|
||||
if (trigger_Motor->total_angle < Tigger_DO + 1.0f && trigger_Motor->total_angle > Tigger_DO - 1.0f)
|
||||
{
|
||||
|
||||
osThreadFlagsClear(READY_TELL); // 清除任务通知标志位
|
||||
}
|
||||
|
||||
break;
|
||||
currentState = BAKC; // 切换到准备发射状态
|
||||
}
|
||||
break;
|
||||
case BAKC:
|
||||
control_pos = BOTTOM_POS;
|
||||
limit_speed = TO_BOTTOM; // 慢速下来
|
||||
if (feedback.fd_gopos >= BOTTOM_POS - 1.0f && feedback.fd_gopos <= BOTTOM_POS + 1.0f)
|
||||
{
|
||||
|
||||
osThreadFlagsClear(READY_TELL); // 清除任务通知标志位
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
//调整go电机位置
|
||||
go1.Pos = control_pos;
|
||||
|
||||
// 调整go电机位置
|
||||
go1.Pos = control_pos;
|
||||
}
|
||||
|
||||
void Shoot::RemoveError() {
|
||||
void Shoot::RemoveError()
|
||||
{
|
||||
currentState = SHOOT_IDLE;
|
||||
control_pos = TOP_POS;
|
||||
if (feedback.fd_gopos >= control_pos - 0.5f&& feedback.fd_gopos<= control_pos + 0.5f)
|
||||
if (feedback.fd_gopos < -209)
|
||||
{
|
||||
t_posSet = Tigger_ZERO;
|
||||
is_hooked = false;
|
||||
t_posSet = Tigger_ZERO;
|
||||
is_hooked = false;
|
||||
}
|
||||
if(trigger_Motor->total_angle>=Tigger_ZERO-5)
|
||||
if (trigger_Motor->total_angle >= Tigger_ZERO - 5)
|
||||
{
|
||||
//认为完全松开
|
||||
control_pos=INIT_POS;
|
||||
BSP_Buzzer_Stop();
|
||||
|
||||
// 认为完全松开
|
||||
control_pos = INIT_POS;
|
||||
BSP_Buzzer_Stop();
|
||||
}
|
||||
limit_speed=3.0f;//慢慢送上去
|
||||
go1.Pos = control_pos;
|
||||
limit_speed = 3.0f; // 慢慢送上去
|
||||
go1.Pos = control_pos;
|
||||
}
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
@ -18,7 +18,8 @@ typedef enum {
|
||||
SHOOT_RETURN, // 自动返回状态
|
||||
//运射配合
|
||||
GO_TOP,
|
||||
BAKC
|
||||
BAKC,
|
||||
WAIT_CHANGE
|
||||
} ShootState_t;
|
||||
|
||||
// // 定义状态枚举
|
||||
|
||||
@ -17,7 +17,7 @@ void FunctionShoot(void *argument)
|
||||
(void)argument; /* 未使用argument，消除警告 */
|
||||
|
||||
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CTRL_SHOOT;
|
||||
|
||||
osDelay(3000);//等待M2006电机启动
|
||||
uint32_t tick = osKernelGetTickCount();
|
||||
|
||||
while(1)
|
||||
|
||||
Loading…
Reference in New Issue
Block a user