//通过ringBuffer里的数据计算每一点处三个轮子的速度
//目的更新wheelOne wheelTwo wheelThree这三个数组里的三轮速度，便于下一次的速度削减
//wheelOne 一号轮速度数组首地址
//wheelTwo 二号轮速度数组首地址
//wheelThree 三号轮速度数组首地址
static public void CalThreeWheelVel(float[] wheelOne, float[] wheelTwo, float[] wheelThree)
{
//分解到三个轮对全局速度进行规划
float n = PointsInfo.pnts.Count();
Calculate.TriWheelVel1_t threeVell;
for (int i = 2; i < n + 1; i++)
{
float angErr = PointsInfo.pnts[i - 1].posAngle - PointsInfo.pnts[i - 2].posAngle;
angErr = angErr > 180 ? angErr - 360 : angErr;
angErr = angErr < -180 ? 360 + angErr : angErr;
float time = 0.0f;
time = (PointsInfo.pnts[i - 1].length - PointsInfo.pnts[i - 2].length) / (PointsInfo.pnts[i - 1].velMax + PointsInfo.pnts[i - 2].velMax) * 2;
float rotationVell = angErr / time;
threeVell = Calculate.CaculateThreeWheelVel(PointsInfo.pnts[i - 1].velMax, PointsInfo.pnts[i - 1].direction, rotationVell, PointsInfo.pnts[i - 1].posAngle);
wheelOne[i - 1] = threeVell.v1;
wheelTwo[i - 1] = threeVell.v2;
wheelThree[i - 1] = threeVell.v3;
}
wheelOne[0] = wheelOne[1];
wheelTwo[0] = wheelTwo[1];
wheelThree[0] = wheelThree[1];
}
//通过降低合速度保证某轮的速度要求
//vellCar 降速前的前进合速度 单位 mm/s
//orientation 速度朝向 单位 度
//rotationalVell 旋转速度 单位 度每秒
//wheelNum 所降速的轮号
// targetWheelVell 所降速的目标
// 返回所降低后的合速度
static public float DecreseVellByOneWheel(float vellCar, float orientation, float rotationalVell, float zAngle, int wheelNum, float targetWheelVell)
{
Calculate.TriWheelVel1_t vell;
int i;
switch (wheelNum)
{
case 1:
//每次合速度乘0.9,直到满足一号轮速度降低至目标速度。对于一些不能满足的，循环10次后自动退出
for (i = 0; i < 10; i++)
{
vellCar *= 0.9f;
vell = Calculate.CaculateThreeWheelVel(vellCar, orientation, rotationalVell, zAngle);
if (Math.Abs(vell.v1) < Math.Abs(targetWheelVell))
{
break;
}
}
break;
case 2:
for (i = 0; i < 10; i++)
{
vellCar *= 0.9f;
vell = Calculate.CaculateThreeWheelVel(vellCar, orientation, rotationalVell, zAngle);
if (Math.Abs(vell.v2) < Math.Abs(targetWheelVell))
{
break;
}
}
break;
case 3:
for (i = 0; i < 10; i++)
{
vellCar *= 0.9f;
vell = Calculate.CaculateThreeWheelVel(vellCar, orientation, rotationalVell, zAngle);
if (Math.Abs(vell.v3) < Math.Abs(targetWheelVell))
{
break;
}
}
break;
}
return(vellCar);
}