From a7b1edaf5ac2cba1e17c1de30198d4f5278515c8 Mon Sep 17 00:00:00 2001 From: sph <1527550984@qq.com> Date: Mon, 11 Mar 2024 22:52:15 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E5=85=A8=E5=90=91=E8=BD=AE?= =?UTF-8?q?=E8=A7=A3=E7=AE=97=EF=BC=8Ccmd=E8=BE=93=E5=85=A5=E5=80=BC?= =?UTF-8?q?=E7=8E=B0=E4=B8=BA=E4=BA=91=E5=8F=B0=E9=80=9F=E5=BA=A6=E5=8D=95?= =?UTF-8?q?=E4=BD=8Dm/s?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- application/chassis/chassis.c | 29 ++++++++++++++--------------- application/cmd/robot_cmd.c | 4 ++-- 2 files changed, 16 insertions(+), 17 deletions(-) diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index 047fcc2..954d667 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -167,17 +167,16 @@ static void MecanumCalculate() vt_rb = chassis_vx + chassis_vy - chassis_cmd_recv.wz * RB_CENTER; } -static void OmniCalculate() -{ - vt_rf = -(HALF_TRACK_WIDTH+HALF_WHEEL_BASE)*chassis_cmd_recv.wz + chassis_vx - chassis_vy; - vt_rb = -(HALF_TRACK_WIDTH+HALF_WHEEL_BASE)*chassis_cmd_recv.wz - chassis_vx - chassis_vy; - vt_lb = -(HALF_TRACK_WIDTH+HALF_WHEEL_BASE)*chassis_cmd_recv.wz - chassis_vx + chassis_vy; - vt_lf = -(HALF_TRACK_WIDTH+HALF_WHEEL_BASE)*chassis_cmd_recv.wz + chassis_vx + chassis_vy; +static void OmniCalculate() { + vt_rf = HALF_WHEEL_BASE * chassis_cmd_recv.wz + chassis_vx * 0.707f + chassis_vy * 0.707f; + vt_rb = HALF_WHEEL_BASE * chassis_cmd_recv.wz + chassis_vx * 0.707f - chassis_vy * 0.707f; + vt_lb = HALF_WHEEL_BASE * chassis_cmd_recv.wz - chassis_vx * 0.707f - chassis_vy * 0.707f; + vt_lf = HALF_WHEEL_BASE * chassis_cmd_recv.wz - chassis_vx * 0.707f + chassis_vy * 0.707f; - vt_rf/=(WHEEL_BASE*1.414f); - vt_rb/=(WHEEL_BASE*1.414f); - vt_lb/=(WHEEL_BASE*1.414f); - vt_lf/=(WHEEL_BASE*1.414f); + vt_rf = (vt_rf / RADIUS_WHEEL) * 180 / PI; + vt_rb = (vt_rb / RADIUS_WHEEL) * 180 / PI; + vt_lb = (vt_lb / RADIUS_WHEEL) * 180 / PI; + vt_lf = (vt_lf / RADIUS_WHEEL) * 180 / PI; } @@ -281,12 +280,12 @@ void ChassisTask() } // 根据云台和底盘的角度offset将控制量映射到底盘坐标系上 - // 底盘逆时针旋转为角度正方向;云台命令的方向以云台指向的方向为x,采用右手系(x指向正北时y在正东) + // 底盘逆时针旋转为角度正方向;云台命令的方向以云台指向的方向为x,采用右手系(x指向正北时y在正)西方 static float sin_theta, cos_theta; - cos_theta = arm_cos_f32(chassis_cmd_recv.offset_angle ); - sin_theta = arm_sin_f32(chassis_cmd_recv.offset_angle ); - //sin_theta = 0; cos_theta = 1; - chassis_vx = chassis_cmd_recv.vx * cos_theta - chassis_cmd_recv.vy * sin_theta; + cos_theta = arm_cos_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD); + sin_theta = arm_sin_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD); + + chassis_vx = -chassis_cmd_recv.vx * cos_theta + chassis_cmd_recv.vy * sin_theta; chassis_vy = chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta; diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index 3f7c535..6b542ad 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -180,8 +180,8 @@ static void RemoteControlSet() // 云台软件限位 // 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整 - chassis_cmd_send.vx = -8.0f * (float)rc_data[TEMP].rc.rocker_r1; // _水平方向 - chassis_cmd_send.vy = 8.0f * (float)rc_data[TEMP].rc.rocker_r_; // 1数值方向 + chassis_cmd_send.vx = 0.02f * (float)rc_data[TEMP].rc.rocker_r1; // _水平方向 + chassis_cmd_send.vy = 0.02f * (float)rc_data[TEMP].rc.rocker_r_; // 1数值方向 // 发射参数