@Test void goToPositionStatic() { assertEquals( new MecanumPowers(1, 0, 0), MecanumPurePursuitController.goToPosition( ORIGIN, ORIGIN, new Waypoint(100, 0, -1), null ) ); assertEquals( new MecanumPowers(0.5, 0, 0), MecanumPurePursuitController.goToPosition( ORIGIN, ORIGIN, new Waypoint(6, 0, -1), null ) ); assertEquals( new MecanumPowers(0, 0.5, 0.5), MecanumPurePursuitController.goToPosition( ORIGIN, ORIGIN, new HeadingControlledWaypoint(0, 6, -1, Math.PI/2), null ) ); assertEquals( new MecanumPowers(-1.0, 0.5, -0.5), MecanumPurePursuitController.goToPosition( new Pose(128, 128, Math.PI/2), ORIGIN, new HeadingControlledWaypoint(128 - 6, 128 - 12, -1, 0), null ) ); assertEquals( new MecanumPowers(0.5, 0.5,0.25), MecanumPurePursuitController.goToPosition( ORIGIN, ORIGIN, new Waypoint(6, 6, -1), null ) ); }
public static MecanumPowers goToPosition(Pose robotPose, Pose robotVelocity, Waypoint target, StopWaypoint finalTarget) { System.out.println("Current velocity: " + robotVelocity.toString()); if (finalTarget == null || robotPose.distance(finalTarget) > 18) { Pose relPPTarget = relDistanceToTarget(robotPose, target); Pose translationPowers = relPPTarget.scale(-1).divideBy(GUNNING_REDUCTION_DISTANCES); System.out.println(translationPowers.toString()); double forwardAngle = target.minus(robotPose).atan(); double backwardAngle = forwardAngle + Math.PI; double angleToForward = MathUtil.angleWrap(forwardAngle - robotPose.heading); double angleToBackward = MathUtil.angleWrap(backwardAngle - robotPose.heading); double autoAngle = Math.abs(angleToForward) < Math.abs(angleToBackward) ? forwardAngle : backwardAngle; double desiredAngle = target instanceof HeadingControlledWaypoint ? ((HeadingControlledWaypoint) target).targetHeading : autoAngle; double angleToTarget = MathUtil.angleWrap(desiredAngle - robotPose.heading); translationPowers.heading = angleToTarget / GUNNING_REDUCTION_DISTANCES.heading; return new MecanumPowers(translationPowers); } else if (robotVelocity.radius() > MIN_SLIP_SPEED && robotPose.distance(finalTarget) > UNDERSHOOT_DIST) { Point t = MathUtil.lineSegmentCircleIntersection(robotPose, finalTarget, finalTarget, UNDERSHOOT_DIST); Pose relVelocity = new Pose(robotVelocity.rotated(-robotPose.heading), robotVelocity.heading); System.out.println(relVelocity); Pose relSlipDistances = relVelocity.multiply(SLIP_DISTANCES); System.out.println(relSlipDistances); Pose relAbsTarget = relDistanceToTarget(robotPose, t).add(relSlipDistances); System.out.println(relAbsTarget); relAbsTarget.heading = -MathUtil.angleWrap(finalTarget.targetHeading - robotPose.heading - relSlipDistances.heading); Pose translationPowers = relAbsTarget.scale(-1).divideBy(GUNNING_REDUCTION_DISTANCES); return new MecanumPowers(translationPowers); } else { Pose relAbsTarget = relDistanceToTarget(robotPose, finalTarget); double angleToTarget = MathUtil.angleWrap(finalTarget.targetHeading - robotPose.heading); Pose dirPowers = new Pose( -MathUtil.powRetainingSign(relAbsTarget.x, CLOSE_EXPONENT), -MathUtil.powRetainingSign(relAbsTarget.y, CLOSE_EXPONENT), MathUtil.powRetainingSign(angleToTarget, CLOSE_EXPONENT) ); return new MecanumPowers(dirPowers.multiply(ONE_AWAY_POWERS)); } }
MecanumPurePursuitController { public static MecanumPowers goToPosition(Pose robotPose, Pose robotVelocity, Waypoint target, StopWaypoint finalTarget) { System.out.println("Current velocity: " + robotVelocity.toString()); if (finalTarget == null || robotPose.distance(finalTarget) > 18) { Pose relPPTarget = relDistanceToTarget(robotPose, target); Pose translationPowers = relPPTarget.scale(-1).divideBy(GUNNING_REDUCTION_DISTANCES); System.out.println(translationPowers.toString()); double forwardAngle = target.minus(robotPose).atan(); double backwardAngle = forwardAngle + Math.PI; double angleToForward = MathUtil.angleWrap(forwardAngle - robotPose.heading); double angleToBackward = MathUtil.angleWrap(backwardAngle - robotPose.heading); double autoAngle = Math.abs(angleToForward) < Math.abs(angleToBackward) ? forwardAngle : backwardAngle; double desiredAngle = target instanceof HeadingControlledWaypoint ? ((HeadingControlledWaypoint) target).targetHeading : autoAngle; double angleToTarget = MathUtil.angleWrap(desiredAngle - robotPose.heading); translationPowers.heading = angleToTarget / GUNNING_REDUCTION_DISTANCES.heading; return new MecanumPowers(translationPowers); } else if (robotVelocity.radius() > MIN_SLIP_SPEED && robotPose.distance(finalTarget) > UNDERSHOOT_DIST) { Point t = MathUtil.lineSegmentCircleIntersection(robotPose, finalTarget, finalTarget, UNDERSHOOT_DIST); Pose relVelocity = new Pose(robotVelocity.rotated(-robotPose.heading), robotVelocity.heading); System.out.println(relVelocity); Pose relSlipDistances = relVelocity.multiply(SLIP_DISTANCES); System.out.println(relSlipDistances); Pose relAbsTarget = relDistanceToTarget(robotPose, t).add(relSlipDistances); System.out.println(relAbsTarget); relAbsTarget.heading = -MathUtil.angleWrap(finalTarget.targetHeading - robotPose.heading - relSlipDistances.heading); Pose translationPowers = relAbsTarget.scale(-1).divideBy(GUNNING_REDUCTION_DISTANCES); return new MecanumPowers(translationPowers); } else { Pose relAbsTarget = relDistanceToTarget(robotPose, finalTarget); double angleToTarget = MathUtil.angleWrap(finalTarget.targetHeading - robotPose.heading); Pose dirPowers = new Pose( -MathUtil.powRetainingSign(relAbsTarget.x, CLOSE_EXPONENT), -MathUtil.powRetainingSign(relAbsTarget.y, CLOSE_EXPONENT), MathUtil.powRetainingSign(angleToTarget, CLOSE_EXPONENT) ); return new MecanumPowers(dirPowers.multiply(ONE_AWAY_POWERS)); } } }
MecanumPurePursuitController { public static MecanumPowers goToPosition(Pose robotPose, Pose robotVelocity, Waypoint target, StopWaypoint finalTarget) { System.out.println("Current velocity: " + robotVelocity.toString()); if (finalTarget == null || robotPose.distance(finalTarget) > 18) { Pose relPPTarget = relDistanceToTarget(robotPose, target); Pose translationPowers = relPPTarget.scale(-1).divideBy(GUNNING_REDUCTION_DISTANCES); System.out.println(translationPowers.toString()); double forwardAngle = target.minus(robotPose).atan(); double backwardAngle = forwardAngle + Math.PI; double angleToForward = MathUtil.angleWrap(forwardAngle - robotPose.heading); double angleToBackward = MathUtil.angleWrap(backwardAngle - robotPose.heading); double autoAngle = Math.abs(angleToForward) < Math.abs(angleToBackward) ? forwardAngle : backwardAngle; double desiredAngle = target instanceof HeadingControlledWaypoint ? ((HeadingControlledWaypoint) target).targetHeading : autoAngle; double angleToTarget = MathUtil.angleWrap(desiredAngle - robotPose.heading); translationPowers.heading = angleToTarget / GUNNING_REDUCTION_DISTANCES.heading; return new MecanumPowers(translationPowers); } else if (robotVelocity.radius() > MIN_SLIP_SPEED && robotPose.distance(finalTarget) > UNDERSHOOT_DIST) { Point t = MathUtil.lineSegmentCircleIntersection(robotPose, finalTarget, finalTarget, UNDERSHOOT_DIST); Pose relVelocity = new Pose(robotVelocity.rotated(-robotPose.heading), robotVelocity.heading); System.out.println(relVelocity); Pose relSlipDistances = relVelocity.multiply(SLIP_DISTANCES); System.out.println(relSlipDistances); Pose relAbsTarget = relDistanceToTarget(robotPose, t).add(relSlipDistances); System.out.println(relAbsTarget); relAbsTarget.heading = -MathUtil.angleWrap(finalTarget.targetHeading - robotPose.heading - relSlipDistances.heading); Pose translationPowers = relAbsTarget.scale(-1).divideBy(GUNNING_REDUCTION_DISTANCES); return new MecanumPowers(translationPowers); } else { Pose relAbsTarget = relDistanceToTarget(robotPose, finalTarget); double angleToTarget = MathUtil.angleWrap(finalTarget.targetHeading - robotPose.heading); Pose dirPowers = new Pose( -MathUtil.powRetainingSign(relAbsTarget.x, CLOSE_EXPONENT), -MathUtil.powRetainingSign(relAbsTarget.y, CLOSE_EXPONENT), MathUtil.powRetainingSign(angleToTarget, CLOSE_EXPONENT) ); return new MecanumPowers(dirPowers.multiply(ONE_AWAY_POWERS)); } }  }
MecanumPurePursuitController { public static MecanumPowers goToPosition(Pose robotPose, Pose robotVelocity, Waypoint target, StopWaypoint finalTarget) { System.out.println("Current velocity: " + robotVelocity.toString()); if (finalTarget == null || robotPose.distance(finalTarget) > 18) { Pose relPPTarget = relDistanceToTarget(robotPose, target); Pose translationPowers = relPPTarget.scale(-1).divideBy(GUNNING_REDUCTION_DISTANCES); System.out.println(translationPowers.toString()); double forwardAngle = target.minus(robotPose).atan(); double backwardAngle = forwardAngle + Math.PI; double angleToForward = MathUtil.angleWrap(forwardAngle - robotPose.heading); double angleToBackward = MathUtil.angleWrap(backwardAngle - robotPose.heading); double autoAngle = Math.abs(angleToForward) < Math.abs(angleToBackward) ? forwardAngle : backwardAngle; double desiredAngle = target instanceof HeadingControlledWaypoint ? ((HeadingControlledWaypoint) target).targetHeading : autoAngle; double angleToTarget = MathUtil.angleWrap(desiredAngle - robotPose.heading); translationPowers.heading = angleToTarget / GUNNING_REDUCTION_DISTANCES.heading; return new MecanumPowers(translationPowers); } else if (robotVelocity.radius() > MIN_SLIP_SPEED && robotPose.distance(finalTarget) > UNDERSHOOT_DIST) { Point t = MathUtil.lineSegmentCircleIntersection(robotPose, finalTarget, finalTarget, UNDERSHOOT_DIST); Pose relVelocity = new Pose(robotVelocity.rotated(-robotPose.heading), robotVelocity.heading); System.out.println(relVelocity); Pose relSlipDistances = relVelocity.multiply(SLIP_DISTANCES); System.out.println(relSlipDistances); Pose relAbsTarget = relDistanceToTarget(robotPose, t).add(relSlipDistances); System.out.println(relAbsTarget); relAbsTarget.heading = -MathUtil.angleWrap(finalTarget.targetHeading - robotPose.heading - relSlipDistances.heading); Pose translationPowers = relAbsTarget.scale(-1).divideBy(GUNNING_REDUCTION_DISTANCES); return new MecanumPowers(translationPowers); } else { Pose relAbsTarget = relDistanceToTarget(robotPose, finalTarget); double angleToTarget = MathUtil.angleWrap(finalTarget.targetHeading - robotPose.heading); Pose dirPowers = new Pose( -MathUtil.powRetainingSign(relAbsTarget.x, CLOSE_EXPONENT), -MathUtil.powRetainingSign(relAbsTarget.y, CLOSE_EXPONENT), MathUtil.powRetainingSign(angleToTarget, CLOSE_EXPONENT) ); return new MecanumPowers(dirPowers.multiply(ONE_AWAY_POWERS)); } }  static MecanumPowers goToPosition(Pose robotPose, Pose robotVelocity, Waypoint target, StopWaypoint finalTarget); }
MecanumPurePursuitController { public static MecanumPowers goToPosition(Pose robotPose, Pose robotVelocity, Waypoint target, StopWaypoint finalTarget) { System.out.println("Current velocity: " + robotVelocity.toString()); if (finalTarget == null || robotPose.distance(finalTarget) > 18) { Pose relPPTarget = relDistanceToTarget(robotPose, target); Pose translationPowers = relPPTarget.scale(-1).divideBy(GUNNING_REDUCTION_DISTANCES); System.out.println(translationPowers.toString()); double forwardAngle = target.minus(robotPose).atan(); double backwardAngle = forwardAngle + Math.PI; double angleToForward = MathUtil.angleWrap(forwardAngle - robotPose.heading); double angleToBackward = MathUtil.angleWrap(backwardAngle - robotPose.heading); double autoAngle = Math.abs(angleToForward) < Math.abs(angleToBackward) ? forwardAngle : backwardAngle; double desiredAngle = target instanceof HeadingControlledWaypoint ? ((HeadingControlledWaypoint) target).targetHeading : autoAngle; double angleToTarget = MathUtil.angleWrap(desiredAngle - robotPose.heading); translationPowers.heading = angleToTarget / GUNNING_REDUCTION_DISTANCES.heading; return new MecanumPowers(translationPowers); } else if (robotVelocity.radius() > MIN_SLIP_SPEED && robotPose.distance(finalTarget) > UNDERSHOOT_DIST) { Point t = MathUtil.lineSegmentCircleIntersection(robotPose, finalTarget, finalTarget, UNDERSHOOT_DIST); Pose relVelocity = new Pose(robotVelocity.rotated(-robotPose.heading), robotVelocity.heading); System.out.println(relVelocity); Pose relSlipDistances = relVelocity.multiply(SLIP_DISTANCES); System.out.println(relSlipDistances); Pose relAbsTarget = relDistanceToTarget(robotPose, t).add(relSlipDistances); System.out.println(relAbsTarget); relAbsTarget.heading = -MathUtil.angleWrap(finalTarget.targetHeading - robotPose.heading - relSlipDistances.heading); Pose translationPowers = relAbsTarget.scale(-1).divideBy(GUNNING_REDUCTION_DISTANCES); return new MecanumPowers(translationPowers); } else { Pose relAbsTarget = relDistanceToTarget(robotPose, finalTarget); double angleToTarget = MathUtil.angleWrap(finalTarget.targetHeading - robotPose.heading); Pose dirPowers = new Pose( -MathUtil.powRetainingSign(relAbsTarget.x, CLOSE_EXPONENT), -MathUtil.powRetainingSign(relAbsTarget.y, CLOSE_EXPONENT), MathUtil.powRetainingSign(angleToTarget, CLOSE_EXPONENT) ); return new MecanumPowers(dirPowers.multiply(ONE_AWAY_POWERS)); } }  static MecanumPowers goToPosition(Pose robotPose, Pose robotVelocity, Waypoint target, StopWaypoint finalTarget); static Pose SLIP_DISTANCES; static double UNDERSHOOT_DIST; static double MIN_SLIP_SPEED; static Pose GUNNING_REDUCTION_DISTANCES; static Pose ONE_AWAY_POWERS; static double CLOSE_EXPONENT; }