diff --git a/src/main/java/com/team766/robot/common/mechanisms/BurroDrive.java b/src/main/java/com/team766/robot/common/mechanisms/BurroDrive.java index 39c3c44c..f9501f24 100644 --- 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,13 +33,16 @@ 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, @@ -47,8 +50,7 @@ public BurroDrive(double trackWidthMeters) { 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 index 5115e1e6..123183b1 100644 --- 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 index 3f0c939a..d22d6e86 100644 --- 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