deepseek/wrobot
1
0 Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

更新踢球方案

Browse Source
This commit is contained in:
myperworld committed 2026-01-24 20:21:02 +08:00
1 parent 72455a5e76
commit 1a74f5d323
3 files changed
+206 -109
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

v1.4 摄像头提乒乓球方案/platformio/src/SerialParser.cpp
+16
View File
@@ -132,6 +132,11 @@ void parseSingleParam(char* cmd) {
} else if (strcmp(param_name, "pid_trace_joyy_d") == 0) {
pid_trace_joyy.D = param_val;
} else if (strcmp(param_name, "pid_yaw_align_p") == 0) {
pid_yaw_align.P = param_val;
} else if (strcmp(param_name, "pid_yaw_align_d") == 0) {
pid_yaw_align.D = param_val;
} else if (strcmp(param_name, "pid_cam_p") == 0) {
pid_cam_p = param_val;
@@ -149,6 +154,17 @@ void parseSingleParam(char* cmd) {
run_kick_ball_dy = param_val;
} else if (strcmp(param_name, "ob_ball_dy") == 0) {
ob_ball_dy = param_val;
} else if (strcmp(param_name, "ready_kick_angel") == 0) {
ready_kick_angel = param_val;
} else if (strcmp(param_name, "ready_leg_height_base") == 0) {
ready_leg_height_base = param_val;
} else if (strcmp(param_name, "yaw_align_threshold") == 0) {
yaw_align_threshold = param_val;
} else if (strcmp(param_name, "kick_back_s2") == 0) {
kick_back_s2 = param_val;
} else if (strcmp(param_name, "yaw_error_coff") == 0) {
yaw_error_coff = param_val;
} else if (strcmp(param_name, "kick_over_count_max") == 0) {
v1.4 摄像头提乒乓球方案/platformio/src/SerialParser.h
+13 -7
View File
@@ -16,15 +16,22 @@ extern float dragon_joyx_factor;
extern PIDController pid_trace_joyx;
extern PIDController pid_trace_joyy;
extern PIDController pid_yaw_align;
extern float pid_cam_p;
extern float pid_cam_d;
extern int chase_ball_s2;
extern int kick_back_s2;
extern int ob_ball_dy;
extern int place_ball_s2;
extern int place_kick_ball_dy;
extern int run_kick_ball_dy;
extern int kick_over_count_max;
extern int yaw_align_threshold;
extern int ready_kick_angel;
extern int ready_leg_height_base;
extern float yaw_error_coff;
extern float leg_height_base;
@@ -67,22 +74,21 @@ void kick_ball_loop();
void SetKickMode(String mode);
void kick_ball_main(int dx, int dy);
// void chase_ball(int dy);
void chase_and_kick_ball(int dy);
void place_kick_ball(int dy);
void kick_ball_core();
void ready_kick_ball();
void aim_ob ( int dx, int dy, String ob);
void aim_ball(int dx, int dy);
void aim_face ( int dx, int dy);
void chase_ball(int dy);
void align_yaw_zero();
void joyx_aim_ball(int dx);
void cam_aim_ball(int dy);
void setCarmeraFixServoAngles(int servo_angle);
void setFrontierServoAngles(int servo_angle);
void setServoAngles(int s1, int s2);
void cancel_trace_mode();
void cancel_kick_mode();
void SerialToMaixcam(String cat, String str);
v1.4 摄像头提乒乓球方案/platformio/src/main.cpp
+177 -102
View File
@@ -277,7 +277,6 @@ Servo cam_fix_servo;
// 追踪模式
bool trace_mode = false; // 追踪模式整体标志
String kick_mode = ""; // 原地踢球模式kick_place_mode 追逐踢球模式kick_run_mode
String kick_status = ""; // aim_ball aim_face aim_ball_over aim_face_over kick_ball_over
bool trace_ob_flag = false; // 是否追踪到位
int dx_to_joyx = 9999;
@@ -293,10 +292,14 @@ int place_kick_ball_dy = -10;
// 追逐球，并踢球
int chase_ball_s2 = 30; // 20
int run_kick_ball_dy = -5; // 20
int kick_back_s2 = 40; // 20
int run_kick_ball_dy = -5;
int ob_ball_dy = 120;
int kick_over_count = 0; // 运行踢球后计数
int kick_over_count_max = 150; // 运行踢球后计数最大值
int kick_over_count_max = 50; // 运行踢球后计数最大值
float yaw_error_coff = 1.0f; // 偏航角误差系数
int ready_kick_angel = 100; // 准备踢球角度
int ready_leg_height_base = 22.0f; // 准备踢球高度
// 摄像头舵机
int current_s1 = 0; // 前档舵机实时角度
@@ -309,6 +312,13 @@ int pid_trace_joyy_output_limit = 10; // PD输出限幅（单次最大调整角
int last_dy = 0; // 上一次的y轴偏差
unsigned long last_dy_time = 0; // 上一次计算偏差的时间戳
// 踢球模式偏航角校准相关
float kick_yaw_zero = 0.0f; // kick_run_mode下的偏航基准角（0度）
int yaw_align_threshold = 10; // 偏航角对齐阈值
bool chased_flag = false; // 已经追到球了
bool align_flag = false; // 已经对齐偏航角了
int yaw_to_joyx = 9999; // 偏航对齐专用的joyx控制值（9999表示无效）
PIDController pid_yaw_align(0.8, 0, 0.2, 100000, 100); // 偏航对齐专用PID
void setup()
{
@@ -628,8 +638,8 @@ void lqr_balance_loop()
gyro_control = pid_gyro(LQR_gyro);
if(kick_mode == "kick_run_mode"){
// 踢球模式
if(trace_mode){
if(dy_to_joyy!= 9999){
wrobot.joyy = dy_to_joyy;
}else{
@@ -992,6 +1002,8 @@ void yaw_loop()
if(trace_mode){
if(dx_to_joyx!= 9999){
wrobot.joyx = dx_to_joyx;
}else if(yaw_to_joyx != 9999){
wrobot.joyx = yaw_to_joyx;
}else{
wrobot.joyx = 0;
}
@@ -1081,6 +1093,8 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) {
sitting_down = 1; // 标记开始坐下动作保存当前的角度零点
pid_trace_joyx.P = 0.5; // 追踪模式
cancel_kick_mode();
}
// 第二次按坐下，让舵机失能
if (remoteData.btnLB && !last_btnLB && !robot_enabled) {
@@ -1279,6 +1293,14 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) {
wrobot.jump = 1;
forward_jump_flag = 1;
}
// 追逐并转向踢会球模式 @@@@@@
if (remoteData.btnY && !last_btnY && robot_enabled && trace_mode) {
if(kick_mode != "kick_back_mode"){
SetKickMode("kick_back_mode");
}else if(kick_mode == "kick_back_mode"){
SetKickMode("kick_place_mode");
}
}
last_btnY = remoteData.btnY;
// 向后跳跃（非踢球模式）
@@ -1288,7 +1310,7 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) {
}
// 追逐踢球模式 @@@@@@
if (remoteData.btnA && !last_btnA && robot_enabled && trace_mode) {
if(kick_mode == "kick_place_mode"){
if(kick_mode != "kick_run_mode"){
SetKickMode("kick_run_mode");
}else if(kick_mode == "kick_run_mode"){
SetKickMode("kick_place_mode");
@@ -1323,7 +1345,7 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) {
if(trace_mode){
SetKickMode("kick_place_mode"); // 原地踢球模式
}else {
cancel_trace_mode();
cancel_kick_mode();
}
}
@@ -1548,14 +1570,15 @@ void kick_ball_loop(){
if(trace_mode){
if (kick_mode == "kick_run_mode") { // 运动追踪模式
setLEDColor(CRGB::Blue);
}else {
}else if(kick_mode == "kick_back_mode") { // 追逐并转向踢会球模式
setLEDColor(CRGB::Red);
}else if(kick_mode == "kick_place_mode"){ // 原地踢球模式
setLEDColor(CRGB::Green);
}
}
// 动态调整高度
}else{
// 非踢球模式下，动态调整高度
if(wrobot.joyy == 0 ){
if(current_s1 > 90){
if(current_s1 > 90){ // 打开挡板
leg_height_base = 30.0f;
}else if(current_s1 > 10){
leg_height_base = constrain(current_s1, 15.0f, 30.0f);
@@ -1563,19 +1586,20 @@ void kick_ball_loop(){
leg_height_base = original_leg_height_base;
}
}
// 在运动踢球时，踢好球，需要有一段停止前进时间
if(kick_over_count > 0 && kick_over_count < kick_over_count_max){
kick_over_count ++;
}else{
kick_over_count = 0;
}
}
// 通过偏差deviation 控制摄像头上下左右运动
void kick_ball_main(int dx, int dy) {
// 在运动踢球时，踢好球，需要有一段停止前进时间
// if(kick_over_count > 0 && kick_over_count < kick_over_count_max){
// kick_over_count ++;
// }else{
// kick_over_count = 0;
// }
// Serial.println("kick_over_count: " + String(kick_over_count) );
// 不是追踪模式
if(!trace_mode){
dx_to_joyx = 9999; // 不左右旋转
@@ -1590,65 +1614,22 @@ void kick_ball_main(int dx, int dy) {
if( trace_mode && (dx == 9999 or dy == 9999)){
dx_to_joyx = 9999; // 不左右旋转
dy_to_joyy = 9999; // 不前后追逐球
trace_ob_flag = false;
last_dy = 0; // 重置相机舵机PID控制器状态
last_dy_time = 0;
setCarmeraFixServoAngles(60);
setFrontierServoAngles(0);
return;
setCarmeraFixServoAngles(60); // 摄像头低头
setFrontierServoAngles(0); // 关闭挡板
// return;
}
// Serial.println("s2: " + String(current_s2) + " dy: " + String(dy));
// 原地踢球模式
if( kick_mode == "kick_place_mode"){
aim_ball(dx,dy);
place_kick_ball(dy);
}
// 运动踢球模式
if(kick_mode == "kick_run_mode"){
aim_ball(dx,dy);
if(kick_over_count){ // 踢好球之后阶段，后退
dy_to_joyy = -50;
}else{
chase_and_kick_ball(dy);
}
}
}
// 追逐球，并踢球
void chase_and_kick_ball(int dy){
// 物体在中间 dy=0
// 物体在上方 dy趋近-100
// 物体在下方 dy趋近 100
// 已经追到球了
if(current_s2 < chase_ball_s2 && dy > run_kick_ball_dy && dy < 120){
dy_to_joyy = 9999;
kick_ball_core(); // 踢球
kick_over_count = 1; // 计数用于后退
}else{ // 通过pid 向前运动
ob_y = ob_ball_dy - dy;
dy_to_joyy = pid_trace_joyy(ob_y);
ready_kick_ball(); // 准备踢球
}
}
// 原地踢球模式的踢球
void place_kick_ball(int dy){
cam_aim_ball(dy); // 相机瞄准
joyx_aim_ball(dx); // 控制左右 使小车正面是否朝向物体
if(current_s2 < place_ball_s2 && dy > place_kick_ball_dy && dy < 120){ // 球到车身下方了，打球
kick_ball_core();
}else { // 追踪到目标
@@ -1656,6 +1637,87 @@ void place_kick_ball(int dy){
}
}
// 运动踢球模式
if(kick_mode == "kick_run_mode"){
cam_aim_ball(dy); // 相机瞄准
if(kick_over_count){ // 踢好球之后阶段
if(kick_over_count > 0 && kick_over_count < kick_over_count_max){
kick_over_count ++;
}else{
kick_over_count = 0;
yaw_to_joyx = 9999; // 不旋转偏航角
}
if(!align_flag){
align_yaw_zero(); // 对齐偏航角
}else{
dy_to_joyy = -50; // 后退
}
}else{
align_flag = false; // 重置对齐标志
joyx_aim_ball(dx); // 控制左右 使小车正面是否朝向物体
if(chased_flag){
if(kick_over_count == 0){
dy_to_joyy = 9999;
kick_ball_core(); // 踢球
kick_over_count = 1; // 计数用于后退
chased_flag = false;
}
}else{
chase_ball(dy);
}
}
}
}
void align_yaw_zero(){
// 获取当前偏航角 & 计算与基准角的偏差
// 小车正对人，kick_yaw_zero 为基准角
// 向右（以人为标准），current_yaw变大（变正数），yaw_error 变小负数
// 向左（以人为标准），current_yaw变小（变负数），yaw_error 变大正数
float current_yaw = mpu6050.getAngleZ();
float yaw_error = kick_yaw_zero - current_yaw; // 基准角 - 当前角 = 需修正的偏差
// 串口测试，写在一行
// Serial.println( "Zero:" + String(kick_yaw_zero) + ",Current:" + String(current_yaw) + ",Error:" + String(yaw_error));
if(abs(yaw_error) < yaw_align_threshold){
Serial.println("aligned");
// 偏航角已对齐：更新状态为“已对齐”
yaw_to_joyx = 9999; // 重置偏航对齐控制值
align_flag = true; // 已经对齐偏航角了
pid_yaw_align.error_prev = 0.0f; // 重置偏航对齐控制器状态
} else {
// Serial.println("aligning");
// 未对齐，通过专用PID计算yaw_to_joyx控制转向
int joyx_direction = (yaw_error > 0) ? -1 : 1; // 偏差为正：需要右转；偏差为负：需要左转
yaw_to_joyx = constrain(pid_yaw_align(abs(yaw_error * yaw_error_coff)) * joyx_direction, -100, 100);
// dy_to_joyy = 9999; // 对齐期间停止前进
align_flag = false;
}
}
// 追逐球，并转向，踢回球
void chase_ball(int dy){
// 物体在中间 dy=0, 物体在上方 dy趋近-100, 物体在下方 dy趋近 100
// 已经追到球了
if(current_s2 < chase_ball_s2 && dy > run_kick_ball_dy && dy < 120){
chased_flag = true;
// Serial.println("chased");
}else{ // 未追到球，继续追逐
ob_y = ob_ball_dy - dy;
dy_to_joyy = pid_trace_joyy(ob_y);
ready_kick_ball(); // 准备踢球
// Serial.println("chasing");
}
}
// 踢足球核心函数，控制前档舵机角度为0，使足球进入射门区域
void kick_ball_core(){
setFrontierServoAngles(0); // 踢球
@@ -1663,37 +1725,17 @@ void kick_ball_core(){
}
// 准备踢球，打开挡板，收到后方
void ready_kick_ball(){
setFrontierServoAngles(100); // 打开挡板，收到后方
leg_height_base = 30.0f; // 下降
if( abs(current_s1 - ready_kick_angel) > 5){
setFrontierServoAngles(ready_kick_angel); // 打开挡板，收到后方
leg_height_base = ready_leg_height_base; // 下降
}
}
void aim_ob ( int dx, int dy, String ob){
//----- 控制左右 使小车正面是否朝向物体--------
// 物体在左方 dx趋近-150
// 物体在右方 dx趋近 150
// 中间 dx=0 只需要一个区间 50
// 判断是否瞄准
if( abs(dx) < 50){ // 小车正面已经瞄准
trace_ob_flag = true;
dx_to_joyx = 9999; // 已经追踪到了目标，不需要再左右调整旋转
pitch_adjust = 0.0f; // debug
}else{ // 没有瞄准
trace_ob_flag = false;
// 相机瞄准
void cam_aim_ball (int dy){
if(dy == 9999){
return;
}
// pid 左右旋转 瞄准目标
if(!trace_ob_flag){
int joyx_derection = (dx > 0) ? -1 : 1;
dx_to_joyx = pid_trace_joyx(abs(dx)) * joyx_derection;
}else{ // 已经正面朝向物体
}
//----- 控制上下 相机舵机控制-----------
// 物体在上方 dy趋近-100
// 物体在下方 dy趋近 100
// 中间 dy = 0
@@ -1724,13 +1766,33 @@ void aim_ob ( int dx, int dy, String ob){
// 偏差过小，重置PD状态
last_dy = 0;
}
}
// 控制左右 使小车正面是否朝向物体
void joyx_aim_ball (int dx){
if(dx == 9999){
return;
}
void aim_ball ( int dx, int dy){
aim_ob ( dx, dy, "ball");
//----- 控制左右 使小车正面是否朝向物体--------
// 物体在左方 dx趋近-150
// 物体在右方 dx趋近 150
// 中间 dx=0 只需要一个区间 50
// 判断是否瞄准
if( abs(dx) < 50){ // 小车正面已经瞄准
trace_ob_flag = true;
dx_to_joyx = 9999; // 已经追踪到了目标，不需要再左右调整旋转
pitch_adjust = 0.0f; // debug
}else{ // 没有瞄准
trace_ob_flag = false;
}
// pid 左右旋转 瞄准目标
if(!trace_ob_flag){
int joyx_derection = (dx > 0) ? -1 : 1;
dx_to_joyx = pid_trace_joyx(abs(dx)) * joyx_derection;
}
void aim_face ( int dx, int dy){
aim_ob ( dx, dy, "face");
}
// 控制摄像头舵机
@@ -1759,17 +1821,21 @@ void SerialToMaixcam(String cat, String str){
}
// 退出追踪模式
void cancel_trace_mode(){
void cancel_kick_mode(){
kick_over_count = 0;
trace_mode = false;
trace_ob_flag = false;
chased_flag = false;
SetKickMode("");
dx_to_joyx = 9999;
dy_to_joyy = 9999;
yaw_to_joyx = 9999;
pid_trace_joyx.error_prev = 0.0f;
pid_trace_joyy.error_prev = 0.0f;
pid_yaw_align.error_prev = 0.0f;
setLEDColor(CRGB::Black);
leg_height_base = original_leg_height_base; // 上升
@@ -1777,5 +1843,14 @@ void cancel_trace_mode(){
}
void SetKickMode(String mode){
kick_mode = mode;
yaw_to_joyx = 9999;
// 进入kick_back_mode时，记录当前偏航角作为基准0度
if(mode == "kick_run_mode" || mode == "kick_back_mode"){
kick_yaw_zero = mpu6050.getAngleZ(); // 读取当前偏航角（Z轴角度）作为基准
pid_yaw_align.error_prev = 0.0f; // 重置偏航对齐PID状态
}
}