public Crawler(RoboticArm leftBack, RoboticArm leftFront, RoboticArm rightBack, RoboticArm rightFront, IMyRemoteControl rc) { LeftBack = leftBack; LeftFront = leftFront; RightBack = rightBack; RightFront = rightFront; Rc = rc; StepIndent = new [] { GetAverageArmSegmentLength(LeftBack), GetAverageArmSegmentLength(LeftFront), GetAverageArmSegmentLength(RightBack), GetAverageArmSegmentLength(RightFront), }.Average() *StepIndentMultiplier; // getting local positions of leg base rotation rotors var lb = VectorUtility.GetLocalVector(Rc.WorldMatrix, Rc.GetPosition(), LeftBack.RotorRotation.Rotor.GetPosition()); var lf = VectorUtility.GetLocalVector(Rc.WorldMatrix, Rc.GetPosition(), LeftFront.RotorRotation.Rotor.GetPosition()); var rb = VectorUtility.GetLocalVector(Rc.WorldMatrix, Rc.GetPosition(), RightBack.RotorRotation.Rotor.GetPosition()); var rf = VectorUtility.GetLocalVector(Rc.WorldMatrix, Rc.GetPosition(), RightFront.RotorRotation.Rotor.GetPosition()); Center = VectorUtility.GetAverageVector( VectorUtility.GetAverageVector(lb, lf), VectorUtility.GetAverageVector(rb, rf)); lb -= Center; lf -= Center; rb -= Center; rf -= Center; LbMarchPoint = VectorUtility.Normalize(lb) * (StepIndent + lb.Length()); LfMarchPoint = VectorUtility.Normalize(lf) * (StepIndent + lf.Length()); RbMarchPoint = VectorUtility.Normalize(rb) * (StepIndent + rb.Length()); RfMarchPoint = VectorUtility.Normalize(rf) * (StepIndent + rf.Length()); Lbh = new Vector3D(LbMarchPoint.X, LbMarchPoint.Y - 0.5, LbMarchPoint.Z); Lfh = new Vector3D(LfMarchPoint.X, LfMarchPoint.Y - 0.5, LfMarchPoint.Z); Rbh = new Vector3D(RbMarchPoint.X, RbMarchPoint.Y - 0.5, RbMarchPoint.Z); Rfh = new Vector3D(RfMarchPoint.X, RfMarchPoint.Y - 0.5, RfMarchPoint.Z); Lbs = new Vector3D(LbMarchPoint.X, LbMarchPoint.Y - 1.5, LbMarchPoint.Z + 3); Lfs = new Vector3D(LfMarchPoint.X, LfMarchPoint.Y - 1.5, LfMarchPoint.Z - 3); Rbs = new Vector3D(RbMarchPoint.X, RbMarchPoint.Y - 1.5, RbMarchPoint.Z + 3); Rfs = new Vector3D(RfMarchPoint.X, RfMarchPoint.Y - 1.5, RfMarchPoint.Z - 3); }
public void KeepMoving(Vector3D destination, double velocity /* Meters per sec*/) { var matrix = RotorRotation.Rotor.WorldMatrix; // getting world points var rotationBasePoint = RotorRotation.Rotor.GetPosition(); var armBasePoint = Rotor1.Rotor.GetPosition(); var armElbowPoint = Rotor2.Rotor.GetPosition(); var armTipPoint = Tip.GetPosition(); // adjust destination in case its something wrong with it var averageLength = (Vector3D.Distance(armBasePoint, armElbowPoint) + Vector3D.Distance(armElbowPoint, armTipPoint)) / 2; destination = CutTrailAtClosestToBase(rotationBasePoint, armTipPoint, destination); destination = BringToSafeZone(destination, armBasePoint, armElbowPoint, armTipPoint); destination = TakeNearbyPoint(destination, armTipPoint, averageLength); // no point doing anything if the tip is already there if (Vector3D.Distance(armTipPoint, destination) < DistanceToCancelMovement) { RotorRotation.Stop(); Rotor1.Stop(); Rotor2.Stop(); return; } var rotationEndPoint = GetRotationEndPoint(destination, rotationBasePoint, armTipPoint); // calculate local points var localArmBasePoint = VectorUtility.GetLocalVector(matrix, rotationBasePoint, armBasePoint); var localArmTipPoint = VectorUtility.GetLocalVector(matrix, rotationBasePoint, armTipPoint); var localDestination = VectorUtility.GetLocalVector(matrix, rotationBasePoint, destination); var localRotationEndPoint = VectorUtility.GetLocalVector(matrix, rotationBasePoint, rotationEndPoint); var localArmElbow = VectorUtility.GetLocalVector(matrix, rotationBasePoint, armElbowPoint); var localArmPlaneNormalPoint = /*localArmBasePoint + */ Vector3D.Rotate(Rotor1.Rotor.WorldMatrix.Up, MatrixD.Transpose(matrix)); // calculating planes to project on var rotationPlane = new PlaneD(Vector3D.Zero, Vector3D.UnitY); // note that rotation plane is XZ var armPlane = new PlaneD(localArmBasePoint, localArmPlaneNormalPoint); // normalizing arm points by projecting them to the arm plane for the arm part calculations var normalizedArmTip = armPlane.ProjectPoint(ref localArmTipPoint); var normalizedArmElbow = armPlane.ProjectPoint(ref localArmElbow); var normalizedDestination = armPlane.ProjectPoint(ref localDestination); // calculate local projected points var localProjectedArmBase = rotationPlane.ProjectPoint(ref localArmBasePoint); var localProjectedTip = rotationPlane.ProjectPoint(ref normalizedArmTip); var localProjectedTipForRotation = rotationPlane.ProjectPoint(ref localArmTipPoint); var localProjectedDestination = rotationPlane.ProjectPoint(ref localDestination); // for rotation // calculate rotation distance var rotationDistance = Math.Abs(VectorUtility.Angle(localProjectedTipForRotation, localProjectedDestination)) * localProjectedTip.Length(); // adjust speeds //var rotationSpeedPart = GetRotationSpeedPart(localDestination, localArmTipPoint, localRotationBase, armDistance); var armDistance = Vector3D.Distance(localDestination, localRotationEndPoint); var rotationSpeedPart = rotationDistance / (rotationDistance + armDistance); var armSpeedPart = 1d - rotationSpeedPart; var rotationVelocity = rotationSpeedPart * velocity; var armVelocity = armSpeedPart * velocity; // calculate base rotor rotation speed var targetRps = rotationVelocity / localProjectedTip.Length(); // calculating tip and destination as 2D points for the arm by rotating them parallel to YZ plane var armBaseElevation = localArmBasePoint.Y; var angle2D = (float)VectorUtility.Angle(localProjectedTip - localProjectedArmBase, Vector3D.UnitX); if (localProjectedTip.Z < 0) { angle2D *= -1; } var armTipR = VectorUtility.Rotate(normalizedArmTip, Vector3D.UnitY, angle2D); var armElbowR = VectorUtility.Rotate(normalizedArmElbow, Vector3D.UnitY, angle2D); var armDestinationR = VectorUtility.Rotate(normalizedDestination, Vector3D.UnitY, angle2D); var armTipPoint2D = new Vector2D(armTipR.X, armTipR.Y - armBaseElevation); var armElbow2D = new Vector2D(armElbowR.X, armElbowR.Y - armBaseElevation); var armDestinationPoint2D = new Vector2D(armDestinationR.X, armDestinationR.Y - armBaseElevation); // launch movement MakeRotationMovement(localProjectedTipForRotation, localProjectedDestination, targetRps); MakeArmMovement(armTipPoint2D, armElbow2D, armDestinationPoint2D, armVelocity); }