drive code

Team766 · Nov 21, 2024 · ae06266 · ae06266
1 parent c01e533
commit ae06266
Show file tree

Hide file tree

Showing 3 changed files with 13 additions and 6 deletions.
diff --git a/src/main/java/com/team766/robot/common/mechanisms/BurroDrive.java b/src/main/java/com/team766/robot/common/mechanisms/BurroDrive.java
@@ -21,10 +21,10 @@ public class BurroDrive extends Drive {
     private DifferentialDriveKinematics differentialDriveKinematics;
     private DifferentialDriveOdometry differentialDriveOdometry;
 
-    // todo set actual ratio
+    // TODO: set actual ratio
     private static final double DRIVE_GEAR_RATIO = 1; // Gear ratio
 
-    // todo set actual radius
+    // TODO: set actual radius
     private static final double WHEEL_RADIUS = 1; // Radius of the wheels
 
     private static final double MOTOR_WHEEL_FACTOR_MPS =
@@ -33,22 +33,24 @@ public class BurroDrive extends Drive {
                     * DRIVE_GEAR_RATIO // Motor radians/sec
                     / (2 * Math.PI); // Motor rotations/sec (what velocity mode takes));
 
-    public BurroDrive(double trackWidthMeters) {
+    // TODO: set actual track width
+    private static final double TRACK_WIDTH_METERS = 1.; // distance between left wheel and right wheel
+
+    public BurroDrive() {
         loggerCategory = Category.DRIVE;
 
         leftMotor = RobotProvider.instance.getMotor(DRIVE_LEFT);
         rightMotor = RobotProvider.instance.getMotor(DRIVE_RIGHT);
 
-        differentialDriveKinematics = new DifferentialDriveKinematics(trackWidthMeters);
+        differentialDriveKinematics = new DifferentialDriveKinematics(TRACK_WIDTH_METERS);
         differentialDriveOdometry =
                 new DifferentialDriveOdometry(
                         leftMotor,
                         rightMotor,
                         WHEEL_RADIUS * 2 * Math.PI,
                         DRIVE_GEAR_RATIO,
                         1.,
-                        0 // TODO
-                        );
+                        TRACK_WIDTH_METERS);
     }
 
     /**

diff --git a/src/main/java/com/team766/robot/rookie_bot/OI.java b/src/main/java/com/team766/robot/rookie_bot/OI.java
@@ -30,6 +30,8 @@ public void run(final Context context) {
             context.waitFor(() -> RobotProvider.instance.hasNewDriverStationData());
             RobotProvider.instance.refreshDriverStationData();
 
+            Robot.drive.drive(joystick0.getAxis(1), joystick0.getAxis(4));
+
             // Add driver controls here - make sure to take/release ownership
             // of mechanisms when appropriate.
         }

diff --git a/src/main/java/com/team766/robot/rookie_bot/Robot.java b/src/main/java/com/team766/robot/rookie_bot/Robot.java
@@ -3,14 +3,17 @@
 import com.team766.framework.AutonomousMode;
 import com.team766.framework.Procedure;
 import com.team766.hal.RobotConfigurator;
+import com.team766.robot.common.mechanisms.BurroDrive;
 import com.team766.robot.example.mechanisms.*;
 
 public class Robot implements RobotConfigurator {
     // Declare mechanisms (as static fields) here
+    public static BurroDrive drive;
 
     @Override
     public void initializeMechanisms() {
         // Initialize mechanisms here
+        drive = new BurroDrive();
     }
 
     @Override