DIST SENSROS FULLY WORKING (POGER)

KuriosityRobotics · Apr 8, 2022 · ab1856a · ab1856a
1 parent eb1a5e1
commit ab1856a
Show file tree

Hide file tree

Showing 2 changed files with 17 additions and 10 deletions.
diff --git a/...c/main/java/com/kuriosityrobotics/firstforward/robot/sensors/DistanceSensorLocaliser.java b/...c/main/java/com/kuriosityrobotics/firstforward/robot/sensors/DistanceSensorLocaliser.java
@@ -32,19 +32,20 @@ public class DistanceSensorLocaliser implements Module, LocationProvider {
 
     private final LocationProvider locationProvider;
     private final ExtendedKalmanFilter filter;
-    private final SharpIRDistance frontLeft, backLeft/*, frontRight, backRight*/;
+    private final SharpIRDistance frontLeft, backLeft, frontRight, backRight;
 
     private double x, y, heading;
 
     public DistanceSensorLocaliser(LocationProvider locationProvider, ExtendedKalmanFilter filter,
-                                   SharpIRDistance frontLeft, SharpIRDistance backLeft) {
+                                   SharpIRDistance frontLeft, SharpIRDistance backLeft,
+                                   SharpIRDistance frontRight, SharpIRDistance backRight) {
         this.locationProvider = locationProvider;
         this.filter = filter;
 
         this.frontLeft = frontLeft;
         this.backLeft = backLeft;
-//        this.frontRight = hardwareMap.get(DistanceSensor.class, "frontRight");
-//        this.backRight = hardwareMap.get(DistanceSensor.class, "backRight");
+        this.frontRight = frontRight;
+        this.backRight = backRight;
     }
 
     private void processPartialState(Double[] sensorData) {
@@ -92,15 +93,15 @@ public void update() {
                 ));
         }
 
-       /* {
-            var bestWallRight = DistanceSensorPair.RIGHT.bestWall(locationProvider.getPose());
+        {
+            var bestWallRight = DistanceSensorPair.RIGHT.bestWall(Pose.of(filter.getVariance()), locationProvider.getPose());
             if (bestWallRight != null)
                 processPartialState(DistanceSensorPair.RIGHT.getPartialState(
-                        frontRight.getDistance(DistanceUnit.INCH),
-                        backRight.getDistance(DistanceUnit.INCH),
+                        frontRight.getDistance(),
+                        backRight.getDistance(),
                         bestWallRight
                 ));
-        }*/
+        }
     }
 
     @Override

diff --git a/TeamCode/src/main/java/com/kuriosityrobotics/firstforward/robot/sensors/SensorThread.java b/TeamCode/src/main/java/com/kuriosityrobotics/firstforward/robot/sensors/SensorThread.java
@@ -55,11 +55,15 @@ public SensorThread(Robot robot) {
 
         var frontLeft = new SharpIRDistance(robot.getHardwareMap(), "frontLeft");
         var backLeft = new SharpIRDistance(robot.getHardwareMap(), "backLeft");
+        var frontRight = new SharpIRDistance(robot.getHardwareMap(), "frontRight");
+        var backRight = new SharpIRDistance(robot.getHardwareMap(), "backRight");
         this.distanceSensorLocaliser = new DistanceSensorLocaliser(
                 robot,
                 theKalmanFilter,
                 frontLeft,
-                backLeft
+                backLeft,
+                frontRight,
+                backRight
         );
 
         sensors = new HashSet<>();
@@ -69,6 +73,8 @@ public SensorThread(Robot robot) {
                 .chain(odometry)
                 .chain(frontLeft)
                 .chain(backLeft)
+                .chain(frontRight)
+                .chain(backRight)
                 .chain(distanceSensorLocaliser));
 
         sensors.forEach(robot.getTelemetryDump()::registerTelemeter);