diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index aa6f0fa6..9fb9a959 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -13,14 +13,12 @@ import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOSim import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIO import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIOPigeon2 import com.team4099.robot2023.subsystems.elevator.Elevator -import com.team4099.robot2023.subsystems.elevator.ElevatorIO import com.team4099.robot2023.subsystems.elevator.ElevatorIONEO import com.team4099.robot2023.subsystems.elevator.ElevatorIOSim import com.team4099.robot2023.subsystems.feeder.Feeder import com.team4099.robot2023.subsystems.feeder.FeederIONeo import com.team4099.robot2023.subsystems.feeder.FeederIOSim import com.team4099.robot2023.subsystems.flywheel.Flywheel -import com.team4099.robot2023.subsystems.flywheel.FlywheelIO import com.team4099.robot2023.subsystems.flywheel.FlywheelIOSim import com.team4099.robot2023.subsystems.flywheel.FlywheelIOTalon import com.team4099.robot2023.subsystems.intake.Intake @@ -33,7 +31,6 @@ import com.team4099.robot2023.subsystems.superstructure.Request import com.team4099.robot2023.subsystems.superstructure.Superstructure import com.team4099.robot2023.subsystems.vision.Vision import com.team4099.robot2023.subsystems.vision.camera.CameraIO -import com.team4099.robot2023.subsystems.vision.camera.CameraIOPhotonvision import com.team4099.robot2023.subsystems.wrist.Wrist import com.team4099.robot2023.subsystems.wrist.WristIOSim import com.team4099.robot2023.subsystems.wrist.WristIOTalon @@ -82,7 +79,7 @@ object RobotContainer { limelight = LimelightVision(LimelightVisionIOReal) intake = Intake(IntakeIOFalconNEO) feeder = Feeder(FeederIONeo) - elevator = Elevator(object: ElevatorIO {}) + elevator = Elevator(ElevatorIONEO) flywheel = Flywheel(FlywheelIOTalon) wrist = Wrist(WristIOTalon) } else { diff --git a/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt b/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt index f696341c..3b43cd50 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt @@ -3,6 +3,8 @@ package com.team4099.robot2023.auto import com.team4099.robot2023.auto.mode.ExamplePathAuto import com.team4099.robot2023.auto.mode.FiveNoteAutoPath import com.team4099.robot2023.auto.mode.FourNoteAutoPath +import com.team4099.robot2023.auto.mode.FourNoteAutoPathWithFirstCenterSide +import com.team4099.robot2023.auto.mode.FourNoteAutoPathWithFirstSourceSide import com.team4099.robot2023.auto.mode.FourNoteLeftCenterLine import com.team4099.robot2023.auto.mode.FourNoteMiddleCenterLine import com.team4099.robot2023.auto.mode.FourNoteRightCenterLine @@ -46,7 +48,9 @@ object AutonomousSelector { // orientationChooser.addOption("Right", 270.degrees) // autoTab.add("Starting Orientation", orientationChooser) - autonomousModeChooser.addOption("Four Note Wing Auto", AutonomousMode.FOUR_NOTE_AUTO_PATH) + autonomousModeChooser.addOption("Four Note Wing Auto (Amp Side Note First, Default)", AutonomousMode.FOUR_NOTE_AUTO_PATH_WITH_AMP_SIDE_FIRST) + autonomousModeChooser.addOption("Four Note Wing Auto (Center Wing Note First)", AutonomousMode.FOUR_NOTE_AUTO_PATH_WITH_CENTER_SIDE_FIRST) + autonomousModeChooser.addOption("Four Note Wing Auto (Source Side Note First)", AutonomousMode.FOUR_NOTE_AUTO_PATH_WITH_SOURCE_SIDE_FIRST) /* @@ -169,7 +173,7 @@ object AutonomousSelector { ) }) .andThen(SixNoteAutoPath(drivetrain, superstructure)) - AutonomousMode.FOUR_NOTE_AUTO_PATH -> + AutonomousMode.FOUR_NOTE_AUTO_PATH_WITH_AMP_SIDE_FIRST -> return WaitCommand(waitTime.inSeconds) .andThen({ val flippedPose = AllianceFlipUtil.apply(FourNoteAutoPath.startingPose) @@ -177,6 +181,22 @@ object AutonomousSelector { drivetrain.resetFieldFrameEstimator(flippedPose) }) .andThen(FourNoteAutoPath(drivetrain, superstructure)) + AutonomousMode.FOUR_NOTE_AUTO_PATH_WITH_CENTER_SIDE_FIRST -> + return WaitCommand(waitTime.inSeconds) + .andThen({ + val flippedPose = AllianceFlipUtil.apply(FourNoteAutoPathWithFirstCenterSide.startingPose) + drivetrain.tempZeroGyroYaw(flippedPose.rotation) + drivetrain.resetFieldFrameEstimator(flippedPose) + }) + .andThen(FourNoteAutoPathWithFirstCenterSide(drivetrain, superstructure)) + AutonomousMode.FOUR_NOTE_AUTO_PATH_WITH_SOURCE_SIDE_FIRST -> + return WaitCommand(waitTime.inSeconds) + .andThen({ + val flippedPose = AllianceFlipUtil.apply(FourNoteAutoPathWithFirstSourceSide.startingPose) + drivetrain.tempZeroGyroYaw(flippedPose.rotation) + drivetrain.resetFieldFrameEstimator(flippedPose) + }) + .andThen(FourNoteAutoPathWithFirstSourceSide(drivetrain, superstructure)) AutonomousMode.FOUR_NOTE_RIGHT_AUTO_PATH -> return WaitCommand(waitTime.inSeconds) .andThen({ @@ -321,7 +341,9 @@ object AutonomousSelector { private enum class AutonomousMode { TEST_AUTO_PATH, - FOUR_NOTE_AUTO_PATH, + FOUR_NOTE_AUTO_PATH_WITH_AMP_SIDE_FIRST, + FOUR_NOTE_AUTO_PATH_WITH_CENTER_SIDE_FIRST, + FOUR_NOTE_AUTO_PATH_WITH_SOURCE_SIDE_FIRST, FOUR_NOTE_RIGHT_AUTO_PATH, FOUR_NOTE_MIDDLE_AUTO_PATH, FOUR_NOTE_LEFT_AUTO_PATH, @@ -338,4 +360,4 @@ object AutonomousSelector { SIX_NOTE_AUTO_PATH, SIX_NOTE_WITH_PICKUP_PATH } -} +} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPath.kt index a0f21fd5..c9c8f584 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPath.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPath.kt @@ -36,14 +36,14 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru startingPose.rotation.inRotation2ds ), FieldWaypoint( - Translation2d(2.9.meters - 0.4.meters, 6.9.meters + 0.1.meters) + Translation2d(2.91.meters - 0.75.meters, 6.82.meters) .translation2d, null, 180.degrees.inRotation2ds, ), FieldWaypoint( - Translation2d(2.9.meters, 6.9.meters + 0.1.meters).translation2d, - null, + Translation2d(2.91.meters - 0.25.meters, 7.meters).translation2d, + 0.degrees.inRotation2ds, 180.degrees.inRotation2ds, ), FieldWaypoint( @@ -73,13 +73,13 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru 180.degrees.inRotation2ds ), // Subwoofer FieldWaypoint( - Translation2d(2.41.meters - 0.4.meters, 4.1.meters).translation2d, + Translation2d(2.41.meters - 0.4.meters, 4.4.meters).translation2d, null, 180.degrees.inRotation2ds ), FieldWaypoint( - Translation2d(2.41.meters + 0.225.meters, 4.1.meters).translation2d, - null, + Translation2d(2.41.meters, 4.1.meters).translation2d, + 0.degrees.inRotation2ds, 180.degrees.inRotation2ds ), FieldWaypoint( @@ -111,7 +111,7 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru FieldWaypoint( Translation2d( ((1.43.meters) + (2.34.meters + 0.3.meters)) / 2 + - 0.25.meters, + 0.25.meters, 5.55.meters ) .translation2d, @@ -127,7 +127,7 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru FieldWaypoint( Translation2d( ((1.43.meters) + (2.34.meters + 0.3.meters)) / 2 + - 0.25.meters, + 0.25.meters, 5.45.meters ) .translation2d, @@ -181,6 +181,6 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru } companion object { - val startingPose = Pose2d(Translation2d(1.42.meters - 3.inches, 5.535.meters), 180.degrees) + val startingPose = Pose2d(Translation2d(1.42.meters + 1.inches, 5.535.meters), 180.degrees) } -} +} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPathWithFirstCenterSide.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPathWithFirstCenterSide.kt new file mode 100644 index 00000000..285831b4 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPathWithFirstCenterSide.kt @@ -0,0 +1,186 @@ +package com.team4099.robot2023.auto.mode + +import com.team4099.lib.trajectory.FieldWaypoint +import com.team4099.robot2023.commands.drivetrain.DrivePathCommand +import com.team4099.robot2023.config.constants.FlywheelConstants +import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.subsystems.superstructure.Superstructure +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup +import edu.wpi.first.wpilibj2.command.WaitCommand +import org.team4099.lib.geometry.Pose2d +import org.team4099.lib.geometry.Translation2d +import org.team4099.lib.units.base.inSeconds +import org.team4099.lib.units.base.inches +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inRotation2ds + +class FourNoteAutoPathWithFirstCenterSide(val drivetrain: Drivetrain, val superstructure: Superstructure) : + SequentialCommandGroup() { + init { + addRequirements(drivetrain, superstructure) + + addCommands( + superstructure.prepSpeakerLowCommand(), + superstructure.scoreCommand().withTimeout(0.5), + WaitCommand(0.5), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + startingPose.translation.translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d( + ((1.43.meters) + (2.34.meters + 0.3.meters)) / 2 + + 0.25.meters, + 5.55.meters + ) + .translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(2.34.meters + 0.3.meters + 0.25.meters, 5.535.meters) + .translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d( + ((1.43.meters) + (2.34.meters + 0.3.meters)) / 2 + + 0.25.meters, + 5.45.meters + ) + .translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + startingPose.translation.translation2d, + null, + 180.degrees.inRotation2ds + ) // Subwoofer + ) + } + ) + .withTimeout(3.235 + 0.5), + WaitCommand(0.3).andThen(superstructure.groundIntakeCommand()) + ), + superstructure.prepSpeakerLowCommand(), + superstructure + .scoreCommand() + .withTimeout(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds + 0.5), + WaitCommand(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + startingPose.translation.translation2d, + null, + 180.degrees.inRotation2ds + ), // Subwoofer + FieldWaypoint( + Translation2d(2.41.meters - 0.4.meters, 4.4.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(2.41.meters, 4.1.meters).translation2d, + 0.degrees.inRotation2ds, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + startingPose.translation.translation2d, + null, + 180.degrees.inRotation2ds + ) + ) + } + ) + .withTimeout(3.235 + 0.5), + WaitCommand(0.5).andThen(superstructure.groundIntakeCommand()) + ), + superstructure.prepSpeakerLowCommand(), + superstructure + .scoreCommand() + .withTimeout(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds + 0.5), + WaitCommand(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + startingPose.translation.translation2d, + null, + startingPose.rotation.inRotation2ds + ), + FieldWaypoint( + Translation2d(2.91.meters - 0.75.meters, 6.9.meters) + .translation2d, + null, + 180.degrees.inRotation2ds, + ), + FieldWaypoint( + Translation2d(2.91.meters - 0.25.meters, 7.1.meters).translation2d, + 0.degrees.inRotation2ds, + 180.degrees.inRotation2ds, + ), + FieldWaypoint( + startingPose.translation.translation2d, + null, + 180.degrees.inRotation2ds + ) + ) + } + ) + .withTimeout(3.235 + 0.5), + WaitCommand(0.5).andThen(superstructure.groundIntakeCommand()) + ), + superstructure.prepSpeakerLowCommand(), + superstructure + .scoreCommand() + .withTimeout(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds + 0.5), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + startingPose.translation.translation2d, null, 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(4.35.meters, 4.85.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(5.92.meters, 3.9.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(8.29.meters, 4.09.meters).translation2d, + null, + 180.degrees.inRotation2ds + ) + ) + } + ), + WaitCommand(1.0).andThen(superstructure.groundIntakeCommand()) + ) + ) + } + + companion object { + val startingPose = Pose2d(Translation2d(1.42.meters + 1.inches, 5.535.meters), 180.degrees) + } +} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPathWithFirstSourceSide.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPathWithFirstSourceSide.kt new file mode 100644 index 00000000..0cb24556 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPathWithFirstSourceSide.kt @@ -0,0 +1,187 @@ +package com.team4099.robot2023.auto.mode + +import com.team4099.lib.trajectory.FieldWaypoint +import com.team4099.robot2023.commands.drivetrain.DrivePathCommand +import com.team4099.robot2023.config.constants.FlywheelConstants +import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.subsystems.superstructure.Superstructure +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup +import edu.wpi.first.wpilibj2.command.WaitCommand +import org.team4099.lib.geometry.Pose2d +import org.team4099.lib.geometry.Translation2d +import org.team4099.lib.units.base.inSeconds +import org.team4099.lib.units.base.inches +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inRotation2ds + +class FourNoteAutoPathWithFirstSourceSide(val drivetrain: Drivetrain, val superstructure: Superstructure) : + SequentialCommandGroup() { + init { + addRequirements(drivetrain, superstructure) + + addCommands( + superstructure.prepSpeakerLowCommand(), + superstructure.scoreCommand().withTimeout(0.5), + WaitCommand(0.5), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + startingPose.translation.translation2d, + null, + 180.degrees.inRotation2ds + ), // Subwoofer + FieldWaypoint( + Translation2d(2.41.meters - 0.4.meters, 4.4.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(2.41.meters, 4.1.meters).translation2d, + 0.degrees.inRotation2ds, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + startingPose.translation.translation2d, + null, + 180.degrees.inRotation2ds + ) + ) + } + ) + .withTimeout(3.235 + 0.5), + WaitCommand(0.5).andThen(superstructure.groundIntakeCommand()) + ), + superstructure.prepSpeakerLowCommand(), + superstructure + .scoreCommand() + .withTimeout(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds + 0.5), + WaitCommand(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds), + + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + startingPose.translation.translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d( + ((1.43.meters) + (2.34.meters + 0.3.meters)) / 2 + + 0.25.meters, + 5.55.meters + ) + .translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(2.34.meters + 0.3.meters + 0.25.meters, 5.535.meters) + .translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d( + ((1.43.meters) + (2.34.meters + 0.3.meters)) / 2 + + 0.25.meters, + 5.45.meters + ) + .translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + startingPose.translation.translation2d, + null, + 180.degrees.inRotation2ds + ) // Subwoofer + ) + } + ) + .withTimeout(3.235 + 0.5), + WaitCommand(0.3).andThen(superstructure.groundIntakeCommand()) + ), + superstructure.prepSpeakerLowCommand(), + superstructure + .scoreCommand() + .withTimeout(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds + 0.5), + WaitCommand(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + startingPose.translation.translation2d, + null, + startingPose.rotation.inRotation2ds + ), + FieldWaypoint( + Translation2d(2.91.meters - 0.75.meters, 6.82.meters) + .translation2d, + null, + 180.degrees.inRotation2ds, + ), + FieldWaypoint( + Translation2d(2.91.meters - 0.25.meters, 7.meters).translation2d, + 0.degrees.inRotation2ds, + 180.degrees.inRotation2ds, + ), + FieldWaypoint( + startingPose.translation.translation2d, + null, + 180.degrees.inRotation2ds + ) + ) + } + ) + .withTimeout(3.235 + 0.5), + WaitCommand(0.5).andThen(superstructure.groundIntakeCommand()) + ), + superstructure.prepSpeakerLowCommand(), + superstructure + .scoreCommand() + .withTimeout(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds + 0.5), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + startingPose.translation.translation2d, null, 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(4.35.meters, 4.85.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(5.92.meters, 3.9.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(8.29.meters, 4.09.meters).translation2d, + null, + 180.degrees.inRotation2ds + ) + ) + } + ), + WaitCommand(1.0).andThen(superstructure.groundIntakeCommand()) + ) + ) + } + + companion object { + val startingPose = Pose2d(Translation2d(1.42.meters + 1.inches, 5.535.meters), 180.degrees) + } +} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt index a77b9930..2fe5292d 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt @@ -56,7 +56,7 @@ object ControlBoard { // val intake = Trigger { driver.rightShoulderButton} val targetAmp = Trigger { driver.aButton } - val prepAmp = Trigger { operator.aButton } + val prepAmp = Trigger { driver.aButton } val prepLow = Trigger { operator.xButton } val prepHighProtected = Trigger { operator.bButton } val prepHigh = Trigger { operator.yButton } diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt index f6427e2e..bad9aaaa 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt @@ -53,8 +53,8 @@ object Constants { } object Tuning { - const val TUNING_MODE = true - const val DEBUGING_MODE = true + const val TUNING_MODE = false + const val DEBUGING_MODE = false const val SIMULATE_DRIFT = false const val DRIFT_CONSTANT = 0.001 diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt index 400586e6..dd6fa822 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt @@ -26,7 +26,6 @@ import org.team4099.lib.units.derived.volts import org.team4099.lib.units.inMetersPerSecond import org.team4099.lib.units.kilo import org.team4099.lib.units.perSecond -import kotlin.math.PI import kotlin.math.sqrt object DrivetrainConstants { @@ -184,4 +183,4 @@ object DrivetrainConstants { val SIM_STEERING_KI = 0.0.volts.perDegreeSeconds val SIM_STEERING_KD = 0.0.volts.perDegreePerSecond } -} \ No newline at end of file +} diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt index f6a63c4f..cb5cc817 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt @@ -35,9 +35,7 @@ object WristConstants { val VOLTAGE_COMPENSATION = 12.0.volts val ABSOLUTE_ENCODER_OFFSET = - ( - -34.5.degrees + 3.95.degrees - ) * ABSOLUTE_ENCODER_TO_MECHANISM_GEAR_RATIO + (-34.5.degrees + 3.95.degrees) * ABSOLUTE_ENCODER_TO_MECHANISM_GEAR_RATIO val WRIST_LENGTH = 18.6.inches val WRIST_INERTIA = 0.7181257183.kilo.grams * 1.0.meters.squared @@ -62,9 +60,9 @@ object WristConstants { val ARBITRARY_FEEDFORWARD = 0.03.volts - val REAL_KP: ProportionalGain = 0.515.volts / 1.0.degrees + val REAL_KP: ProportionalGain = 0.31.volts / 1.0.degrees val REAL_KI: IntegralGain = 0.0.volts / (1.0.degrees * 1.0.seconds) - val REAL_KD: DerivativeGain = 0.03.volts / 1.0.degrees.perSecond + val REAL_KD: DerivativeGain = 0.05.volts / 1.0.degrees.perSecond val FIRST_STAGE_POS_SWITCH_THRESHOLD = 3.0.degrees val FIRST_STAGE_VEL_SWITCH_THRESHOLD = 3.0.degrees.perSecond @@ -107,7 +105,7 @@ object WristConstants { val CLIMB_ANGLE = 0.0.degrees val TRAP_ANGLE = 35.degrees - val INTAKE_ANGLE = (-33.75).degrees + val INTAKE_ANGLE = (-33.25).degrees val IDLE_ANGLE_HAS_GAMEPEICE = -34.degrees val PASSING_SHOT_ANGLE = -34.degrees } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt index 8cd621ce..69d5fa87 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt @@ -80,7 +80,9 @@ class SwerveModuleIOTalon( private val potentiometerOutput: Double get() { - return if (label == Constants.Drivetrain.FRONT_RIGHT_MODULE_NAME || label == Constants.Drivetrain.BACK_RIGHT_MODULE_NAME) { + return if (label == Constants.Drivetrain.FRONT_RIGHT_MODULE_NAME || + label == Constants.Drivetrain.BACK_RIGHT_MODULE_NAME + ) { potentiometer.voltage / RobotController.getVoltage5V() * 2.0 * PI } else { 2 * PI - potentiometer.voltage / RobotController.getVoltage5V() * 2.0 * Math.PI @@ -221,9 +223,9 @@ class SwerveModuleIOTalon( Logger.recordOutput( "$label/drivePosition", driveFalcon.position.value * - (PI) * - DrivetrainConstants.WHEEL_DIAMETER.inMeters * - DrivetrainConstants.DRIVE_SENSOR_GEAR_RATIO + (PI) * + DrivetrainConstants.WHEEL_DIAMETER.inMeters * + DrivetrainConstants.DRIVE_SENSOR_GEAR_RATIO ) Logger.recordOutput("$label/drivePositionUnits", driveSensor.position.inMeters) inputs.drivePosition = driveSensor.position @@ -341,7 +343,9 @@ class SwerveModuleIOTalon( override fun zeroSteering(isInAutonomous: Boolean) { steeringFalcon.setPosition( steeringSensor.positionToRawUnits( - if (label == Constants.Drivetrain.FRONT_RIGHT_MODULE_NAME || label == Constants.Drivetrain.BACK_RIGHT_MODULE_NAME){ + if (label == Constants.Drivetrain.FRONT_RIGHT_MODULE_NAME || + label == Constants.Drivetrain.BACK_RIGHT_MODULE_NAME + ) { (potentiometerOutput.radians - zeroOffset) } else { (2 * PI).radians - (potentiometerOutput.radians - zeroOffset) @@ -439,4 +443,4 @@ class SwerveModuleIOTalon( driveFalcon.setControl(DutyCycleOut(input.inVolts / 12.0)) } } -} \ No newline at end of file +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt index 2f758847..e6fc39f4 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt @@ -454,8 +454,9 @@ class Superstructure( } } SuperstructureStates.GROUND_INTAKE_CLEAN_UP_PUSH_BACK -> { - feeder.currentRequest = Request.FeederRequest.OpenLoopIntake(-1.0.volts) - intake.currentRequest = Request.IntakeRequest.OpenLoop(0.0.volts, 0.0.volts) + feeder.currentRequest = Request.FeederRequest.OpenLoopIntake(-2.volts) + intake.currentRequest = Request.IntakeRequest.OpenLoop(-0.5.volts, 0.0.volts) + if (!feeder.hasNote) { nextState = SuperstructureStates.GROUND_INTAKE_CLEAN_UP_PUSH_FORWARD } @@ -467,8 +468,8 @@ class Superstructure( } } SuperstructureStates.GROUND_INTAKE_CLEAN_UP_PUSH_FORWARD -> { - feeder.currentRequest = Request.FeederRequest.OpenLoopIntake(1.0.volts) - intake.currentRequest = Request.IntakeRequest.OpenLoop(0.0.volts, 0.0.volts) + feeder.currentRequest = Request.FeederRequest.OpenLoopIntake(2.0.volts) + intake.currentRequest = Request.IntakeRequest.OpenLoop(0.25.volts, 0.0.volts) if (feeder.hasNote || RobotBase.isSimulation()) { currentRequest = Request.SuperstructureRequest.Idle() nextState = SuperstructureStates.IDLE diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIOTalon.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIOTalon.kt index b09b6984..19e3dff3 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIOTalon.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIOTalon.kt @@ -151,7 +151,6 @@ object WristIOTalon : WristIO { wristConfiguration.Voltage.PeakForwardVoltage = 3.0 wristConfiguration.Voltage.PeakReverseVoltage = -3.0 - wristConfiguration.CurrentLimits.SupplyCurrentLimit = WristConstants.WRIST_SUPPLY_CURRENT_LIMIT.inAmperes wristConfiguration.CurrentLimits.SupplyCurrentThreshold =