570 lines
17 KiB
C++
570 lines
17 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"
|
||
|
||
|
||
#define HANGDING_TWO 0 // 是否使用两次运球
|
||
|
||
extern RC_ctrl_t rc_ctrl;
|
||
extern int ball_exit;
|
||
|
||
//伸缩
|
||
//外死点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
|
||
|
||
// 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;
|
||
}
|
||
}
|
||
|
||
int runCount = 0; // 运球次数计数
|
||
int last_ball_state = 0; // 上一次的光电状态
|
||
int step = 0; // 0:持球 1:击球 2:反弹 3:完全离开
|
||
|
||
#if HANGDING_TWO == 0
|
||
|
||
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();
|
||
}
|
||
|
||
#else
|
||
|
||
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();
|
||
}
|
||
|
||
#endif
|
||
|
||
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
|