#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 M_SPEED 4000 #define I_ANGLE1 180 #define I_ANGLE2 -73.5 #define O_ANGLE1 340 #define O_ANGLE2 -239 const fp32 Ball:: Extend_speed_PID[3] = { 50, 0.0, 0.1}; const fp32 Ball:: Extend_angle_PID[3]= { 50, 0.3, 0}; //PE11 气缸 Ball ::Ball() { detect_init(); //两个伸缩6020 左207 右208 Extern_Motor[0] = get_motor_point(6); Extern_Motor[1] = get_motor_point(7); Extern_Motor[0]->type = GM6020; Extern_Motor[1]->type = GM6020; PID_init(&extend_speed_pid[0],PID_POSITION,Extend_speed_PID,20000, 2000); PID_init(&extend_angle_pid[0],PID_POSITION,Extend_angle_PID,20000, 1000); PID_init(&extend_speed_pid[1],PID_POSITION,Extend_speed_PID,15000, 5000); PID_init(&extend_angle_pid[1],PID_POSITION,Extend_angle_PID,15000, 5000); e_result[0] = 0; e_result[1] = 0; angleSet[0] = 0; angleSet[1] = 0; //状态机状态初始化 currentState1= BALL_IDLE; } void Ball ::Extend_mcontrol(int angle1,int angle2) { int16_t delta[2]; angleSet[0] = angle1/2; angleSet[1] = angle2/2; fp32 angle_get[2]; angle_get[0] = motor_ecd_to_angle_change(Extern_Motor[0]->ecd, angleSet[0]); angle_get[1] = motor_ecd_to_angle_change(Extern_Motor[1]->ecd, angleSet[1]); delta[0] = PID_calc(&extend_angle_pid[0], angle_get[0] , angleSet[0]); e_result[0] = PID_calc(&extend_speed_pid[0], Extern_Motor[0]->speed_rpm, delta[0]); delta[1] = PID_calc(&extend_angle_pid[1], angle_get[1] , angleSet[1]); e_result[1] = PID_calc(&extend_speed_pid[1], Extern_Motor[1]->speed_rpm, delta[1]); } // E键 sw[1] 👆 200 shoot 中 1000 stop sw[2]👇1800 // G键 sw[6]👆 1800 中 1000 👇200 // sw[5] 👆 200 👇1800 //左旋 sw[7] 200 --1800 void Ball::rc_mode() { if(rc_ctrl.sw[6]==1800) { rc_key=UP2; } if(rc_ctrl.sw[6]==1000) { rc_key=MIDDLE2; } if(rc_ctrl.sw[6]==200) { rc_key=DOWN2; } if(rc_ctrl.sw[5]==200) { extern_key=IN; } if(rc_ctrl.sw[5]==1800) { extern_key=OUT; } } void Ball::Move_Extend(void) { if(extern_key==IN) { Extend_mcontrol(I_ANGLE1, I_ANGLE2); // 内伸 } else if(extern_key==OUT) { Extend_mcontrol(O_ANGLE1, O_ANGLE2); // 外伸 } else { Extend_mcontrol(O_ANGLE1, O_ANGLE2); // 外伸 } } 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::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_cmd_1FF(0,0,e_result[0],e_result[1],&hcan1); 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; } }