//通过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); }