#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 key; //伸缩 #define I_ANGLE 0 #define O_ANGLE 140 //PE11 气缸 Ball ::Ball() { detect_init(); //小米电机 feedback=get_CyberGear_point(); //小米电机初始化 xiaomi.position = 0; // xiaomi.speed = 20; // xiaomi.K_P = 100; // xiaomi.K_D =20; // xiaomi.K_C = 2 ; xiaomi.Pmax =1; //状态机状态初始化 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 TESTBALL void Ball::ballDown(void) { HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_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 = 0; } if(extern_key==OUT) { xiaomi.position = 90; } } 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(PAW_GPIO_Port, PAW_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(); break; } Send_control(); } void Ball::Send_control() { CAN_XiaoMi(1,&xiaomi,&hcan2); osDelay(1); } int ball_state = 0; int triggerCount = 0; // 光电传感器触发计数 int last_ball_state = 1; // 上一次的光电状态 //(有球 0,无球 1) void Ball::ballHadling(void) { switch (currentState1) { case BALL_IDLE: HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET); //确保爪气缸关闭 HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭 if (key > 0 ||rc_key == DOWN2) // 检测按键是否被按下 { key = 0; // 重置按键状态 triggerCount = 0; // 重置触发计数 currentState1 = BALL_FORWARD; } break; case BALL_FORWARD: HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_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_state == 0 && last_ball_state == 1) // 检测到状态从无球变为有球 { //osDelay(10); // 延时去抖动 triggerCount++; // 增加触发计数 if (triggerCount == 1) // 第一次触发 { currentState1 = BALL_FLAG; // 切换到等待第二次触发状态 } } last_ball_state = ball_state; // 更新上一次的状态 break; case BALL_FLAG: osDelay(10); // 延时 50ms if (triggerCount == 1 && ball_state == 0 && last_ball_state == 1) // 第二次检测到球 { triggerCount++; // 增加触发计数 currentState1 = BALL_CLOSE; // 切换到闭合气缸状态 } last_ball_state = ball_state; // 更新上一次的状态 break; case BALL_CLOSE: if (triggerCount == 2) // 确保是第二次触发 { osDelay(50); HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET); // 闭合气缸爪子 currentState1 = BALL_FINISH; // 切换到反转状态 } break; case BALL_FINISH: osDelay(50); // 延时 50ms HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET); // 确保气缸爪子闭合 HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭 key = 0; // 重置按键状态 triggerCount = 0; // 重置触发计数 //currentState1 = BALL_IDLE; // 回到空闲状态 break; default: currentState1 = BALL_IDLE; // 默认回到空闲状态 break; } } #endif #if ONE_CONTROL //任务通知来作全过程 void Ball::ballDown(void) { xiaomi.position = I_ANGLE; if(feedback->position_deg== I_ANGLE) // 确保伸缩电机到位 { HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_SET); // 打开气缸爪子 HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭 } if(ball_state==0)//检测到球 { osThreadFlagsSet(task_struct.thread.ball, BALL_DOWN); //osThreadFlagsSet(task_struct.thread.shoot, BALL_DOWN); xiaomi.position = O_ANGLE; } if(hand_thread & BALL_DOWN) { if(feedback->position_deg== O_ANGLE) { osThreadFlagsSet(task_struct.thread.shoot, EXTEND_OK); //告诉发射球出去了 osThreadFlagsSet(task_struct.thread.ball, EXTEND_OK); //告诉发射球出去了 } } } void Ball::Idle_control() { HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET); //确保爪气缸关闭 HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭 if(hand_thread & EXTEND_OK) { xiaomi.position = O_ANGLE; // 伸缩电机伸出去 } else { xiaomi.position = I_ANGLE; // 伸缩电机回位,底盘可能跑的更好 } if (currentState1 == BALL_FINISH) { currentState1 = BALL_IDLE; } else { currentState1 = BALL_IDLE; // 默认回到空闲状态 } } void Ball::ball_control() { hand_thread = osThreadFlagsGet(); // 获取任务通知标志位 ball_state = HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin); // 读取光电状态(有球 1，无球 0) switch (rc_key){ case MIDDLE2: Idle_control(); break; case UP2: ballDown(); break; case DOWN2: ballHadling(); break; } Send_control(); } void Ball::Send_control() { CAN_XiaoMi(1,&xiaomi,&hcan2); osDelay(1); } int ball_state = 0; int triggerCount = 0; // 光电传感器触发计数 int last_ball_state = 1; // 上一次的光电状态 //(有球 0,无球 1) void Ball::ballHadling(void) { switch (currentState1) { case BALL_IDLE: HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET); //确保爪气缸关闭 HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭 if (key > 0 ||rc_key == DOWN2) // 检测按键是否被按下 { key = 0; // 重置按键状态 triggerCount = 0; // 重置触发计数 currentState1 = BALL_FORWARD; } break; case BALL_FORWARD: HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_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_state == 0 && last_ball_state == 1) // 检测到状态从无球变为有球 { //osDelay(10); // 延时去抖动 triggerCount++; // 增加触发计数 if (triggerCount == 1) // 第一次触发 { currentState1 = BALL_FLAG; // 切换到等待第二次触发状态 } } last_ball_state = ball_state; // 更新上一次的状态 break; case BALL_FLAG: osDelay(10); // 延时 50ms if (triggerCount == 1 && ball_state == 0 && last_ball_state == 1) // 第二次检测到球 { triggerCount++; // 增加触发计数 currentState1 = BALL_CLOSE; // 切换到闭合气缸状态 } last_ball_state = ball_state; // 更新上一次的状态 break; case BALL_CLOSE: if (triggerCount == 2) // 确保是第二次触发 { osDelay(50); HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET); // 闭合气缸爪子 currentState1 = BALL_FINISH; // 切换到反转状态 } break; case BALL_FINISH: osDelay(50); // 延时 50ms HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET); // 确保气缸爪子闭合 HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭 key = 0; // 重置按键状态 triggerCount = 0; // 重置触发计数 //currentState1 = BALL_IDLE; // 回到空闲状态 break; default: currentState1 = BALL_IDLE; // 默认回到空闲状态 break; } } #endif