logging

Team766 · Jan 30, 2024 · 6f7bfbe · 6f7bfbe
1 parent cb99a36
commit 6f7bfbe
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 0 deletions.
diff --git a/src/main/java/com/team766/odometry/Odometry.java b/src/main/java/com/team766/odometry/Odometry.java
@@ -78,6 +78,13 @@ public Odometry(GyroReader gyro, MotorController[] motors, CANcoder[] CANCoders,
         WHEEL_CIRCUMFERENCE = wheelCircumference;
         GEAR_RATIO = gearRatio;
         ENCODER_TO_REVOLUTION_CONSTANT = encoderToRevolutionConstant;
+        currentPosition = new Pose2d(0, 0, new Rotation2d());
+		for (int i = 0; i < motorCount; i++) {
+			prevPositions[i] = new Pose2d(0, 0, new Rotation2d());
+			currPositions[i] = new Pose2d(0, 0, new Rotation2d());
+			prevEncoderValues[i] = 0;
+			currEncoderValues[i] = 0;
+		}
     }
 
     /**

diff --git a/src/main/java/com/team766/robot/gatorade/OI.java b/src/main/java/com/team766/robot/gatorade/OI.java
@@ -107,6 +107,7 @@ public void run(Context context) {
                     && Math.abs(leftJoystickX) + Math.abs(leftJoystickY) + Math.abs(rightJoystickX)
                             > 0) {
                 context.takeOwnership(Robot.drive);
+                log("current pos: "+Robot.drive.getCurrentPosition());
                 // If a button is pressed, drive is just fine adjustment
                 if (rightJoystick.getButton(InputConstants.FINE_DRIVING)) {
                     Robot.drive.controlFieldOriented(