diff --git a/TeamCode/src/main/java/com/kuriosityrobotics/firstforward/robot/sensors/DistanceSensorPair.java b/TeamCode/src/main/java/com/kuriosityrobotics/firstforward/robot/sensors/DistanceSensorPair.java index ce4ba426..d740310f 100644 --- a/TeamCode/src/main/java/com/kuriosityrobotics/firstforward/robot/sensors/DistanceSensorPair.java +++ b/TeamCode/src/main/java/com/kuriosityrobotics/firstforward/robot/sensors/DistanceSensorPair.java @@ -6,9 +6,7 @@ import static java.lang.Math.PI; import static java.lang.Math.atan2; import static java.lang.Math.cos; -import static java.lang.Math.sqrt; -import com.kuriosityrobotics.firstforward.robot.sensors.kf.ExtendedKalmanFilter; import com.kuriosityrobotics.firstforward.robot.util.math.Point; import com.kuriosityrobotics.firstforward.robot.util.math.Pose; @@ -114,19 +112,19 @@ public Wall bestWall(Pose covariance, Pose robotPose) { var sensor2Line = Lines.fromPointAndAngle(sensor2, sensorHeading, DistanceSensorLocaliser.p); var sensor2Segment = sensor2Line.rayFrom(sensor2); - if (wallInRangeOfRays(covariance, robotPose.heading, Wall.LEFT, sensor1Segment, sensor2Segment)) + if (wallInRangeOfRays(covariance, Wall.LEFT, sensor1Segment, sensor2Segment)) return Wall.LEFT; - else if (wallInRangeOfRays(covariance, robotPose.heading, Wall.RIGHT, sensor1Segment, sensor2Segment)) + else if (wallInRangeOfRays(covariance, Wall.RIGHT, sensor1Segment, sensor2Segment)) return Wall.RIGHT; - else if (wallInRangeOfRays(covariance, robotPose.heading, Wall.FRONT, sensor1Segment, sensor2Segment)) + else if (wallInRangeOfRays(covariance, Wall.FRONT, sensor1Segment, sensor2Segment)) return Wall.FRONT; - else if (wallInRangeOfRays(covariance, robotPose.heading, Wall.BACK, sensor1Segment, sensor2Segment)) + else if (wallInRangeOfRays(covariance, Wall.BACK, sensor1Segment, sensor2Segment)) return Wall.BACK; else return null; } - private boolean wallInRangeOfRays(Pose variance, double heading, Wall wall, Ray... rays) { + private boolean wallInRangeOfRays(Pose variance, Wall wall, Ray... rays) { for (var sensorRay : rays) { var wallIntersection = sensorRay.intersection(wall.getSegment()); if (wallIntersection == null) diff --git a/TeamCode/src/main/java/com/kuriosityrobotics/firstforward/robot/sensors/IMU.java b/TeamCode/src/main/java/com/kuriosityrobotics/firstforward/robot/sensors/IMU.java index e9cabc6e..3cf70b40 100644 --- a/TeamCode/src/main/java/com/kuriosityrobotics/firstforward/robot/sensors/IMU.java +++ b/TeamCode/src/main/java/com/kuriosityrobotics/firstforward/robot/sensors/IMU.java @@ -1,8 +1,6 @@ package com.kuriosityrobotics.firstforward.robot.sensors; import static com.kuriosityrobotics.firstforward.robot.util.math.MathUtil.angleWrap; -import static com.kuriosityrobotics.firstforward.robot.util.math.MathUtil.mean; -import static com.kuriosityrobotics.firstforward.robot.util.math.MathUtil.median; import static com.kuriosityrobotics.firstforward.robot.util.math.MathUtil.rotate; import static java.lang.Math.abs; import static java.lang.Math.toDegrees; @@ -20,7 +18,6 @@ import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.util.ReadWriteFile; -import org.apache.commons.collections4.queue.CircularFifoQueue; import org.apache.commons.geometry.euclidean.twod.Vector2D; import org.firstinspires.ftc.robotcore.external.navigation.Acceleration; import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; @@ -41,7 +38,6 @@ public class IMU implements Module { public static final String CALIBRATION_FILE = "imu_calibration.json"; private static final File SETTINGS_FILE = AppUtil.getInstance().getSettingsFile(CALIBRATION_FILE); - private static final double BIAS_PER_REVOLUTION = toRadians(-1); private final BNO055IMU imu; private final ExtendedKalmanFilter filter; diff --git a/TeamCode/src/main/java/com/kuriosityrobotics/firstforward/robot/sensors/Odometry.java b/TeamCode/src/main/java/com/kuriosityrobotics/firstforward/robot/sensors/Odometry.java index 947004cb..941888b2 100644 --- a/TeamCode/src/main/java/com/kuriosityrobotics/firstforward/robot/sensors/Odometry.java +++ b/TeamCode/src/main/java/com/kuriosityrobotics/firstforward/robot/sensors/Odometry.java @@ -1,7 +1,5 @@ package com.kuriosityrobotics.firstforward.robot.sensors; -import static com.kuriosityrobotics.firstforward.robot.pathfollow.motionprofiling.MotionProfile.ROBOT_MAX_ACCEL; -import static com.kuriosityrobotics.firstforward.robot.pathfollow.motionprofiling.MotionProfile.ROBOT_MAX_DECCEL; import static com.kuriosityrobotics.firstforward.robot.util.math.MathUtil.rotate; import static java.lang.Math.pow; @@ -37,10 +35,6 @@ public class Odometry extends RollingVelocityCalculator implements Module, Locat private double xVel = 0; private double yVel = 0; private double angleVel = 0; - // old vel for acceleration calculations - private double oldxVel = 0; - private double oldyVel = 0; - private double oldangleVel = 0; // change in position of the robot private double dx = 0; private double dy = 0; @@ -55,8 +49,6 @@ public class Odometry extends RollingVelocityCalculator implements Module, Locat private double oldY = 0; private double oldHeading = 0; private double lastUpdateTime = 0; - private double thetaLR = 0; - private double thetaFB = 0; public Odometry(HardwareMap hardwareMap, Pose pose, ExtendedKalmanFilter kalmanFilter, IMU imu) { this.worldX = pose.x; @@ -76,28 +68,18 @@ public Odometry(HardwareMap hardwareMap, Pose pose, ExtendedKalmanFilter kalmanF FileDump.addField("yVel", this); } - private double accelerationSigmoid(double dq, double accel) { - if (accel > 0) - return dq * (imu.isOffGround() /*|| accel <= 2 * ROBOT_MAX_ACCEL*/ ? 2 : .8); - else - return dq * (imu.isOffGround() /*|| -accel <= 2 * ROBOT_MAX_DECCEL*/ ? .8 : 2); - - } - public void update() { var now = SystemClock.elapsedRealtime(); - var dt = (now - lastUpdateTime) / 1000.; + var dt = (now - lastUpdateTime) / 1000.; if (lastUpdateTime == 0) dt = 1. / maxFrequency(); calculatePosition(); - var accel = imu.getAcceleration(); - var headingDeviation = accelerationSigmoid(dHeading, Math.hypot(accel.x, accel.y)); - calculateInstantaneousVelocity(); this.calculateRollingVelocity(new PoseInstant(getPose(), SystemClock.elapsedRealtime() / 1000.0)); + var accel = imu.getAcceleration(); kalmanFilter.builder() .time(now) .mean(dx, dy, dHeading) @@ -105,7 +87,7 @@ public void update() { .variance( pow(dt * accel.x * .3, 2), pow(dt * accel.y * .3, 2), - pow(headingDeviation, 2) + pow(dHeading * (imu.isOffGround() ? 2 : .8), 2) ) .predict(); } @@ -143,10 +125,6 @@ private void calculateInstantaneousVelocity() { oldY = worldY; oldHeading = worldHeadingRad; - oldxVel = xVel; - oldyVel = yVel; - oldangleVel = angleVel; - lastUpdateTime = currentUpdateTime; } @@ -165,9 +143,6 @@ private void updateWorldPosition(double dLeftPod, double dRightPod, double dMeca double dThetaLR = (dLeftPodInches - dRightPodInches) / (2 * P); double dThetaFB = (dMecanumFrontPodInches - dMecanumBackPodInches) / (2 * Q); - thetaLR += dThetaLR; - thetaFB += dThetaFB; - double X = Math.abs(dLeftPodInches + dRightPodInches); double Y = Math.abs(dMecanumFrontPodInches + dMecanumBackPodInches); @@ -284,10 +259,6 @@ public Pose getInstantaneousVelocity() { return new Pose(xVel, yVel, angleVel); } - public double getVelMag() { - return Math.hypot(xVel, yVel); - } - @Override public String getName() { return "Odometry"; diff --git a/TeamCode/src/main/java/com/kuriosityrobotics/firstforward/robot/sensors/kf/ExtendedKalmanFilter.java b/TeamCode/src/main/java/com/kuriosityrobotics/firstforward/robot/sensors/kf/ExtendedKalmanFilter.java index 87acf137..f2227024 100644 --- a/TeamCode/src/main/java/com/kuriosityrobotics/firstforward/robot/sensors/kf/ExtendedKalmanFilter.java +++ b/TeamCode/src/main/java/com/kuriosityrobotics/firstforward/robot/sensors/kf/ExtendedKalmanFilter.java @@ -140,8 +140,6 @@ private void backwardPass(int before) { } history.forEach(state -> { -// if (!state.getMean().equals(smoothedMeans.get(state))) -// Log.d("EKF/smooth", format("before: {0}, after: {1}", state.getMean(), smoothedMeans.get(state))); state.setMean(smoothedMeans.get(state)); state.setCovariance(smoothedVariances.get(state)); }); diff --git a/TeamCode/src/main/java/com/kuriosityrobotics/firstforward/robot/util/math/Point.java b/TeamCode/src/main/java/com/kuriosityrobotics/firstforward/robot/util/math/Point.java index 3ffff077..ebdb1792 100644 --- a/TeamCode/src/main/java/com/kuriosityrobotics/firstforward/robot/util/math/Point.java +++ b/TeamCode/src/main/java/com/kuriosityrobotics/firstforward/robot/util/math/Point.java @@ -3,6 +3,7 @@ import static com.kuriosityrobotics.firstforward.robot.util.Constants.Field.FULL_FIELD; import static com.kuriosityrobotics.firstforward.robot.util.math.MathUtil.doublesEqual; import static java.lang.Math.cos; +import static java.lang.Math.hypot; import static java.lang.Math.sin; import androidx.annotation.NonNull; @@ -112,5 +113,9 @@ public Point add(Point point) { public Point rotate(double angle) { return new Point(x * cos(angle) + y * sin(angle), -x * sin(angle) + y * cos(angle)); } + + public double norm() { + return hypot(x, y); + } } diff --git a/TeamCode/src/main/java/com/kuriosityrobotics/firstforward/robot/util/math/Pose.java b/TeamCode/src/main/java/com/kuriosityrobotics/firstforward/robot/util/math/Pose.java index f185c980..b335c705 100644 --- a/TeamCode/src/main/java/com/kuriosityrobotics/firstforward/robot/util/math/Pose.java +++ b/TeamCode/src/main/java/com/kuriosityrobotics/firstforward/robot/util/math/Pose.java @@ -37,10 +37,6 @@ public static Pose fieldMirror(double x, double y, double heading) { return new Pose(FULL_FIELD - x, y, angleWrap(-heading)); } - public double norm() { - return hypot(x, y); - } - public static Pose relativeMirror(double x, double y, double heading) { return new Pose(-x, y, angleWrap(-heading)); }