428 lines
10 KiB
C++
428 lines
10 KiB
C++
#include "ball.hpp"
|
||
#include "bsp_delay.h"
|
||
#include "remote_control.h"
|
||
#include "detect.h"
|
||
#include "userTask.h"
|
||
#include "user_math.h"
|
||
#include "shoot.hpp"
|
||
|
||
extern RC_ctrl_t rc_ctrl;
|
||
extern int ball_exit;
|
||
|
||
//伸缩
|
||
#define I_ANGLE 147
|
||
#define O_ANGLE 187
|
||
#define WAIT_POS 170
|
||
|
||
//PE11 气缸
|
||
|
||
Ball ::Ball()
|
||
{
|
||
detect_init();
|
||
|
||
//小米电机
|
||
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 = 0; //
|
||
// xiaomi.speed = 0; //
|
||
// xiaomi.K_P = 0; //
|
||
// xiaomi.K_D =0; //
|
||
// xiaomi.K_C = 0 ;
|
||
// xiaomi.Pmax =0;
|
||
|
||
//状态机状态初始化
|
||
currentState1= BALL_IDLE;
|
||
|
||
}
|
||
|
||
|
||
// E键 sw[1] 👆 200 shoot 中 1000 stop sw[2]👇1800
|
||
// G键 sw[6]👆 200 中 1000 👇1800
|
||
// sw[5] 👆 200 👇1800
|
||
//左旋 sw[7] 200 --1800
|
||
void Ball::rc_mode()
|
||
{
|
||
if(rc_ctrl.sw[6]==200)
|
||
{
|
||
rc_key=UP2;
|
||
}
|
||
if(rc_ctrl.sw[6]==1000)
|
||
{
|
||
rc_key=MIDDLE2;
|
||
}
|
||
if(rc_ctrl.sw[6]==1800)
|
||
{
|
||
rc_key=DOWN2;
|
||
}
|
||
|
||
if(rc_ctrl.sw[5]==200)
|
||
{
|
||
extern_key=IN;
|
||
}
|
||
if(rc_ctrl.sw[5]==1800)
|
||
{
|
||
extern_key=OUT;
|
||
}
|
||
|
||
if(rc_ctrl.sw[5]==1800)
|
||
{
|
||
ready_key=SIDE;
|
||
}
|
||
if(rc_ctrl.sw[5]==200)
|
||
{
|
||
ready_key=0; //默认不准备
|
||
}
|
||
}
|
||
|
||
void Ball::Send_control()
|
||
{
|
||
|
||
CAN_XiaoMi(1,&xiaomi,&hcan2);
|
||
osDelay(1);
|
||
|
||
}
|
||
|
||
#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)
|
||
{
|
||
xiaomi.position = I_ANGLE;
|
||
}
|
||
if(extern_key==OUT)
|
||
{
|
||
xiaomi.position = O_ANGLE;
|
||
}
|
||
}
|
||
|
||
void Ball::ball_control()
|
||
{
|
||
ball_state = HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin); // 读取光电状态(有球 1，无球 0)
|
||
|
||
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); // 确保下气缸关闭
|
||
if (currentState1 == BALL_FINISH)
|
||
{
|
||
currentState1 = BALL_IDLE;
|
||
}
|
||
else {
|
||
currentState1 = BALL_IDLE; // 默认回到空闲状态
|
||
}
|
||
break;
|
||
|
||
case UP2:
|
||
ballDown();
|
||
break;
|
||
|
||
case DOWN2:
|
||
ballHadling();
|
||
//test_Handling();
|
||
|
||
break;
|
||
|
||
}
|
||
|
||
Send_control();
|
||
|
||
}
|
||
|
||
|
||
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) // 检测按键是否被按下
|
||
{
|
||
|
||
|
||
currentState1 = BALL_FORWARD;
|
||
}
|
||
break;
|
||
|
||
case BALL_FORWARD:
|
||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET); // 打开气缸爪子
|
||
osDelay(5);
|
||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_SET); // 打开下气缸
|
||
currentState1 = BALL_DROP; // 切换到球下落状态
|
||
break;
|
||
|
||
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) // 球离开
|
||
{
|
||
currentState1 = BALL_FLAG;
|
||
}
|
||
last_ball_state = ball_state;
|
||
break;
|
||
|
||
case BALL_FLAG:
|
||
osDelay(10); // 延时 50ms
|
||
// 等待球弹回再次检测到球
|
||
if (ball_state == 0 && last_ball_state == 1) // 球弹回
|
||
{
|
||
currentState1 = BALL_CLOSE;
|
||
}
|
||
last_ball_state = ball_state;
|
||
break;
|
||
|
||
case BALL_CLOSE:
|
||
|
||
osDelay(25);
|
||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 闭合气缸爪子
|
||
currentState1 = BALL_FINISH; // 切换到反转状态
|
||
break;
|
||
|
||
case BALL_FINISH:
|
||
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); // 确保下气缸关闭
|
||
|
||
//currentState1 = BALL_IDLE; // 回到空闲状态
|
||
|
||
break;
|
||
|
||
default:
|
||
currentState1 = BALL_IDLE; // 默认回到空闲状态
|
||
break;
|
||
}
|
||
|
||
|
||
}
|
||
|
||
|
||
|
||
#endif
|
||
|
||
#if ONE_CONTROL
|
||
|
||
void Ball::ball_control()
|
||
{
|
||
hand_thread = osThreadFlagsGet(); // 获取任务通知标志位
|
||
ball_state = HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin); // 读取光电状态(有球 1，无球 0)
|
||
|
||
//进攻
|
||
if(ready_key==SIDE)
|
||
{
|
||
switch (rc_key){
|
||
case MIDDLE2:
|
||
Idle_control();
|
||
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); // 确保下气缸关闭
|
||
|
||
}
|
||
|
||
// 发送小米电机控制数据
|
||
Send_control();
|
||
|
||
}
|
||
|
||
|
||
//任务通知来作全过程
|
||
|
||
void Ball::ballDown(void)
|
||
{
|
||
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 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_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_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); // 确保下气缸关闭
|
||
|
||
|
||
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;
|
||
|
||
}
|
||
|
||
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)// 确保伸缩电机到位
|
||
{
|
||
currentState1 = BALL_FORWARD;
|
||
}
|
||
|
||
}
|
||
break;
|
||
|
||
case BALL_FORWARD:
|
||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET); // 打开气缸爪子
|
||
osDelay(5);
|
||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_SET); // 打开下气缸
|
||
currentState1 = BALL_DROP; // 切换到球下落状态
|
||
break;
|
||
|
||
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) // 球离开
|
||
{
|
||
currentState1 = BALL_FLAG;
|
||
}
|
||
last_ball_state = ball_state;
|
||
break;
|
||
|
||
case BALL_FLAG:
|
||
osDelay(10); // 延时 50ms
|
||
// 等待球弹回再次检测到球
|
||
if (ball_state == 0 && last_ball_state == 1) // 球弹回
|
||
{
|
||
currentState1 = BALL_CLOSE;
|
||
}
|
||
last_ball_state = ball_state;
|
||
break;
|
||
|
||
case BALL_CLOSE:
|
||
|
||
osDelay(25);
|
||
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);
|
||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保气缸爪子闭合
|
||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
|
||
break;
|
||
|
||
default:
|
||
currentState1 = BALL_IDLE; // 默认回到空闲状态
|
||
break;
|
||
}
|
||
|
||
|
||
}
|
||
|
||
|
||
#endif
|
||
|