diff --git a/src/main/kotlin/frc/team449/RobotLoop.kt b/src/main/kotlin/frc/team449/RobotLoop.kt index 7d9c57c..0b6647c 100644 --- a/src/main/kotlin/frc/team449/RobotLoop.kt +++ b/src/main/kotlin/frc/team449/RobotLoop.kt @@ -13,7 +13,6 @@ import frc.team449.auto.RoutineChooser import frc.team449.commands.light.BlairChasing import frc.team449.commands.light.BreatheHue import frc.team449.commands.light.Rainbow -import frc.team449.subsystems.FieldConstants import frc.team449.subsystems.drive.swerve.SwerveSim import frc.team449.subsystems.vision.VisionConstants import monologue.Annotations.Log @@ -101,18 +100,6 @@ class RobotLoop : TimedRobot(), Logged { } else { BreatheHue(robot.light, 95).schedule() } - - if (DriverStation.getAlliance().getOrNull() == DriverStation.Alliance.Red) { - FieldConstants.SPEAKER_POSE = FieldConstants.RED_SPEAKER_POSE - } else if (DriverStation.getAlliance().getOrNull() == DriverStation.Alliance.Blue) { - FieldConstants.SPEAKER_POSE = FieldConstants.BLUE_SPEAKER_POSE - } - - if (DriverStation.getAlliance().getOrNull() == DriverStation.Alliance.Red) { - FieldConstants.PASS_POSE = FieldConstants.RED_PASS_POSE - } else if (DriverStation.getAlliance().getOrNull() == DriverStation.Alliance.Blue) { - FieldConstants.PASS_POSE = FieldConstants.BLUE_PASS_POSE - } } override fun autonomousPeriodic() {} @@ -125,18 +112,6 @@ class RobotLoop : TimedRobot(), Logged { (robot.light.currentCommand ?: InstantCommand()).cancel() robot.drive.defaultCommand = robot.driveCommand - - if (DriverStation.getAlliance().getOrNull() == DriverStation.Alliance.Red) { - FieldConstants.SPEAKER_POSE = FieldConstants.RED_SPEAKER_POSE - } else if (DriverStation.getAlliance().getOrNull() == DriverStation.Alliance.Blue) { - FieldConstants.SPEAKER_POSE = FieldConstants.BLUE_SPEAKER_POSE - } - - if (DriverStation.getAlliance().getOrNull() == DriverStation.Alliance.Red) { - FieldConstants.PASS_POSE = FieldConstants.RED_PASS_POSE - } else if (DriverStation.getAlliance().getOrNull() == DriverStation.Alliance.Blue) { - FieldConstants.PASS_POSE = FieldConstants.BLUE_PASS_POSE - } } override fun teleopPeriodic() { diff --git a/src/main/kotlin/frc/team449/subsystems/FieldConstants.kt b/src/main/kotlin/frc/team449/subsystems/FieldConstants.kt index 93a8dd6..4206aea 100644 --- a/src/main/kotlin/frc/team449/subsystems/FieldConstants.kt +++ b/src/main/kotlin/frc/team449/subsystems/FieldConstants.kt @@ -1,19 +1,6 @@ package frc.team449.subsystems -import edu.wpi.first.math.geometry.Translation2d -import edu.wpi.first.math.util.Units - object FieldConstants { const val fieldLength = 16.4592 const val fieldWidth = 8.2296 - - /** Find these constants in meters for the blue alliance */ - val BLUE_SPEAKER_POSE = Translation2d(Units.inchesToMeters(-1.5), Units.inchesToMeters(218.42)) - val RED_SPEAKER_POSE = Translation2d(Units.inchesToMeters(652.73), Units.inchesToMeters(218.42)) - - val RED_PASS_POSE = Translation2d(14.50, 6.746) - val BLUE_PASS_POSE = Translation2d(fieldLength - 14.50, 6.746) - - var SPEAKER_POSE = BLUE_SPEAKER_POSE - var PASS_POSE = BLUE_PASS_POSE } diff --git a/src/main/kotlin/frc/team449/subsystems/RobotConstants.kt b/src/main/kotlin/frc/team449/subsystems/RobotConstants.kt index ece8515..1954f16 100644 --- a/src/main/kotlin/frc/team449/subsystems/RobotConstants.kt +++ b/src/main/kotlin/frc/team449/subsystems/RobotConstants.kt @@ -18,10 +18,7 @@ object RobotConstants { const val ROTATION_DEADBAND = .125 val SNAP_TO_ANGLE_TOLERANCE_RAD = Units.degreesToRadians(3.5) - /** In kilograms, include bumpers and battery and all */ - const val ROBOT_WEIGHT = 55.0 // TODO: find - - /** Drive configuration */ + /** Drive Configuration */ val MAX_LINEAR_SPEED = SwerveConstants.MAX_ATTAINABLE_MK4I_SPEED // m/s const val MAX_ROT_SPEED = 5.804 * PI / 4 // r ad/s @@ -48,10 +45,8 @@ object RobotConstants { const val ALIGN_ROT_SPEED = 7 * PI / 2 - val IR_CHANNEL = 3 - val IR_CHANNEL2 = 4 - - // Robot dimensions (INCLUDING BUMPERS) + // Robot Dimensions (INCLUDING BUMPERS) val ROBOT_WIDTH = Units.inchesToMeters(27.25 + 3.25 * 2) val ROBOT_LENGTH = Units.inchesToMeters(27.5 + 3.25 * 2) + val ROBOT_WEIGHT = 55.0 // TODO: find (incl. bumpers + battery) (kg) } diff --git a/src/main/kotlin/frc/team449/subsystems/drive/swerve/SwerveSim.kt b/src/main/kotlin/frc/team449/subsystems/drive/swerve/SwerveSim.kt index 369588d..ca0e6de 100644 --- a/src/main/kotlin/frc/team449/subsystems/drive/swerve/SwerveSim.kt +++ b/src/main/kotlin/frc/team449/subsystems/drive/swerve/SwerveSim.kt @@ -25,7 +25,7 @@ class SwerveSim( Pose2d() ) - var odometryPose = odometryTracker.poseMeters + var odometryPose: Pose2d = odometryTracker.poseMeters override fun periodic() { val currTime = getFPGATimestamp()