diff --git a/src/main/java/com/team766/robot/common/DriverOI.java b/src/main/java/com/team766/robot/common/DriverOI.java index 28081d4f6..df647410e 100644 --- a/src/main/java/com/team766/robot/common/DriverOI.java +++ b/src/main/java/com/team766/robot/common/DriverOI.java @@ -5,13 +5,13 @@ import com.team766.hal.JoystickReader; import com.team766.robot.common.constants.ControlConstants; import com.team766.robot.common.constants.InputConstants; -import com.team766.robot.common.mechanisms.Drive; +import com.team766.robot.common.mechanisms.SwerveDrive; public class DriverOI extends OIFragment { protected static final double FINE_DRIVING_COEFFICIENT = 0.25; - protected final Drive drive; + protected final SwerveDrive drive; protected final JoystickReader leftJoystick; protected final JoystickReader rightJoystick; protected double rightJoystickY = 0; @@ -21,7 +21,7 @@ public class DriverOI extends OIFragment { private final OICondition movingJoysticks; - public DriverOI(Drive drive, JoystickReader leftJoystick, JoystickReader rightJoystick) { + public DriverOI(SwerveDrive drive, JoystickReader leftJoystick, JoystickReader rightJoystick) { this.drive = drive; this.leftJoystick = leftJoystick; this.rightJoystick = rightJoystick; diff --git a/src/main/java/com/team766/robot/common/mechanisms/Drive.java b/src/main/java/com/team766/robot/common/mechanisms/SwerveDrive.java similarity index 99% rename from src/main/java/com/team766/robot/common/mechanisms/Drive.java rename to src/main/java/com/team766/robot/common/mechanisms/SwerveDrive.java index 761e7edc8..126214492 100644 --- a/src/main/java/com/team766/robot/common/mechanisms/Drive.java +++ b/src/main/java/com/team766/robot/common/mechanisms/SwerveDrive.java @@ -27,7 +27,7 @@ import java.util.Optional; import org.apache.commons.math3.geometry.euclidean.twod.Vector2D; -public class Drive extends Mechanism { +public class SwerveDrive extends Mechanism { private final SwerveConfig config; @@ -58,7 +58,7 @@ public class Drive extends Mechanism { private double x; private double y; - public Drive(SwerveConfig config) { + public SwerveDrive(SwerveConfig config) { loggerCategory = Category.DRIVE; this.config = config; diff --git a/src/main/java/com/team766/robot/common/procedures/FollowPath.java b/src/main/java/com/team766/robot/common/procedures/FollowPath.java index a9d526aaa..c768efb2e 100644 --- a/src/main/java/com/team766/robot/common/procedures/FollowPath.java +++ b/src/main/java/com/team766/robot/common/procedures/FollowPath.java @@ -7,7 +7,7 @@ import com.team766.framework.Context; import com.team766.framework.Procedure; import com.team766.robot.common.constants.PathPlannerConstants; -import com.team766.robot.common.mechanisms.Drive; +import com.team766.robot.common.mechanisms.SwerveDrive; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.DriverStation; @@ -16,7 +16,7 @@ import java.util.Optional; public class FollowPath extends Procedure { - private final Drive drive; + private final SwerveDrive drive; private PathPlannerPath path; // may be flipped private final ReplanningConfig replanningConfig; private final PPHolonomicDriveController controller; @@ -27,14 +27,14 @@ public FollowPath( PathPlannerPath path, ReplanningConfig replanningConfig, PPHolonomicDriveController controller, - Drive drive) { + SwerveDrive drive) { this.path = path; this.replanningConfig = replanningConfig; this.controller = controller; this.drive = drive; } - public FollowPath(String autoName, PPHolonomicDriveController controller, Drive drive) { + public FollowPath(String autoName, PPHolonomicDriveController controller, SwerveDrive drive) { this( PathPlannerPath.fromPathFile(autoName), PathPlannerConstants.REPLANNING_CONFIG, diff --git a/src/main/java/com/team766/robot/common/procedures/PathSequenceAuto.java b/src/main/java/com/team766/robot/common/procedures/PathSequenceAuto.java index 486f1d38b..8ee06be09 100644 --- a/src/main/java/com/team766/robot/common/procedures/PathSequenceAuto.java +++ b/src/main/java/com/team766/robot/common/procedures/PathSequenceAuto.java @@ -9,7 +9,7 @@ import com.team766.framework.RunnableWithContext; import com.team766.robot.common.constants.ConfigConstants; import com.team766.robot.common.constants.PathPlannerConstants; -import com.team766.robot.common.mechanisms.Drive; +import com.team766.robot.common.mechanisms.SwerveDrive; import com.team766.robot.reva.Robot; import com.team766.robot.reva.VisionUtil.VisionSpeakerHelper; import com.team766.robot.reva.procedures.MoveClimbersToBottom; @@ -22,7 +22,7 @@ public class PathSequenceAuto extends Procedure { private final LinkedList pathItems; - private final Drive drive; + private final SwerveDrive drive; private final Pose2d initialPosition; private final PPHolonomicDriveController controller; private VisionSpeakerHelper visionSpeakerHelper; @@ -32,7 +32,7 @@ public class PathSequenceAuto extends Procedure { * @param drive The instantiation of drive for the robot (pass in Robot.drive) * @param initialPosition Starting position on Blue Alliance in meters (gets flipped when on red) */ - public PathSequenceAuto(Drive drive, Pose2d initialPosition) { + public PathSequenceAuto(SwerveDrive drive, Pose2d initialPosition) { pathItems = new LinkedList(); this.drive = drive; this.controller = createDriveController(drive); @@ -40,7 +40,7 @@ public PathSequenceAuto(Drive drive, Pose2d initialPosition) { visionSpeakerHelper = new VisionSpeakerHelper(drive); } - private PPHolonomicDriveController createDriveController(Drive drive) { + private PPHolonomicDriveController createDriveController(SwerveDrive drive) { double maxSpeed = ConfigFileReader.getInstance() .getDouble(ConfigConstants.PATH_FOLLOWING_MAX_MODULE_SPEED_MPS) diff --git a/src/main/java/com/team766/robot/gatorade/Robot.java b/src/main/java/com/team766/robot/gatorade/Robot.java index 55ba4f0b6..737d4873f 100644 --- a/src/main/java/com/team766/robot/gatorade/Robot.java +++ b/src/main/java/com/team766/robot/gatorade/Robot.java @@ -4,7 +4,7 @@ import com.team766.framework.Procedure; import com.team766.hal.RobotConfigurator; import com.team766.robot.common.SwerveConfig; -import com.team766.robot.common.mechanisms.Drive; +import com.team766.robot.common.mechanisms.SwerveDrive; import com.team766.robot.gatorade.constants.SwerveDriveConstants; import com.team766.robot.gatorade.mechanisms.*; @@ -14,7 +14,7 @@ public class Robot implements RobotConfigurator { public static Wrist wrist; public static Elevator elevator; public static Shoulder shoulder; - public static Drive drive; + public static SwerveDrive drive; public static Lights lights; @Override @@ -24,7 +24,7 @@ public void initializeMechanisms() { wrist = new Wrist(); elevator = new Elevator(); shoulder = new Shoulder(); - drive = new Drive(config); + drive = new SwerveDrive(config); lights = new Lights(); } diff --git a/src/main/java/com/team766/robot/reva/DriverOI.java b/src/main/java/com/team766/robot/reva/DriverOI.java index 0423d2287..b8ad7e89b 100644 --- a/src/main/java/com/team766/robot/reva/DriverOI.java +++ b/src/main/java/com/team766/robot/reva/DriverOI.java @@ -5,7 +5,7 @@ import com.team766.framework.OIFragment; import com.team766.hal.JoystickReader; import com.team766.robot.common.constants.ControlConstants; -import com.team766.robot.common.mechanisms.Drive; +import com.team766.robot.common.mechanisms.SwerveDrive; import com.team766.robot.reva.VisionUtil.VisionSpeakerHelper; import com.team766.robot.reva.constants.InputConstants; import com.team766.robot.reva.mechanisms.Intake; @@ -19,7 +19,7 @@ public class DriverOI extends OIFragment { protected static final double FINE_DRIVING_COEFFICIENT = 0.25; protected VisionSpeakerHelper visionSpeakerHelper; - protected final Drive drive; + protected final SwerveDrive drive; protected final Shoulder shoulder; protected final Intake intake; protected final Shooter shooter; @@ -35,7 +35,7 @@ public class DriverOI extends OIFragment { private LaunchedContext visionContext; public DriverOI( - Drive drive, + SwerveDrive drive, Shoulder shoulder, Intake intake, Shooter shooter, diff --git a/src/main/java/com/team766/robot/reva/Robot.java b/src/main/java/com/team766/robot/reva/Robot.java index ea8c3acd5..c54ae82e8 100644 --- a/src/main/java/com/team766/robot/reva/Robot.java +++ b/src/main/java/com/team766/robot/reva/Robot.java @@ -6,7 +6,7 @@ import com.team766.hal.RobotConfigurator; import com.team766.logging.LoggerExceptionUtils; import com.team766.robot.common.SwerveConfig; -import com.team766.robot.common.mechanisms.Drive; +import com.team766.robot.common.mechanisms.SwerveDrive; import com.team766.robot.reva.mechanisms.Climber; import com.team766.robot.reva.mechanisms.ForwardApriltagCamera; import com.team766.robot.reva.mechanisms.Intake; @@ -17,7 +17,7 @@ public class Robot implements RobotConfigurator { // Declare mechanisms (as static fields) here - public static Drive drive; + public static SwerveDrive drive; public static Climber climber; public static Shoulder shoulder; public static Intake intake; @@ -31,7 +31,7 @@ public class Robot implements RobotConfigurator { public void initializeMechanisms() { SwerveConfig config = new SwerveConfig(); lights = new Lights(); - drive = new Drive(config); + drive = new SwerveDrive(config); climber = new Climber(); shoulder = new Shoulder(); intake = new Intake(); diff --git a/src/main/java/com/team766/robot/reva/VisionUtil/VisionSpeakerHelper.java b/src/main/java/com/team766/robot/reva/VisionUtil/VisionSpeakerHelper.java index 2768bfa69..3ef517c71 100644 --- a/src/main/java/com/team766/robot/reva/VisionUtil/VisionSpeakerHelper.java +++ b/src/main/java/com/team766/robot/reva/VisionUtil/VisionSpeakerHelper.java @@ -2,7 +2,7 @@ import com.team766.ViSIONbase.AprilTagGeneralCheckedException; import com.team766.ViSIONbase.GrayScaleCamera; -import com.team766.robot.common.mechanisms.Drive; +import com.team766.robot.common.mechanisms.SwerveDrive; import com.team766.robot.reva.Robot; import com.team766.robot.reva.constants.VisionConstants; import edu.wpi.first.math.geometry.Rotation2d; @@ -17,12 +17,12 @@ public class VisionSpeakerHelper { int tagId; double angle; GrayScaleCamera camera; - Drive drive; + SwerveDrive drive; Translation2d absTargetPos; Translation2d relativeTranslation2d; // TODO: make this static - public VisionSpeakerHelper(Drive drive) { + public VisionSpeakerHelper(SwerveDrive drive) { camera = Robot.forwardApriltagCamera.getCamera(); this.drive = drive; } diff --git a/src/main/java/com/team766/robot/swerveandshoot/Robot.java b/src/main/java/com/team766/robot/swerveandshoot/Robot.java index 7fb59152e..5208f1115 100644 --- a/src/main/java/com/team766/robot/swerveandshoot/Robot.java +++ b/src/main/java/com/team766/robot/swerveandshoot/Robot.java @@ -4,7 +4,7 @@ import com.team766.framework.Procedure; import com.team766.hal.RobotConfigurator; import com.team766.robot.common.SwerveConfig; -import com.team766.robot.common.mechanisms.Drive; +import com.team766.robot.common.mechanisms.SwerveDrive; import com.team766.robot.swerveandshoot.constants.SwerveDriveConstants; import com.team766.robot.swerveandshoot.mechanisms.*; @@ -13,7 +13,7 @@ public class Robot implements RobotConfigurator { public static TempPickerUpper tempPickerUpper; public static TempShooter tempShooter; public static Lights lights; - public static Drive drive; + public static SwerveDrive drive; public static NoteCamera noteDetectorCamera; public static ForwardApriltagCamera forwardApriltagCamera; @@ -23,7 +23,7 @@ public void initializeMechanisms() { tempPickerUpper = new TempPickerUpper(); tempShooter = new TempShooter(); lights = new Lights(); - drive = new Drive(config); + drive = new SwerveDrive(config); noteDetectorCamera = new NoteCamera(); forwardApriltagCamera = new ForwardApriltagCamera(); }