Add OdometryUpdater, fix DistanceMovement for relative movement, chan…

…ge VirtualField for new changes, and add an odometryUpdater to OdometryMovementTest

TeamCode/src/main/java/centerstage/auto/OdometryMovementTest.java

@@ -6,41 +6,38 @@
 import com.pocolifo.robobase.bootstrap.AutonomousOpMode;
 import com.pocolifo.robobase.novel.hardware.NovelOdometry;
 import com.pocolifo.robobase.novel.motion.NovelMecanumDriver;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.hardware.DcMotor;
+import com.pocolifo.robobase.virtualfield.OdometryUpdater;
 
 @Autonomous
 public class OdometryMovementTest extends AutonomousOpMode {
     private CenterStageRobotConfiguration c;
     private NovelOdometry odometry;
     private NovelMecanumDriver driver;
     private DistanceMovement movement;
+    private OdometryUpdater updater;
 
     @Override
     public void initialize() {
         this.c = new CenterStageRobotConfiguration(this.hardwareMap);
         this.driver = this.c.createDriver(Constants.Coefficients.SOFTWARE_ROBOT_COEFFICIENTS);
         this.odometry = this.c.createOdometry();
         this.movement = new DistanceMovement(driver, odometry, c.imu);
+        this.updater = new OdometryUpdater(odometry);
     }
 
     @Override
     public void run() {
-//        try {
-            this.c.fr.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-            this.c.fl.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-            this.c.br.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-            this.c.bl.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-
-             movement.rotateTo(180);
-
-//            Thread.sleep(2000);
-//            movement.moveTo(new Vector3D(0, 0, 990));
-            System.out.println("ive moveddd");
-
-            this.driver.stop();
-//        } catch (InterruptedException e) {
-//            e.printStackTrace();
-//        }
+        updater.start();
+
+        this.c.fr.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        this.c.fl.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        this.c.br.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        this.c.bl.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+
+        movement.rotate(180);
+
+        System.out.println("ive moveddd");
+
+        this.driver.stop();
     }
 }

TeamCode/src/main/java/com/pocolifo/robobase/virtualfield/DistanceMovement.java

@@ -3,10 +3,6 @@
 import com.pocolifo.robobase.novel.hardware.NovelOdometry;
 import com.pocolifo.robobase.novel.motion.NovelMecanumDriver;
 import com.pocolifo.robobase.reconstructor.Pose;
-import com.qualcomm.robotcore.hardware.IMU;
-
-import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
-import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
 
 public class DistanceMovement {
     private static final double PRECISION_IN = 0.5;
@@ -17,45 +13,44 @@ public class DistanceMovement {
     private final IMU imu;
     private double positionalDifference = -1;
     private double rotationaldifference = -1;
-    private Vector3D position;
 
     public DistanceMovement(NovelMecanumDriver movementController, NovelOdometry odometry, IMU imu) {
         this.movementController = movementController;
         this.odometry = odometry;
         this.imu = imu;
     }
 
-    public void updatePositionalAndRotationalDifference(Vector3D target) {
-        position = getPosition();
-        odometry.update();
-        positionalDifference = Math.sqrt(Math.pow(position.getX() - target.getX(), 2) + Math.pow(position.getY() - target.getY(), 2));
-        rotationaldifference = Math.abs(position.getZ() - target.getZ());
+    public void updatePositionalAndRotationalDifference(Vector3D movement, Vector3D position) {
+        positionalDifference = Math.sqrt(Math.pow(position.getX() - target.getX(), 2) + Math.pow(position.getY() - movement.getY(), 2));
+        rotationaldifference = Math.abs(position.getZ() - movement.getZ());
     }
 
-    private void goToPosition(Vector3D target) {
+    private void moveBy(Vector3D movement) {
 
-        updatePositionalAndRotationalDifference(target);
+        updatePositionalAndRotationalDifference(movement);
+        Vector3D position = getPosition();
 
         while (positionalDifference > PRECISION_IN || rotationaldifference > PERCISION_DEG) {
-            updatePositionalAndRotationalDifference(target);
+            position = getPosition();
+            updatePositionalAndRotationalDifference(movement, position);
             System.out.println(position);
-            Vector3D velocity = getNewVelocity(position, target, SPEED);
+            Vector3D velocity = getNewVelocity(position, movement, SPEED);
 
             movementController.setVelocity(velocity);
         }
         movementController.setVelocity(Vector3D.ZERO);
     }
 
-    public void rotateTo(double degrees) {
-        goToPosition(new Vector3D(getPosition().getX(), getPosition().getY(), degrees));
+    public void rotate(double degrees) {
+        moveBy(new Vector3D(0, 0, degrees));
     }
 
-    public void transformTo(double x, double y) {
-        goToPosition(new Vector3D(x, y, getPosition().getZ()));
+    public void transform(double x, double y) {
+        moveBy(new Vector3D(x, y, 0));
     }
 
-    public void moveTo(double x, double y, double degrees) {
-        goToPosition(new Vector3D(x, y, degrees));
+    public void move(double x, double y, double degrees) {
+        moveBy(new Vector3D(x, y, degrees));
     }
 
     public Vector3D poseToVector3D(Pose pose) {

TeamCode/src/main/java/com/pocolifo/robobase/virtualfield/OdometryUpdater.java

@@ -0,0 +1,33 @@
+package com.pocolifo.robobase.virtualfield;
+
+import com.pocolifo.robobase.novel.hardware.NovelOdometry;
+
+public class OdometryUpdater extends Thread {
+    private NovelOdometry odometry;
+    private int updatesPerSecond;
+    private boolean running;
+    public void stopRunning() {
+        running = false;
+    }
+
+    public OdometryUpdater(NovelOdometry odometry) {
+        this(odometry, 500);
+    }
+
+    public OdometryUpdater(NovelOdometry odometry, int updatesPerSecond) {
+        this.odometry = odometry;
+        this.updatesPerSecond = updatesPerSecond;
+        running = true;
+    }
+    @Override
+    public void run() {
+        while (running) {
+            odometry.update();
+            try {
+                Thread.sleep(1000 / updatesPerSecond);
+            } catch (InterruptedException e) {
+                throw new RuntimeException(e);
+            }
+        }
+    }
+}

TeamCode/src/main/java/com/pocolifo/robobase/virtualfield/VirtualField.java

@@ -5,10 +5,7 @@
 import com.pocolifo.robobase.reconstructor.PathFinder;
 import com.pocolifo.robobase.reconstructor.Pose;
 
-import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
-
 import java.io.IOException;
-import java.util.List;
 
 import centerstage.CenterStageRobotConfiguration;
 
@@ -31,18 +28,25 @@ public Vector3D getFieldPosition() {
 
     private void resetRotation() {
         double rotationNeeded = 0 - getFieldPosition().getZ();
-        movement.rotateTo(rotationNeeded);
+        movement.rotate(rotationNeeded);
+    }
+
+    public void rotateTo(int degrees) {
+        int degreesNeeded = degrees - getFieldPosition().getZ();
+        movement.rotate(degreesNeeded);
     }
 
-    public void pathTo(int x, int y) {
+    public void pathTo(int x, int y, int degrees) {
         resetRotation();
 
         List<Vector3D> path = pathFinder.findPath(getFieldPosition(), new Vector3D(x, y, 0));
 
         for (Vector3D point : path) {
             Vector3D diff = point.subtract(getFieldPosition());
 
-            movement.transformTo(diff.getX(), diff.getY());
+            movement.move(diff.getX(), diff.getY());
         }
+
+        rotateTo(degrees);
     }
 }