R1_up/User/module/ball.cpp

625 lines
18 KiB
C++
Raw Blame History

This file contains ambiguous Unicode characters

This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.

#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 165
//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
// H键 sw[6] 👆200 👇1800
//左旋 sw[7] 200 --1800
int last_sw5 = 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] == 200 && last_sw5 == 1800) // 只在1800->200的瞬间触发
// {
// rc_key = DOWN2;
// }
// else
// {
// rc_key = 0; // 其他情况不触发
// }
// last_sw5 = rc_ctrl.sw[5];
}
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); // 读取光电状态(有球 0无球 1)
// //ball_state = HAL_GPIO_ReadPin(TEST_GPIO_Port, TEST_Pin); // 读取光电状态(有球 0无球 1)
// 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();
// //Close();
// 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 DOWN2:
// ballHadling();
// break;
// }
// Send_control();
// }
//int ball_state = 0;
int runCount = 0; // 运球次数计数
int last_ball_state = 0; // 上一次的光电状态
int step = 0; // 0:持球 1:击球 2:反弹 3:完全离开
void Ball::ball_control()
{
ball_state = HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin); // 读取光电状态(有球 0无球 1)
Move_Extend();
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) // 检测按键是否被按下
{
runCount = 0; // 每次拨动重新计数
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(100);
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 闭合气缸爪子
currentState1 = BALL_FINISH; // 切换到反转状态
break;
case BALL_FINISH:
osDelay(200); // 延时 50ms
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保气缸爪子闭合
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
runCount++;
osDelay(200); // 延时 50ms
if (runCount < 2)
{
currentState1 = BALL_FORWARD; // 继续下一次运球
osDelay(50);
}
else
{
currentState1 = BALL_IDLE; // 完成两次,回到空闲
}
break;
default:
currentState1 = BALL_IDLE; // 默认回到空闲状态
break;
}
Send_control();
}
// void Ball::ball_control()
// {
// ball_state = HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin); // 读取光电状态(有球 0无球 1)
// Move_Extend();
// 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(100);
// 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;
// }
// Send_control();
// }
void Ball::Close(void)
{
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; // 回到空闲状态
}
void Ball::Hadling(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)
{
ball_exit = 0; // 进入流程时清零
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); // 关闭下气缸
// 只响应第一次边沿
if (ball_exit == 1)
{
currentState1 = BALL_FLAG;
}
break;
}
}
void Ball::exit_Handling(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)
{
ball_exit = 0; // 进入流程时清零
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); // 关闭下气缸
// 只响应第一次边沿
if (ball_exit == 1)
{
currentState1 = BALL_FLAG;
}
break;
case BALL_FLAG:
//osDelay(10); // 延时 10ms
// 只响应第二次边沿
if (ball_exit == 3)
{
currentState1 = BALL_CLOSE;
}
break;
case BALL_CLOSE:
osDelay(25);
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 闭合气缸爪子
currentState1 = BALL_FINISH; // 切换到反转状态
break;
case BALL_FINISH:
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保气缸爪子闭合
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
ball_exit = 0; // 流程完成后清零
break;
default:
currentState1 = BALL_IDLE; // 默认回到空闲状态
break;
}
}
void Ball::test_up(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) // 检测按键是否被按下
{
step = 0;
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(50); // 延时 100ms
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 关闭下气缸
// 步骤0等待球离开持球->无球)
if (step == 0 && ball_state == 1 && last_ball_state == 0)
{
step = 1;
}
// 步骤1等待球反弹回来无球->持球)
else if (step == 1 && ball_state == 0 && last_ball_state == 1)
{
step = 2;
}
// 步骤2等待球完全上升离开持球->无球)
else if (step == 2 && ball_state == 1 && last_ball_state == 0)
{
step = 3;
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;
}
}
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(100);
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;
}
}
void Ball::ballHadling_two(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)
{
runCount = 0; // 每次拨动重新计数
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);
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 关闭下气缸
if (ball_state == 1 && last_ball_state == 0) // 球离开
{
currentState1 = BALL_FLAG;
}
last_ball_state = ball_state;
break;
case BALL_FLAG:
osDelay(10);
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);
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保气缸爪子闭合
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
runCount++;
if (runCount < 2)
{
currentState1 = BALL_FORWARD; // 继续下一次运球
}
else
{
currentState1 = BALL_IDLE; // 完成两次,回到空闲
}
break;
default:
currentState1 = BALL_IDLE;
break;
}
}
#endif