Hshine
/
sentry_left
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

修改全向轮解算，cmd输入值现为云台速度单位m/s

This commit is contained in:
sph 2024-03-11 22:52:15 +08:00
parent 885df3d5ed
commit a7b1edaf5a
2 changed files with 16 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
application/chassis/chassis.c
View File

@ -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;

4
application/cmd/robot_cmd.c
View File

@ -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数值方向
// 发射参数