fixed issues, somewhat working in sim

simConfig.txt

@@ -1,10 +1,11 @@
 {
+	"robotConfigurator": "com.team766.robot.rookie_bot.Robot",
 	"drive": {
-		"leftMotor": {
+		"Left": {
 			"deviceId": 6,
 			"sensorScale": 0.0524
 		},
-		"rightMotor": {
+		"Right": {
 			"deviceId": 4,
 			"sensorScale": 0.0524
 		},

src/main/java/com/team766/odometry/DifferentialDriveOdometry.java

@@ -1,7 +1,9 @@
 package com.team766.odometry;
-
 import com.team766.hal.MotorController;
 import com.team766.library.RateLimiter;
+import com.team766.logging.Category;
+import com.team766.logging.Logger;
+import com.team766.logging.Severity;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 
@@ -70,11 +72,11 @@ public void setCurrentPosition(Pose2d position) {
      * Updates the encoder values for both the left and right wheels.
      */
     private void setCurrentEncoderValues() {
-        currLeftEncoderValue = leftMotor.getSensorPosition();
-        currRightEncoderValue = rightMotor.getSensorPosition();
-
         prevLeftEncoderValue = currLeftEncoderValue;
         prevRightEncoderValue = currRightEncoderValue;
+
+        currLeftEncoderValue = leftMotor.getSensorPosition();
+        currRightEncoderValue = rightMotor.getSensorPosition();
     }
 
     /**
@@ -89,6 +91,9 @@ private void updatePosition() {
                 (currRightEncoderValue - prevRightEncoderValue)
                         * (wheelCircumference / (gearRatio * encoderToRevolutionConstant));
 
+        Logger.get(Category.ODOMETRY).logRaw(Severity.DEBUG, "left: " + deltaLeft);
+        Logger.get(Category.ODOMETRY).logRaw(Severity.DEBUG, "right: " + deltaRight);
+
         // Calculate the distance the robot traveled and the rotation change
         double distanceTraveled = (deltaLeft + deltaRight) / 2;
         double rotationChange = (deltaRight - deltaLeft) / wheelBaseWidth;

src/main/java/com/team766/robot/common/mechanisms/BurroDrive.java

@@ -25,7 +25,7 @@ public class BurroDrive extends Drive {
     private static final double DRIVE_GEAR_RATIO = 1; // Gear ratio
 
     // TODO: set actual radius
-    private static final double WHEEL_RADIUS = 1; // Radius of the wheels
+    private static final double WHEEL_RADIUS = 0.05; // Radius of the wheels
 
     private static final double MOTOR_WHEEL_FACTOR_MPS =
             1.
@@ -34,7 +34,7 @@ public class BurroDrive extends Drive {
                     / (2 * Math.PI); // Motor rotations/sec (what velocity mode takes));
 
     // TODO: set actual track width
-    private static final double TRACK_WIDTH_METERS = 1.; // distance between left wheel and right wheel
+    private static final double TRACK_WIDTH_METERS = 1.05; // distance between left wheel and right wheel
 
     public BurroDrive() {
         loggerCategory = Category.DRIVE;
@@ -49,7 +49,7 @@ public BurroDrive() {
                         rightMotor,
                         WHEEL_RADIUS * 2 * Math.PI,
                         DRIVE_GEAR_RATIO,
-                        1.,
+                        20,
                         TRACK_WIDTH_METERS);
     }
 
@@ -111,4 +111,10 @@ public ChassisSpeeds getChassisSpeeds() {
     }
 
     public void setCross() {}
+
+    @Override
+    public void run() {
+        differentialDriveOdometry.run();
+        log(differentialDriveOdometry.getCurrentPosition().toString());
+    }
 }

src/main/java/com/team766/robot/rookie_bot/OI.java

@@ -25,12 +25,15 @@ public OI() {
     }
 
     public void run(final Context context) {
+
+        context.takeOwnership(Robot.drive);
+
         while (true) {
             // wait for driver station data (and refresh it using the WPILib APIs)
             context.waitFor(() -> RobotProvider.instance.hasNewDriverStationData());
             RobotProvider.instance.refreshDriverStationData();
 
-            Robot.drive.drive(joystick0.getAxis(1), joystick0.getAxis(4));
+            Robot.drive.drive(joystick0.getAxis(1), joystick1.getAxis(0));
 
             // Add driver controls here - make sure to take/release ownership
             // of mechanisms when appropriate.