2026-10-20 16:45:51 +08:00
|
|
|
|
/// 差速转向算法:摇杆坐标 -> 线速度/角速度(物理量)
|
|
|
|
|
|
///
|
|
|
|
|
|
/// 协议变更(2026-10):0x00 指令不再下发左右轮速(int16),
|
|
|
|
|
|
/// 改为下发线速度(m/s)、角速度(rad/s) 的 double 值,
|
|
|
|
|
|
/// 速度到轮速的运动学换算改由机器端完成。
|
2026-01-18 20:21:14 +08:00
|
|
|
|
class DiffSteerUseCase {
|
2026-10-20 16:45:51 +08:00
|
|
|
|
/// 线速度满量程(m/s),以后按车型配置
|
|
|
|
|
|
static const double maxLinearSpeed = 1.0;
|
2026-01-18 20:21:14 +08:00
|
|
|
|
|
2026-10-20 16:45:51 +08:00
|
|
|
|
/// 角速度满量程(rad/s),以后按车型配置
|
|
|
|
|
|
static const double maxAngularSpeed = 2.0;
|
2026-01-18 20:21:14 +08:00
|
|
|
|
|
2026-10-20 16:45:51 +08:00
|
|
|
|
/// 摇杆满格值(CCJoystick 输出范围 ±100)
|
|
|
|
|
|
static const int stickFullScale = 100;
|
2026-01-18 20:21:14 +08:00
|
|
|
|
|
2026-10-20 16:45:51 +08:00
|
|
|
|
/// 摇杆坐标 (x, y) -> {'linear': 线速度 m/s, 'angular': 角速度 rad/s}
|
|
|
|
|
|
Map<String, double> calculate(int x, int y) {
|
|
|
|
|
|
// 摇杆原始值先归一化为 0~1 比例,再乘满量程物理量
|
|
|
|
|
|
double linear = (y / stickFullScale) * maxLinearSpeed;
|
2026-10-28 14:38:24 +08:00
|
|
|
|
// 实测机器端角速度正方向与摇杆相反:推右(x>0)需发负值才是右转,故乘 -1
|
|
|
|
|
|
// (2026-10 实测修正:前进后退正常、左右反了,由 App 侧翻转符号)
|
|
|
|
|
|
double angular = -(x / stickFullScale) * maxAngularSpeed;
|
2026-01-18 20:21:14 +08:00
|
|
|
|
|
2026-10-20 16:45:51 +08:00
|
|
|
|
// 倒车补偿:倒车时转向反向,保持与旧轮速版一致的操作手感
|
|
|
|
|
|
// (若联调确认机器端已做该补偿,删除本行)
|
|
|
|
|
|
if (linear < 0) angular = -angular;
|
2026-01-18 20:21:14 +08:00
|
|
|
|
|
2026-10-20 16:45:51 +08:00
|
|
|
|
return {'linear': linear, 'angular': angular};
|
2026-01-18 20:21:14 +08:00
|
|
|
|
}
|
|
|
|
|
|
}
|