diff --git a/simgui-ds.json b/simgui-ds.json index def7ba7b..684084ed 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -4,6 +4,11 @@ "visible": true } }, + "System Joysticks": { + "window": { + "visible": false + } + }, "keyboardJoysticks": [ { "axisConfig": [ diff --git a/src/main/deploy/pathplanner/paths/New New New New Path.path b/src/main/deploy/pathplanner/paths/New New New New Path.path index bba2a556..e5d9680f 100644 --- a/src/main/deploy/pathplanner/paths/New New New New Path.path +++ b/src/main/deploy/pathplanner/paths/New New New New Path.path @@ -112,12 +112,28 @@ }, { "anchor": { - "x": 8.06421609994235, - "y": 5.87883546728025 + "x": 4.387298188658877, + "y": 4.872072048831598 }, "prevControl": { - "x": 4.787635103213693, - "y": 7.584219112563609 + "x": 1.1107171919302203, + "y": 6.5774556941149545 + }, + "nextControl": { + "x": 7.663879185387534, + "y": 3.1666884035482408 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.862336685610871, + "y": 3.9777719789814583 + }, + "prevControl": { + "x": 7.51532475874072, + "y": 3.0879616157138097 }, "nextControl": null, "isLocked": false, diff --git a/src/main/kotlin/com/team4099/robot2023/Robot.kt b/src/main/kotlin/com/team4099/robot2023/Robot.kt index ea1281cd..2ec17495 100644 --- a/src/main/kotlin/com/team4099/robot2023/Robot.kt +++ b/src/main/kotlin/com/team4099/robot2023/Robot.kt @@ -8,7 +8,7 @@ import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.subsystems.falconspin.MotorChecker import com.team4099.robot2023.util.Alert import com.team4099.robot2023.util.Alert.AlertType -import com.team4099.robot2023.util.DebugLogger +import com.team4099.robot2023.util.CustomLogger import com.team4099.robot2023.util.FMSData import com.team4099.robot2023.util.NTSafePublisher import edu.wpi.first.hal.AllianceStationID @@ -197,7 +197,7 @@ object Robot : LoggedRobot() { "LoggedRobot/RemainingRamMB", Runtime.getRuntime().freeMemory() / 1024 / 1024 ) - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "LoggedRobot/totalMS", (Clock.realTimestamp - startTime).inMilliseconds ) @@ -213,7 +213,6 @@ object Robot : LoggedRobot() { } override fun teleopInit() { - FMSData.allianceColor = DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue) RobotContainer.zeroSensors(isInAutonomous = false) RobotContainer.mapTeleopControls() RobotContainer.getAutonomousCommand().cancel() diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index 6b8fbe4a..45904c6b 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -2,11 +2,8 @@ package com.team4099.robot2023 import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.auto.AutonomousSelector -import com.team4099.robot2023.commands.drivetrain.LockDriveCommand import com.team4099.robot2023.commands.drivetrain.ResetGyroYawCommand import com.team4099.robot2023.commands.drivetrain.TargetAngleCommand -import com.team4099.robot2023.commands.drivetrain.TargetNoteCommand -import com.team4099.robot2023.commands.drivetrain.TargetSpeakerCommand import com.team4099.robot2023.commands.drivetrain.TeleopDriveCommand import com.team4099.robot2023.config.ControlBoard import com.team4099.robot2023.config.constants.Constants @@ -38,7 +35,7 @@ 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 -import com.team4099.robot2023.util.driver.Ryan +import com.team4099.robot2023.util.driver.Jessika import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj.RobotBase import edu.wpi.first.wpilibj2.command.Command @@ -113,16 +110,13 @@ object RobotContainer { drivetrain.defaultCommand = TeleopDriveCommand( - driver = Ryan(), + driver = Jessika(), { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) }, { ControlBoard.slowMode }, drivetrain ) - - superstructure.defaultCommand = superstructure.forceIdleIfAutoAimCommand() - /* module steeing tuning @@ -170,11 +164,15 @@ object RobotContainer { limelight.limelightState = LimelightVision.LimelightStates.TARGETING_GAME_PIECE ControlBoard.resetGyro.whileTrue(ResetGyroYawCommand(drivetrain)) + + ControlBoard.intake.whileTrue(superstructure.groundIntakeCommand()) + + /* ControlBoard.intake.whileTrue( ParallelCommandGroup( superstructure.groundIntakeCommand(), TargetNoteCommand( - driver = Ryan(), + driver = Jessika(), { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) }, @@ -186,11 +184,13 @@ object RobotContainer { ) ) + */ + ControlBoard.prepAmp.whileTrue(superstructure.prepAmpCommand()) ControlBoard.passingShotAlignment.whileTrue( TargetAngleCommand( - driver = Ryan(), + driver = Jessika(), { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) }, @@ -214,7 +214,7 @@ object RobotContainer { ParallelCommandGroup( superstructure.prepSpeakerMidCommand(), TargetAngleCommand( - driver = Ryan(), + driver = Jessika(), { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) }, @@ -240,9 +240,10 @@ object RobotContainer { ControlBoard.passingShot.whileTrue(superstructure.passingShotCommand()) ControlBoard.underStagePassingShot.whileTrue(superstructure.underStageCommand()) + /* ControlBoard.targetAmp.whileTrue( TargetAngleCommand( - driver = Ryan(), + driver = Jessika(), { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) }, @@ -252,6 +253,8 @@ object RobotContainer { ) ) + */ + ControlBoard.climbAlignFar.whileTrue( runOnce({ setClimbAngle = @@ -286,7 +289,7 @@ object RobotContainer { ) ControlBoard.climbAutoAlign.whileTrue( TargetAngleCommand( - driver = Ryan(), + driver = Jessika(), { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) }, @@ -297,7 +300,7 @@ object RobotContainer { ) // ControlBoard.climbAlignLeft.whileTrue( // TargetAngleCommand( - // driver = Ryan(), + // driver = Jessika(), // { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, // { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, // { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) }, @@ -313,7 +316,7 @@ object RobotContainer { // // ControlBoard.climbAlignRight.whileTrue( // TargetAngleCommand( - // driver = Ryan(), + // driver = Jessika(), // { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, // { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, // { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) }, @@ -327,10 +330,12 @@ object RobotContainer { // ) // ) + /* + ControlBoard.targetSpeaker.whileTrue( ParallelCommandGroup( TargetSpeakerCommand( - driver = Ryan(), + driver = Jessika(), { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) }, @@ -343,7 +348,9 @@ object RobotContainer { ) ) - ControlBoard.lockWheels.whileTrue(LockDriveCommand(drivetrain)) + */ + + // ControlBoard.lockWheels.whileTrue(LockDriveCommand(drivetrain)) // ControlBoard.characterizeSubsystem.whileTrue( // WheelRadiusCharacterizationCommand( diff --git a/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt b/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt index 5c11ff5c..f696341c 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt @@ -12,7 +12,9 @@ import com.team4099.robot2023.auto.mode.PreloadAndLeaveFromSourceSubwooferAutoPa import com.team4099.robot2023.auto.mode.SixNoteAutoPath import com.team4099.robot2023.auto.mode.TestAutoPath import com.team4099.robot2023.auto.mode.ThreeNoteAndPickupCenterlineSourceAutoPath +import com.team4099.robot2023.auto.mode.ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath import com.team4099.robot2023.auto.mode.ThreeNoteCenterlineFromAmpAutoPath +import com.team4099.robot2023.auto.mode.ThreeNoteCenterlineWithoutFirstNoteAmpAutoPath import com.team4099.robot2023.auto.mode.TwoNoteCenterlineFromAmpAutoPath import com.team4099.robot2023.auto.mode.TwoNoteCenterlineFromSourceAutoPath import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain @@ -81,10 +83,22 @@ object AutonomousSelector { "Three Note + Pickup Centerline Auto from Amp Side of Subwoofer", AutonomousMode.THREE_NOTE_CENTERLINE_FROM_AMP ) + + autonomousModeChooser.addOption( + "Three Note + Pickup Centerline WITHOUT First Note Auto from Amp Side of Subwoofer", + AutonomousMode.THREE_NOTE_CENTERLINE_WITHOUT_FIRST_NOTE_FROM_AMP + ) + autonomousModeChooser.addOption( "Three Note + Pickup Centerline Auto from Source Side of Subwoofer", AutonomousMode.THREE_NOTE_AND_PICKUP_CENTERLINE_FROM_SOURCE ) + + autonomousModeChooser.addOption( + "Three Note + Pickup Centerline WITHOUT First Note Auto from Source Side of Subwoofer", + AutonomousMode.THREE_NOTE_AND_PICKUP_CENTERLINE_WITHOUT_FIRST_NOTE_FROM_SOURCE + ) + autonomousModeChooser.addOption( "Preload + Leave from Amp Side of Subwoofer", AutonomousMode.PRELOAD_AND_LEAVE_LEFT_SUBWOOFER @@ -128,7 +142,7 @@ object AutonomousSelector { } val waitTime: Time - get() = 0.0.seconds + get() = waitBeforeCommandSlider.getDouble(0.0).seconds val secondaryWaitTime: Time get() = secondaryWaitInAuto.getDouble(0.0).seconds @@ -222,6 +236,17 @@ object AutonomousSelector { drivetrain.resetFieldFrameEstimator(flippedPose) }) .andThen(ThreeNoteCenterlineFromAmpAutoPath(drivetrain, superstructure)) + AutonomousMode.THREE_NOTE_CENTERLINE_WITHOUT_FIRST_NOTE_FROM_AMP -> + return WaitCommand(waitTime.inSeconds) + .andThen({ + val flippedPose = + AllianceFlipUtil.apply( + ThreeNoteCenterlineWithoutFirstNoteAmpAutoPath.startingPose + ) + drivetrain.tempZeroGyroYaw(flippedPose.rotation) + drivetrain.resetFieldFrameEstimator(flippedPose) + }) + .andThen(ThreeNoteCenterlineWithoutFirstNoteAmpAutoPath(drivetrain, superstructure)) AutonomousMode.THREE_NOTE_AND_PICKUP_CENTERLINE_FROM_SOURCE -> return WaitCommand(waitTime.inSeconds) .andThen({ @@ -231,6 +256,21 @@ object AutonomousSelector { drivetrain.resetFieldFrameEstimator(flippedPose) }) .andThen(ThreeNoteAndPickupCenterlineSourceAutoPath(drivetrain, superstructure)) + AutonomousMode.THREE_NOTE_AND_PICKUP_CENTERLINE_WITHOUT_FIRST_NOTE_FROM_SOURCE -> + return WaitCommand(waitTime.inSeconds) + .andThen({ + val flippedPose = + AllianceFlipUtil.apply( + ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath.startingPose + ) + drivetrain.tempZeroGyroYaw(flippedPose.rotation) + drivetrain.resetFieldFrameEstimator(flippedPose) + }) + .andThen( + ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath( + drivetrain, superstructure + ) + ) AutonomousMode.PRELOAD_AND_LEAVE_LEFT_SUBWOOFER -> return WaitCommand(waitTime.inSeconds) .andThen({ @@ -288,7 +328,9 @@ object AutonomousSelector { TWO_NOTE_CENTERLINE_FROM_SOURCE, TWO_NOTE_CENTERLINE_FROM_AMP, THREE_NOTE_CENTERLINE_FROM_AMP, + THREE_NOTE_CENTERLINE_WITHOUT_FIRST_NOTE_FROM_AMP, THREE_NOTE_AND_PICKUP_CENTERLINE_FROM_SOURCE, + THREE_NOTE_AND_PICKUP_CENTERLINE_WITHOUT_FIRST_NOTE_FROM_SOURCE, PRELOAD_AND_LEAVE_LEFT_SUBWOOFER, PRELOAD_AND_LEAVE_RIGHT_SUBWOOFER, PRELOAD_AND_LEAVE_CENTER_SUBWOOFER, 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 f52427eb..a0f21fd5 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPath.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPath.kt @@ -11,6 +11,7 @@ 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 @@ -23,7 +24,7 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru addCommands( superstructure.prepSpeakerLowCommand(), superstructure.scoreCommand().withTimeout(0.5), - WaitCommand(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds), + WaitCommand(0.5), ParallelCommandGroup( DrivePathCommand.createPathInFieldFrame( drivetrain, @@ -54,11 +55,9 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru } ) .withTimeout(3.235 + 0.5), - WaitCommand(0.5) - .andThen(superstructure.groundIntakeCommand()) - .andThen(WaitCommand(1.75)) - .andThen(superstructure.prepSpeakerLowCommand()) + WaitCommand(0.5).andThen(superstructure.groundIntakeCommand()) ), + superstructure.prepSpeakerLowCommand(), superstructure .scoreCommand() .withTimeout(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds + 0.5), @@ -92,11 +91,9 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru } ) .withTimeout(3.235 + 0.5), - WaitCommand(0.5) - .andThen(superstructure.groundIntakeCommand()) - .andThen(WaitCommand(1.75)) - .andThen(superstructure.prepSpeakerLowCommand()) + WaitCommand(0.5).andThen(superstructure.groundIntakeCommand()) ), + superstructure.prepSpeakerLowCommand(), superstructure .scoreCommand() .withTimeout(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds + 0.5), @@ -146,15 +143,12 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru } ) .withTimeout(3.235 + 0.5), - WaitCommand(0.3) - .andThen(superstructure.groundIntakeCommand()) - .andThen(WaitCommand(1.75)) - .andThen(superstructure.prepSpeakerLowCommand()) + 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, @@ -164,12 +158,12 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru startingPose.translation.translation2d, null, 180.degrees.inRotation2ds ), FieldWaypoint( - Translation2d(4.45.meters, 4.89.meters).translation2d, + Translation2d(4.35.meters, 4.85.meters).translation2d, null, 180.degrees.inRotation2ds ), FieldWaypoint( - Translation2d(5.92.meters, 4.0.meters).translation2d, + Translation2d(5.92.meters, 3.9.meters).translation2d, null, 180.degrees.inRotation2ds ), @@ -187,6 +181,6 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru } companion object { - val startingPose = Pose2d(Translation2d(1.42.meters, 5.535.meters), 180.degrees) + val startingPose = Pose2d(Translation2d(1.42.meters - 3.inches, 5.535.meters), 180.degrees) } } diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteAndPickupCenterlineSourceAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteAndPickupCenterlineSourceAutoPath.kt index b3c255b9..35854e93 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteAndPickupCenterlineSourceAutoPath.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteAndPickupCenterlineSourceAutoPath.kt @@ -4,7 +4,6 @@ import com.team4099.lib.trajectory.FieldWaypoint import com.team4099.robot2023.commands.drivetrain.DrivePathCommand import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import com.team4099.robot2023.subsystems.superstructure.Superstructure -import edu.wpi.first.wpilibj2.command.ConditionalCommand import edu.wpi.first.wpilibj2.command.ParallelCommandGroup import edu.wpi.first.wpilibj2.command.SequentialCommandGroup import edu.wpi.first.wpilibj2.command.WaitCommand @@ -44,217 +43,100 @@ class ThreeNoteAndPickupCenterlineSourceAutoPath( Translation2d(8.32.meters, 0.78.meters).translation2d, null, 180.degrees.inRotation2ds - ), - FieldWaypoint( - Translation2d(6.89.meters, 1.73.meters).translation2d, - null, - 180.degrees.inRotation2ds ) ) } ), WaitCommand(1.5).andThen(superstructure.groundIntakeCommand()) ), - ConditionalCommand( - ParallelCommandGroup( - DrivePathCommand.createPathInFieldFrame( - drivetrain, - { - listOf( - FieldWaypoint( - Translation2d(6.89.meters, 1.73.meters).translation2d, - null, - 180.degrees.inRotation2ds - ), - FieldWaypoint( - Translation2d(4.33.meters, 1.67.meters).translation2d, - null, - (180 - 43.37583640633171).degrees.inRotation2ds - ), - ) - } - ), - superstructure.prepManualSpeakerCommand(-2.2.degrees, 4000.rotations.perMinute) - ) - .andThen(superstructure.scoreCommand()) - .andThen( - ParallelCommandGroup( - DrivePathCommand.createPathInFieldFrame( - drivetrain, - { - listOf( - FieldWaypoint( - Translation2d(4.33.meters, 1.67.meters).translation2d, - null, - (180 - 43.3758).degrees.inRotation2ds - ), - FieldWaypoint( - Translation2d( - (4.33.meters + 8.32.meters) / 2, - (2.44 + 0.78).meters / 2 - 0.1.meters - ) - .translation2d, - null, - ((180 - 43.3758 + 220) / 2 + 10).degrees.inRotation2ds, - ), - FieldWaypoint( - Translation2d(8.32.meters, 2.44.meters).translation2d, - null, - 220.degrees.inRotation2ds - ), - FieldWaypoint( - Translation2d(7.0.meters, 2.40.meters).translation2d, - null, - 220.degrees.inRotation2ds - ) - ) - } + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(8.32.meters, 0.78.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(4.33.meters, 1.67.meters).translation2d, + null, + (180 - 43.37583640633171).degrees.inRotation2ds + ), + ) + } + ), + superstructure.prepManualSpeakerCommand(-4.5.degrees, 4000.rotations.perMinute), + superstructure.scoreCommand(), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(4.33.meters, 1.67.meters).translation2d, + null, + (180 - 43.3758).degrees.inRotation2ds ), - WaitCommand(1.0).andThen(superstructure.groundIntakeCommand()), - ) - ), - ParallelCommandGroup( - DrivePathCommand.createPathInFieldFrame( - drivetrain, - { - listOf( - FieldWaypoint( - Translation2d(6.89.meters, 1.73.meters).translation2d, - null, - 180.degrees.inRotation2ds - ), - FieldWaypoint( - Translation2d(8.32.meters, 2.44.meters).translation2d, - null, - 220.degrees.inRotation2ds - ), - FieldWaypoint( - Translation2d(7.0.meters, 2.40.meters).translation2d, - null, - 220.degrees.inRotation2ds + FieldWaypoint( + Translation2d( + (4.33.meters + 8.32.meters) / 2, + (2.44 + 0.78).meters / 2 - 0.1.meters ) + .translation2d, + null, + ((180 - 43.3758 + 210) / 2 + 10).degrees.inRotation2ds, + ), + FieldWaypoint( + Translation2d(8.32.meters, 2.44.meters).translation2d, + null, + 210.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d( + (4.33.meters + 8.32.meters) / 2, + (2.44 + 0.78).meters / 2 + 0.1.meters + ) + .translation2d, + null, + ((180 - 43.3758 + 210) / 2 + 10).degrees.inRotation2ds, + ), + FieldWaypoint( + Translation2d(4.33.meters, 1.67.meters).translation2d, + null, + (180 - 43.3758).degrees.inRotation2ds ) - } - ), - superstructure.groundIntakeCommand() + ) + } ), - { superstructure.holdingNote } + WaitCommand(1.0).andThen(superstructure.groundIntakeCommand()) ), - ConditionalCommand( - ParallelCommandGroup( - DrivePathCommand.createPathInFieldFrame( - drivetrain, - { - listOf( - FieldWaypoint( - Translation2d(7.0.meters, 2.40.meters).translation2d, - null, - 220.degrees.inRotation2ds - ), - FieldWaypoint( - Translation2d( - (4.33.meters + 8.32.meters) / 2, - (2.44 + 0.78).meters / 2 + 0.1.meters - ) - .translation2d, - null, - ((180 - 43.3758 + 220) / 2 + 10).degrees.inRotation2ds, - ), - FieldWaypoint( - Translation2d(4.33.meters, 1.67.meters).translation2d, - null, - (180 - 43.3758).degrees.inRotation2ds - ) - ) - } - ), - superstructure.prepManualSpeakerCommand(-2.2.degrees, 4000.rotations.perMinute) - ) - .andThen(superstructure.scoreCommand()) - .andThen( - ParallelCommandGroup( - DrivePathCommand.createPathInFieldFrame( - drivetrain, - { - listOf( - FieldWaypoint( - Translation2d(4.33.meters, 1.67.meters).translation2d, - null, - (180 - 43.3758).degrees.inRotation2ds - ), - FieldWaypoint( - Translation2d(4.84.meters, 4.09.meters).translation2d, - null, - 180.degrees.inRotation2ds - ), - FieldWaypoint( - Translation2d(8.2.meters, 4.06.meters).translation2d, - null, - 180.degrees.inRotation2ds - ), - FieldWaypoint( - Translation2d(7.5.meters, 2.0.meters).translation2d, - null, - 180.degrees.inRotation2ds - ), - FieldWaypoint( - Translation2d(5.5.meters, 1.85.meters).translation2d, - null, - (180 - 35.02902884839783).degrees.inRotation2ds - ) - ) - } + superstructure.prepManualSpeakerCommand(-4.5.degrees, 4000.rotations.perMinute), + superstructure.scoreCommand(), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(4.33.meters, 1.67.meters).translation2d, + null, + (180 - 43.3758).degrees.inRotation2ds ), - WaitCommand(1.5) - .andThen(superstructure.groundIntakeCommand()) - .andThen(WaitCommand(2.0)) - .andThen( - superstructure.prepManualSpeakerCommand( - -2.0.degrees, 4400.rotations.perMinute - ) - ) - ) - .andThen(superstructure.scoreCommand()) - ), - ParallelCommandGroup( - DrivePathCommand.createPathInFieldFrame( - drivetrain, - { - listOf( - FieldWaypoint( - Translation2d(7.0.meters, 2.40.meters).translation2d, - null, - 220.degrees.inRotation2ds - ), - FieldWaypoint( - Translation2d(8.2.meters, 4.06.meters).translation2d, - null, - 220.degrees.inRotation2ds - ), - FieldWaypoint( - Translation2d(7.5.meters, 2.0.meters).translation2d, - null, - 180.degrees.inRotation2ds - ), - FieldWaypoint( - Translation2d(5.5.meters, 1.85.meters).translation2d, - null, - (180 - 35.02902884839783).degrees.inRotation2ds - ) - ) - } - ), - superstructure - .groundIntakeCommand() - .andThen(WaitCommand(2.0)) - .andThen( - superstructure.prepManualSpeakerCommand( - -2.0.degrees, 4400.rotations.perMinute + FieldWaypoint( + Translation2d(4.84.meters, 4.09.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(8.32.meters, 4.12.meters).translation2d, + null, + 180.degrees.inRotation2ds ) ) - ) - .andThen(superstructure.scoreCommand()), - { superstructure.holdingNote } + } + ), + WaitCommand(1.5).andThen(superstructure.groundIntakeCommand()) ) ) } diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath.kt new file mode 100644 index 00000000..79d5f281 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath.kt @@ -0,0 +1,129 @@ +package com.team4099.robot2023.auto.mode + +import com.team4099.lib.trajectory.FieldWaypoint +import com.team4099.robot2023.commands.drivetrain.DrivePathCommand +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.meters +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inRotation2ds +import org.team4099.lib.units.derived.rotations +import org.team4099.lib.units.perMinute + +class ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath( + val drivetrain: Drivetrain, + val superstructure: Superstructure +) : SequentialCommandGroup() { + init { + addRequirements(drivetrain) + addCommands( + superstructure.prepSpeakerLowCommand(), + superstructure.scoreCommand().withTimeout(0.75), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + startingPose.translation.translation2d, + null, + startingPose.rotation.inRotation2ds + ), + FieldWaypoint( + Translation2d(4.33.meters, 1.67.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(8.32.meters, 2.44.meters).translation2d, + null, + 220.degrees.inRotation2ds + ) + ) + } + ), + WaitCommand(1.5).andThen(superstructure.groundIntakeCommand()) + ), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(8.32.meters, 2.44.meters).translation2d, + null, + 220.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(6.89.meters, 1.73.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(4.33.meters, 1.67.meters).translation2d, + null, + (180 - 43.37583640633171).degrees.inRotation2ds + ), + ) + } + ), + superstructure.prepManualSpeakerCommand(-4.5.degrees, 4000.rotations.perMinute) + ) + .andThen(superstructure.scoreCommand()) + .andThen( + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(4.33.meters, 1.67.meters).translation2d, + null, + (180 - 43.3758).degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(4.84.meters, 4.09.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(8.2.meters, 4.06.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(7.5.meters, 2.0.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(4.33.meters, 1.67.meters).translation2d, + null, + (180 - 43.37583640633171).degrees.inRotation2ds + ) + ) + } + ), + WaitCommand(1.5) + .andThen(superstructure.groundIntakeCommand()) + .andThen(WaitCommand(2.0)) + .andThen( + superstructure.prepManualSpeakerCommand( + -4.5.degrees, 4000.rotations.perMinute + ) + ) + ) + .andThen(superstructure.scoreCommand()) + ) + ) + } + + companion object { + val startingPose = Pose2d(0.63.meters, 4.44.meters, 120.degrees) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteCenterlineFromAmpAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteCenterlineFromAmpAutoPath.kt index cc1b75c0..e1677455 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteCenterlineFromAmpAutoPath.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteCenterlineFromAmpAutoPath.kt @@ -4,7 +4,6 @@ import com.team4099.lib.trajectory.FieldWaypoint import com.team4099.robot2023.commands.drivetrain.DrivePathCommand import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import com.team4099.robot2023.subsystems.superstructure.Superstructure -import edu.wpi.first.wpilibj2.command.ConditionalCommand import edu.wpi.first.wpilibj2.command.ParallelCommandGroup import edu.wpi.first.wpilibj2.command.SequentialCommandGroup import edu.wpi.first.wpilibj2.command.WaitCommand @@ -19,9 +18,8 @@ import org.team4099.lib.units.perMinute class ThreeNoteCenterlineFromAmpAutoPath( val drivetrain: Drivetrain, - val superstructure: Superstructure, + val superstructure: Superstructure ) : SequentialCommandGroup() { - init { addRequirements(drivetrain) addCommands( @@ -51,222 +49,152 @@ class ThreeNoteCenterlineFromAmpAutoPath( Translation2d(8.27.meters + 2.feet, 7.45.meters).translation2d, null, 180.degrees.inRotation2ds - ), - FieldWaypoint( - Translation2d(7.0.meters, 6.65.meters).translation2d, - null, - 180.degrees.inRotation2ds ) ) } ), WaitCommand(2.25).andThen(superstructure.groundIntakeCommand()) ), - ConditionalCommand( - ParallelCommandGroup( - DrivePathCommand.createPathInFieldFrame( - drivetrain, - { - listOf( - FieldWaypoint( - Translation2d(7.0.meters, 6.65.meters).translation2d, - null, - 180.degrees.inRotation2ds - ), - FieldWaypoint( - Translation2d(3.9.meters, 6.45.meters).translation2d, - null, - (180 + 13.856).degrees.inRotation2ds - ), - ) - } - ), - WaitCommand(1.0) - .andThen( - superstructure - .prepManualSpeakerCommand(-7.degrees, 4000.rotations.perMinute) - .withTimeout(1.0) + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(8.27.meters + 2.feet, 7.45.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(3.9.meters, 6.45.meters).translation2d, + null, + (180 + 13.856).degrees.inRotation2ds + ), ) - ) - .andThen(superstructure.scoreCommand().withTimeout(0.5)) + } + ), + WaitCommand(1.0) .andThen( - ParallelCommandGroup( - DrivePathCommand.createPathInFieldFrame( - drivetrain, - { - listOf( - FieldWaypoint( - Translation2d(3.9.meters, 6.45.meters).translation2d, - null, - (180 + 13.856).degrees.inRotation2ds - ), - FieldWaypoint( - Translation2d( - (3.9 + 8.27).meters / 2, (6.45 + 7.45).meters / 2 - ) - .translation2d, - null, - ((180 + 13.856 + 165).degrees / 2).inRotation2ds - ), - FieldWaypoint( - Translation2d(8.27.meters, 5.78.meters).translation2d, - null, - 160.degrees.inRotation2ds - ), - FieldWaypoint( - Translation2d(7.0.meters, 5.7.meters).translation2d, - null, - 160.degrees.inRotation2ds - ) - ) - } + superstructure + .prepManualSpeakerCommand(-7.degrees, 4000.rotations.perMinute) + .withTimeout(1.0) + ) + ), + superstructure.scoreCommand().withTimeout(0.5), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(3.9.meters, 6.45.meters).translation2d, + null, + (180 + 13.856).degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d((3.9 + 8.27).meters / 2, (6.45 + 7.45).meters / 2) + .translation2d, + null, + ((180 + 13.856 + 165).degrees / 2).inRotation2ds + ), + FieldWaypoint( + Translation2d(8.5.meters, 5.6.meters).translation2d, + null, + 150.degrees.inRotation2ds ), - WaitCommand(1.0).andThen(superstructure.groundIntakeCommand()) ) - ), - ParallelCommandGroup( - DrivePathCommand.createPathInFieldFrame( - drivetrain, - { - listOf( - FieldWaypoint( - Translation2d(7.0.meters, 6.65.meters).translation2d, - null, - 180.degrees.inRotation2ds - ), - FieldWaypoint( - Translation2d(8.27.meters, 5.78.meters).translation2d, - null, - 160.degrees.inRotation2ds - ), - FieldWaypoint( - Translation2d(7.0.meters, 5.7.meters).translation2d, - null, - 160.degrees.inRotation2ds - ) - ) - } - ), - superstructure.groundIntakeCommand() + } ), - { superstructure.holdingNote } + WaitCommand(1.0).andThen(superstructure.groundIntakeCommand()) ), - ConditionalCommand( - ParallelCommandGroup( - DrivePathCommand.createPathInFieldFrame( - drivetrain, - { - listOf( - FieldWaypoint( - Translation2d(7.0.meters, 5.7.meters).translation2d, - null, - 160.degrees.inRotation2ds - ), - FieldWaypoint( - Translation2d((3.9 + 8.27).meters / 2, (5.7 + 7.45).meters / 2) - .translation2d, - null, - ((180 + 13.856 + 160).degrees / 2).inRotation2ds - ), - FieldWaypoint( - Translation2d(3.9.meters, 6.45.meters).translation2d, - null, - (180 + 13.856).degrees.inRotation2ds - ), - ) - } - ), - superstructure.prepManualSpeakerCommand(-7.degrees, 4000.rotations.perMinute) - ) - .andThen(superstructure.scoreCommand().withTimeout(0.5)) + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(8.5.meters, 5.6.meters).translation2d, + null, + 150.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d((3.9 + 8.27).meters / 2, (6.45 + 7.45).meters / 2) + .translation2d, + null, + ((180 + 13.856 + 160).degrees / 2).inRotation2ds + ), + FieldWaypoint( + Translation2d(3.9.meters, 6.45.meters).translation2d, + null, + (180 + 13.856).degrees.inRotation2ds + ), + ) + } + ), + WaitCommand(1.0) .andThen( - ParallelCommandGroup( - DrivePathCommand.createPathInFieldFrame( - drivetrain, - { - listOf( - FieldWaypoint( - Translation2d(3.9.meters, 6.45.meters).translation2d, - null, - (180 + 13.856).degrees.inRotation2ds - ), - FieldWaypoint( - Translation2d( - (3.9 + 8.27).meters / 2, (5.7 + 7.45).meters / 2 - ) - .translation2d, - null, - ((180 + 13.856 + 165).degrees / 2).inRotation2ds - ), - FieldWaypoint( - Translation2d(8.27.meters, 4.11.meters).translation2d, - null, - 140.degrees.inRotation2ds - ), - FieldWaypoint( - Translation2d(7.5.meters, 2.0.meters).translation2d, - null, - 140.degrees.inRotation2ds - ), - FieldWaypoint( - Translation2d(5.5.meters, 1.85.meters).translation2d, - null, - (180 - 35.02902884839783).degrees.inRotation2ds - ) - ) - } + superstructure.prepManualSpeakerCommand(-7.degrees, 4000.rotations.perMinute) + ) + ), + superstructure.scoreCommand().withTimeout(0.5), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(3.9.meters, 6.45.meters).translation2d, + null, + (180 + 13.856).degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d((3.9 + 8.27).meters / 2, (6.45 + 7.45).meters / 2) + .translation2d, + null, + ((180 + 13.856 + 165).degrees / 2).inRotation2ds + ), + FieldWaypoint( + Translation2d(8.27.meters, 4.11.meters).translation2d, + null, + 140.degrees.inRotation2ds ), - WaitCommand(1.0) - .andThen(superstructure.groundIntakeCommand()) - .andThen(WaitCommand(2.0)) - .andThen( - superstructure.prepManualSpeakerCommand( - -2.0.degrees, 4400.rotations.perMinute - ) - ) ) - .andThen(superstructure.scoreCommand()) - ), - ParallelCommandGroup( - DrivePathCommand.createPathInFieldFrame( - drivetrain, - { - listOf( - FieldWaypoint( - Translation2d(7.0.meters, 5.7.meters).translation2d, - null, - 160.degrees.inRotation2ds - ), - FieldWaypoint( - Translation2d(8.27.meters, 4.11.meters).translation2d, - null, - 140.degrees.inRotation2ds - ), - FieldWaypoint( - Translation2d(7.5.meters, 2.0.meters).translation2d, - null, - 140.degrees.inRotation2ds - ), - FieldWaypoint( - Translation2d(5.5.meters, 1.85.meters).translation2d, - null, - (180 - 35.02902884839783).degrees.inRotation2ds - ) - ) - } - ), - superstructure - .groundIntakeCommand() - .andThen(WaitCommand(2.0)) - .andThen( - superstructure.prepManualSpeakerCommand( - -2.0.degrees, 4400.rotations.perMinute - ) + } + ), + WaitCommand(1.0).andThen(superstructure.groundIntakeCommand()) + ), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(8.27.meters, 4.11.meters).translation2d, + null, + 140.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d((3.9 + 8.27).meters / 2, (6.45 + 7.45).meters / 2) + .translation2d, + null, + ((180 + 13.856 + 160).degrees / 2).inRotation2ds + ), + FieldWaypoint( + Translation2d(3.9.meters, 6.45.meters).translation2d, + null, + (180 + 13.856).degrees.inRotation2ds + ), ) - ) - .andThen(superstructure.scoreCommand()), - { superstructure.holdingNote } - ) + } + ), + WaitCommand(1.0) + .andThen( + superstructure.prepManualSpeakerCommand( + -5.5.degrees, 4000.rotations.perMinute + ) + ) + ), + superstructure.scoreCommand() ) } diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteCenterlineWithoutFirstNoteAmpAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteCenterlineWithoutFirstNoteAmpAutoPath.kt new file mode 100644 index 00000000..e0008435 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteCenterlineWithoutFirstNoteAmpAutoPath.kt @@ -0,0 +1,148 @@ +package com.team4099.robot2023.auto.mode + +import com.team4099.lib.trajectory.FieldWaypoint +import com.team4099.robot2023.commands.drivetrain.DrivePathCommand +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.meters +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inRotation2ds +import org.team4099.lib.units.derived.rotations +import org.team4099.lib.units.perMinute + +class ThreeNoteCenterlineWithoutFirstNoteAmpAutoPath( + val drivetrain: Drivetrain, + val superstructure: Superstructure, +) : SequentialCommandGroup() { + + init { + addRequirements(drivetrain) + addCommands( + superstructure.prepSpeakerLowCommand(), + superstructure.scoreCommand().withTimeout(0.75), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + startingPose.translation.translation2d, + null, + startingPose.rotation.inRotation2ds + ), + FieldWaypoint( + Translation2d(1.90.meters, 6.76.meters).translation2d, + null, + 210.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(2.87.meters, 6.27.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(5.85.meters, 6.44.meters).translation2d, + null, + 170.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(8.27.meters, 5.78.meters).translation2d, + null, + 160.degrees.inRotation2ds + ) + ) + } + ), + WaitCommand(2.25).andThen(superstructure.groundIntakeCommand()) + ), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(8.27.meters, 5.78.meters).translation2d, + null, + 160.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d((3.9 + 8.27).meters / 2, (5.7 + 7.45).meters / 2) + .translation2d, + null, + ((180 + 13.856 + 160).degrees / 2).inRotation2ds + ), + FieldWaypoint( + Translation2d(3.9.meters, 6.45.meters).translation2d, + null, + (180 + 13.856).degrees.inRotation2ds + ) + ) + } + ), + superstructure.prepManualSpeakerCommand(-7.degrees, 4000.rotations.perMinute) + ) + .andThen(superstructure.scoreCommand().withTimeout(0.5)) + .andThen( + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(3.9.meters, 6.45.meters).translation2d, + null, + (180 + 13.856).degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d( + (3.9 + 8.27).meters / 2, (5.25 + 7.45).meters / 2 + ) + .translation2d, + null, + ((180 + 13.856 + 165).degrees / 2).inRotation2ds + ), + FieldWaypoint( + Translation2d(8.27.meters, 4.11.meters).translation2d, + null, + 140.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d( + (3.9 + 8.27).meters / 2, + (5.25 + 7.45).meters / 2 + 0.2.meters + ) + .translation2d, + null, + ((180 + 13.856 + 160).degrees / 2).inRotation2ds + ), + FieldWaypoint( + Translation2d(3.9.meters, 6.45.meters).translation2d, + null, + (180 + 13.856).degrees.inRotation2ds + ) + ) + } + ), + WaitCommand(1.0) + .andThen(superstructure.groundIntakeCommand()) + .andThen(WaitCommand(2.0)) + .andThen( + superstructure.prepManualSpeakerCommand( + -7.degrees, 4000.rotations.perMinute + ) + ) + ) + .andThen(superstructure.scoreCommand()) + ) + ) + } + + companion object { + val startingPose = Pose2d(0.75.meters, 6.70.meters, 240.degrees) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/commands/DriveModuleSteeringCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/DriveModuleSteeringCommand.kt index afefc58c..674d20a5 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/DriveModuleSteeringCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/DriveModuleSteeringCommand.kt @@ -1,7 +1,7 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import com.team4099.robot2023.util.DebugLogger +import com.team4099.robot2023.util.CustomLogger import edu.wpi.first.wpilibj2.command.Command import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest @@ -20,6 +20,6 @@ class DriveModuleSteeringCommand(val drivetrain: Drivetrain) : Command() { } override fun execute() { - DebugLogger.recordDebugOutput("ActiveCommands/ZeroSensorsCommand", true) + CustomLogger.recordDebugOutput("ActiveCommands/ZeroSensorsCommand", true) } } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/SysIdCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/SysIdCommand.kt index b73b6c7b..a75c01e7 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/SysIdCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/SysIdCommand.kt @@ -1,6 +1,6 @@ package com.team4099.robot2023.commands -import com.team4099.robot2023.util.DebugLogger +import com.team4099.robot2023.util.CustomLogger import edu.wpi.first.wpilibj.Timer import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard import edu.wpi.first.wpilibj2.command.Command @@ -108,7 +108,7 @@ class SysIdCommand : Command { data += (subsystemData.velRadPerSec / (2 * Math.PI)).toString() + "," } - DebugLogger.recordDebugOutput("ActiveCommands/SysIdCommand", true) + CustomLogger.recordDebugOutput("ActiveCommands/SysIdCommand", true) } // Called once the command ends or is interrupted. diff --git a/src/main/kotlin/com/team4099/robot2023/commands/characterization/WheelRadiusCharacterizationCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/characterization/WheelRadiusCharacterizationCommand.kt index 67f089a5..9fe7de30 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/characterization/WheelRadiusCharacterizationCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/characterization/WheelRadiusCharacterizationCommand.kt @@ -9,7 +9,7 @@ import com.team4099.lib.logging.LoggedTunableNumber import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import com.team4099.robot2023.subsystems.superstructure.Request -import com.team4099.robot2023.util.DebugLogger +import com.team4099.robot2023.util.CustomLogger import edu.wpi.first.math.MathUtil import edu.wpi.first.math.filter.SlewRateLimiter import edu.wpi.first.math.kinematics.ChassisSpeeds @@ -22,7 +22,6 @@ import org.team4099.lib.units.derived.inRadians import org.team4099.lib.units.derived.radians import org.team4099.lib.units.derived.rotations import kotlin.math.hypot -import kotlin.time.times class WheelRadiusCharacterizationCommand( val drivetrain: Drivetrain, @@ -84,16 +83,16 @@ class WheelRadiusCharacterizationCommand( averageWheelPositionDelta /= 4.0 currentEffectiveWheelRadius = (accumGyroYawRads * driveRadius / averageWheelPositionDelta).meters - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "Drivetrain/RadiusCharacterization/DrivePosition", averageWheelPositionDelta.inRadians ) - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "Drivetrain/RadiusCharacterization/AccumGyroYawRads", accumGyroYawRads.inRadians ) - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "Drivetrain/RadiusCharacterization/LastGyroYawRads", lastGyroYawRads.inRadians ) - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "Drivetrain/RadiusCharacterization/CurrentWheelRadiusInches", currentEffectiveWheelRadius.inInches ) @@ -101,12 +100,12 @@ class WheelRadiusCharacterizationCommand( override fun end(interrupted: Boolean) { if (accumGyroYawRads <= (Math.PI * 2.0).radians) { - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "Drivetrain/radiansOffFromWheelRadius", ((Math.PI * 2.0).radians - accumGyroYawRads).inRadians ) } else { - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "Drivetrain/effectiveWheelRadius", currentEffectiveWheelRadius.inInches ) } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt index 7549882c..34487fba 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt @@ -11,7 +11,7 @@ import com.team4099.lib.trajectory.Waypoint import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import com.team4099.robot2023.util.AllianceFlipUtil -import com.team4099.robot2023.util.DebugLogger +import com.team4099.robot2023.util.CustomLogger import com.team4099.robot2023.util.FrameType import com.team4099.robot2023.util.Velocity2d import edu.wpi.first.math.kinematics.ChassisSpeeds @@ -384,7 +384,7 @@ private constructor( Logger.recordOutput("Pathfollow/isAtReference", swerveDriveController.atReference()) Logger.recordOutput("Pathfollow/trajectoryTimeSeconds", trajectory.totalTimeSeconds) - DebugLogger.recordDebugOutput("ActiveCommands/DrivePathCommand", true) + CustomLogger.recordDebugOutput("ActiveCommands/DrivePathCommand", true) if (thetakP.hasChanged()) thetaPID.proportionalGain = thetakP.get() if (thetakI.hasChanged()) thetaPID.integralGain = thetakI.get() @@ -414,7 +414,7 @@ private constructor( } override fun end(interrupted: Boolean) { - DebugLogger.recordDebugOutput("ActiveCommands/DrivePathCommand", false) + CustomLogger.recordDebugOutput("ActiveCommands/DrivePathCommand", false) if (interrupted) { DriverStation.reportError(errorString, true) // Stop where we are if interrupted diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetGyroYawCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetGyroYawCommand.kt index d3545ff7..e0c24fbc 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetGyroYawCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetGyroYawCommand.kt @@ -1,7 +1,7 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import com.team4099.robot2023.util.DebugLogger +import com.team4099.robot2023.util.CustomLogger import edu.wpi.first.wpilibj2.command.Command import org.team4099.lib.units.derived.Angle import org.team4099.lib.units.derived.degrees @@ -17,7 +17,7 @@ class ResetGyroYawCommand(val drivetrain: Drivetrain, val toAngle: Angle = 0.0.d } override fun execute() { - DebugLogger.recordDebugOutput("ActiveCommands/ResetGyroYawCommand", true) + CustomLogger.recordDebugOutput("ActiveCommands/ResetGyroYawCommand", true) } override fun isFinished(): Boolean { diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetPoseCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetPoseCommand.kt index b8e35831..000cc679 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetPoseCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetPoseCommand.kt @@ -2,7 +2,7 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import com.team4099.robot2023.util.AllianceFlipUtil -import com.team4099.robot2023.util.DebugLogger +import com.team4099.robot2023.util.CustomLogger import edu.wpi.first.wpilibj2.command.Command import org.team4099.lib.geometry.Pose2d @@ -14,12 +14,12 @@ class ResetPoseCommand(val drivetrain: Drivetrain, val pose: Pose2d) : Command() override fun initialize() { drivetrain.resetFieldFrameEstimator(AllianceFlipUtil.apply(pose)) drivetrain.zeroGyroYaw(AllianceFlipUtil.apply(pose).rotation) - DebugLogger.recordDebugOutput("Drivetrain/lastResetPose", AllianceFlipUtil.apply(pose).pose2d) - DebugLogger.recordDebugOutput("ActiveCommands/ResetPoseCommand", true) + CustomLogger.recordDebugOutput("Drivetrain/lastResetPose", AllianceFlipUtil.apply(pose).pose2d) + CustomLogger.recordDebugOutput("ActiveCommands/ResetPoseCommand", true) } override fun isFinished(): Boolean { - DebugLogger.recordDebugOutput("ActiveCommands/ResetPoseCommand", false) + CustomLogger.recordDebugOutput("ActiveCommands/ResetPoseCommand", false) return true } } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetZeroCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetZeroCommand.kt index 8d2ddce4..acd806b3 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetZeroCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetZeroCommand.kt @@ -1,7 +1,7 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import com.team4099.robot2023.util.DebugLogger +import com.team4099.robot2023.util.CustomLogger import edu.wpi.first.wpilibj2.command.Command class ResetZeroCommand(val drivetrain: Drivetrain) : Command() { @@ -14,6 +14,6 @@ class ResetZeroCommand(val drivetrain: Drivetrain) : Command() { } override fun execute() { - DebugLogger.recordDebugOutput("ActiveCommands/ResetZeroCommand", true) + CustomLogger.recordDebugOutput("ActiveCommands/ResetZeroCommand", true) } } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetAngleCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetAngleCommand.kt index 5ac1cd55..3aaf5ed1 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetAngleCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetAngleCommand.kt @@ -4,7 +4,7 @@ import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import com.team4099.robot2023.subsystems.superstructure.Request -import com.team4099.robot2023.util.DebugLogger +import com.team4099.robot2023.util.CustomLogger import com.team4099.robot2023.util.driver.DriverProfile import edu.wpi.first.wpilibj.RobotBase import edu.wpi.first.wpilibj2.command.Command @@ -109,14 +109,14 @@ class TargetAngleCommand( override fun execute() { drivetrain.defaultCommand.end(true) - DebugLogger.recordDebugOutput("ActiveCommands/TargetAngleCommand", true) + CustomLogger.recordDebugOutput("ActiveCommands/TargetAngleCommand", true) Logger.recordOutput( "Testing/CurrentDrivetrainRotation", drivetrain.odomTRobot.rotation.inDegrees ) val thetaFeedback = thetaPID.calculate(drivetrain.odomTRobot.rotation, targetAngle()) - DebugLogger.recordDebugOutput("Testing/error", thetaPID.error.inDegrees) - DebugLogger.recordDebugOutput("Testing/thetaFeedback", thetaFeedback.inDegreesPerSecond) + CustomLogger.recordDebugOutput("Testing/error", thetaPID.error.inDegrees) + CustomLogger.recordDebugOutput("Testing/thetaFeedback", thetaFeedback.inDegreesPerSecond) drivetrain.currentRequest = Request.DrivetrainRequest.OpenLoop( @@ -131,7 +131,7 @@ class TargetAngleCommand( } override fun end(interrupted: Boolean) { - DebugLogger.recordDebugOutput("ActiveCommands/TargetAngleCommand", false) + CustomLogger.recordDebugOutput("ActiveCommands/TargetAngleCommand", false) drivetrain.currentRequest = Request.DrivetrainRequest.OpenLoop( driver.rotationSpeedClampedSupplier(turn, slowMode), diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetNoteCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetNoteCommand.kt index b83fcdd6..7cf3e7c6 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetNoteCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetNoteCommand.kt @@ -6,7 +6,7 @@ import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import com.team4099.robot2023.subsystems.feeder.Feeder import com.team4099.robot2023.subsystems.limelight.LimelightVision import com.team4099.robot2023.subsystems.superstructure.Request -import com.team4099.robot2023.util.DebugLogger +import com.team4099.robot2023.util.CustomLogger import com.team4099.robot2023.util.driver.DriverProfile import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj.RobotBase @@ -116,11 +116,12 @@ class TargetNoteCommand( override fun execute() { drivetrain.defaultCommand.end(true) + Logger.recordOutput("ActiveCommands/TargetNoteCommand", true) val thetaFeedback = thetaPID.calculate(limelight.targetGamePieceTx ?: 0.0.degrees, 0.0.degrees) - DebugLogger.recordDebugOutput("NoteAlignment/error", thetaPID.error.inDegrees) - DebugLogger.recordDebugOutput("NoteAlignment/thetaFeedback", thetaFeedback.inDegreesPerSecond) + CustomLogger.recordDebugOutput("NoteAlignment/error", thetaPID.error.inDegrees) + CustomLogger.recordDebugOutput("NoteAlignment/thetaFeedback", thetaFeedback.inDegreesPerSecond) if (!feeder.hasNote && limelight.inputs.gamePieceTargets.size > 0) { val driveVector = driver.driveSpeedClampedSupplier(driveX, driveY, slowMode) diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetSpeakerCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetSpeakerCommand.kt index 31b9d0da..abf9ba00 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetSpeakerCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetSpeakerCommand.kt @@ -124,7 +124,6 @@ class TargetSpeakerCommand( } drivetrain.defaultCommand.end(true) - superstructure.defaultCommand.end(true) Logger.recordOutput("ActiveCommands/TargetAngleCommand", true) Logger.recordOutput( "Testing/CurrentDrivetrainRotation", drivetrain.odomTRobot.rotation.inDegrees diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt index 0ce65a00..6e068e01 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt @@ -1,7 +1,7 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import com.team4099.robot2023.util.DebugLogger +import com.team4099.robot2023.util.CustomLogger import com.team4099.robot2023.util.driver.DriverProfile import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj2.command.Command @@ -28,7 +28,7 @@ class TeleopDriveCommand( val rotation = driver.rotationSpeedClampedSupplier(turn, slowMode) drivetrain.currentRequest = DrivetrainRequest.OpenLoop(rotation, speed) - DebugLogger.recordDebugOutput("ActiveCommands/TeleopDriveCommand", true) + CustomLogger.recordDebugOutput("ActiveCommands/TeleopDriveCommand", true) } } override fun isFinished(): Boolean { @@ -36,6 +36,6 @@ class TeleopDriveCommand( } override fun end(interrupted: Boolean) { - DebugLogger.recordDebugOutput("ActiveCommands/TeleopDriveCommand", false) + CustomLogger.recordDebugOutput("ActiveCommands/TeleopDriveCommand", false) } } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TestDriveCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TestDriveCommand.kt index 7ab857e6..111242dc 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TestDriveCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TestDriveCommand.kt @@ -2,7 +2,7 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import com.team4099.robot2023.subsystems.superstructure.Request -import com.team4099.robot2023.util.DebugLogger +import com.team4099.robot2023.util.CustomLogger import edu.wpi.first.math.kinematics.ChassisSpeeds import edu.wpi.first.wpilibj2.command.Command import org.team4099.lib.units.derived.inRotation2ds @@ -16,7 +16,7 @@ class TestDriveCommand(val drivetrain: Drivetrain) : Command() { 4.0, 0.0, 0.0, drivetrain.odomTRobot.rotation.inRotation2ds ) ) - DebugLogger.recordDebugOutput("ActiveCommands/TestDriveCommand", true) + CustomLogger.recordDebugOutput("ActiveCommands/TestDriveCommand", true) } override fun isFinished(): Boolean { diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ZeroSensorsCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ZeroSensorsCommand.kt index 4bdabc03..ff3178e1 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ZeroSensorsCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ZeroSensorsCommand.kt @@ -1,7 +1,7 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import com.team4099.robot2023.util.DebugLogger +import com.team4099.robot2023.util.CustomLogger import edu.wpi.first.wpilibj2.command.Command import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest @@ -20,6 +20,6 @@ class ZeroSensorsCommand(val drivetrain: Drivetrain) : Command() { } override fun execute() { - DebugLogger.recordDebugOutput("ActiveCommands/ZeroSensorsCommand", true) + CustomLogger.recordDebugOutput("ActiveCommands/ZeroSensorsCommand", true) } } diff --git a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt index 89c0b871..a77b9930 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj.GenericHID import edu.wpi.first.wpilibj2.command.button.Trigger import org.team4099.lib.joystick.XboxOneGamepad import java.util.function.Consumer +import kotlin.math.absoluteValue /** * Maps buttons on the driver and operator controllers to specific actions with meaningful variable @@ -28,7 +29,13 @@ object ControlBoard { get() = -driver.leftYAxis val turn: Double - get() = driver.rightXAxis * DrivetrainConstants.TELEOP_TURNING_SPEED_PERCENT + get() { + return if (driver.rightXAxis.absoluteValue < 0.90) { + driver.rightXAxis * DrivetrainConstants.TELEOP_TURNING_SPEED_PERCENT + } else { + driver.rightXAxis + } + } val slowMode: Boolean get() = driver.leftShoulderButton @@ -73,7 +80,7 @@ object ControlBoard { val targetSpeaker = Trigger { driver.xButton } // TODO: switch back to climbAlignLeft val climbAutoAlign = Trigger { driver.bButton } - val lockWheels = Trigger { driver.selectButton } + val lockWheels = Trigger { driver.startButton } // week0 controls } 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 21fab9c7..4598ced7 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt @@ -33,7 +33,7 @@ object DrivetrainConstants { const val FOC_ENABLED = true const val MINIMIZE_SKEW = false - const val TELEOP_TURNING_SPEED_PERCENT = 0.7 + const val TELEOP_TURNING_SPEED_PERCENT = 0.6 const val OMOMETRY_UPDATE_FREQUENCY = 250.0 diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt index 3d66af21..84ccbda5 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt @@ -34,5 +34,5 @@ object FeederConstants { val OUTTAKE_NOTE_VOLTAGE = (-6).volts - val beamBreakFilterTime = 0.01.seconds + val beamBreakFilterTime = 0.05.seconds } diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt index 65a05646..1e61a4d2 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt @@ -28,7 +28,7 @@ object IntakeConstants { const val CENTER_WHEEL_GEAR_RATIO = 34.0 / 14.0 // TODO: Update the idle roller voltage later - val IDLE_ROLLER_VOLTAGE = 2.0.volts + val IDLE_ROLLER_VOLTAGE = 1.5.volts val IDLE_CENTER_WHEEL_VOLTAGE = 0.0.volts val INTAKE_ROLLER_VOLTAGE = -12.volts diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/LEDConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/LEDConstants.kt index c17767dd..3331a31a 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/LEDConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/LEDConstants.kt @@ -1,6 +1,7 @@ package com.team4099.robot2023.config.constants import com.ctre.phoenix.led.Animation +import com.ctre.phoenix.led.StrobeAnimation import org.team4099.lib.units.base.amps import org.team4099.lib.units.derived.volts @@ -33,6 +34,7 @@ object LEDConstants { // Yellow BATTERY_DISPLAY(null, 255, 105, 0), LOW_BATTERY_WARNING(null, 67, 36, 255), + TUNING_MODE_WARNING(StrobeAnimation(255, 0, 0), 0, 0, 0), WHITE(null, 255, 255, 255), // Green 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 8f4d6f13..cb60870f 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt @@ -37,9 +37,10 @@ object WristConstants { val ABSOLUTE_ENCODER_OFFSET = ( 97.72227856659904.degrees - 35.degrees + 1.90.degrees - - 0.5.degrees - - 1.1.degrees - // add to drop angle + 0.55.degrees - + 0.6.degrees - // add to drop angle 1.degrees - + 0.5.degrees - 96.3.degrees ) * ABSOLUTE_ENCODER_TO_MECHANISM_GEAR_RATIO val WRIST_LENGTH = 18.6.inches @@ -66,14 +67,14 @@ object WristConstants { val ARBITRARY_FEEDFORWARD = 0.03.volts - val REAL_KP: ProportionalGain = 0.72.volts / 1.0.degrees + val REAL_KP: ProportionalGain = 0.79.volts / 1.0.degrees val REAL_KI: IntegralGain = 0.0.volts / (1.0.degrees * 1.0.seconds) val REAL_KD: DerivativeGain = 0.0.volts / 1.0.degrees.perSecond val FIRST_STAGE_POS_SWITCH_THRESHOLD = 3.0.degrees val FIRST_STAGE_VEL_SWITCH_THRESHOLD = 3.0.degrees.perSecond - val FIRST_STAGE_KP: ProportionalGain = 0.69.volts / 1.0.degrees + val FIRST_STAGE_KP: ProportionalGain = 0.9.volts / 1.0.degrees val FIRST_STAGE_KI: IntegralGain = 0.0.volts / (1.0.degrees * 1.0.seconds) val FIRST_STAGE_KD: DerivativeGain = 0.0.volts / 1.0.degrees.perSecond @@ -99,7 +100,7 @@ object WristConstants { val SIM_WRIST_KS = 0.15.volts } - val WRIST_TOLERANCE = 0.25.degrees + val WRIST_TOLERANCE = 0.3.degrees val IDLE_ANGLE = (-33.5).degrees diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt index 9812deaa..22abd3f4 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt @@ -385,6 +385,10 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB // Transitions nextState = fromRequestToState(currentRequest) } + DrivetrainState.LOCK_WHEELS -> { + lockWheels() + nextState = fromRequestToState(currentRequest) + } } currentState = nextState @@ -755,18 +759,10 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB } fun lockWheels() { - DrivetrainIOReal.getSwerveModules()[1].setOpenLoop( - DrivetrainConstants.FL_LOCKING_ANGLE, 0.meters.perSecond, true - ) - DrivetrainIOReal.getSwerveModules()[1].setOpenLoop( - DrivetrainConstants.FR_LOCKING_ANGLE, 0.meters.perSecond, true - ) - DrivetrainIOReal.getSwerveModules()[1].setOpenLoop( - DrivetrainConstants.BL_LOCKING_ANGLE, 0.meters.perSecond, true - ) - DrivetrainIOReal.getSwerveModules()[1].setOpenLoop( - DrivetrainConstants.BR_LOCKING_ANGLE, 0.meters.perSecond, true - ) + swerveModules[0].setOpenLoop(DrivetrainConstants.FL_LOCKING_ANGLE, 0.meters.perSecond, true) + swerveModules[1].setOpenLoop(DrivetrainConstants.FR_LOCKING_ANGLE, 0.meters.perSecond, true) + swerveModules[2].setOpenLoop(DrivetrainConstants.BL_LOCKING_ANGLE, 0.meters.perSecond, true) + swerveModules[3].setOpenLoop(DrivetrainConstants.BR_LOCKING_ANGLE, 0.meters.perSecond, true) } fun lockWheelsCommand(): Command { diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/falconspin/Motor.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/falconspin/Motor.kt index a738e9ae..02cb16b3 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/falconspin/Motor.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/falconspin/Motor.kt @@ -23,6 +23,8 @@ interface MotorType typealias REVNeo = MotorType +typealias Kraken = MotorType + typealias Falcon = MotorType typealias SimMotor = MotorType diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/falconspin/MotorChecker.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/falconspin/MotorChecker.kt index 2f88041b..a3e2cf97 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/falconspin/MotorChecker.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/falconspin/MotorChecker.kt @@ -1,6 +1,6 @@ package com.team4099.robot2023.subsystems.falconspin -import com.team4099.robot2023.util.DebugLogger +import com.team4099.robot2023.util.CustomLogger import edu.wpi.first.wpilibj.DriverStation import org.littletonrobotics.junction.Logger import org.team4099.lib.units.base.inAmperes @@ -27,7 +27,7 @@ object MotorChecker { } fun periodic() { - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "MotorChecker/subsystemNames", subsystemHardware.keys.toTypedArray() ) @@ -69,7 +69,7 @@ object MotorChecker { } } } - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "MotorChecker/$subsystemName/motorNames", motorNames.toTypedArray() ) @@ -89,57 +89,57 @@ object MotorChecker { // not clean but whatever fun logMotor(subsystemName: String, motor: Motor) { - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "MotorChecker/$subsystemName/${motor.name}/AppliedVoltageVolts", motor.appliedVoltage.inVolts ) - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "MotorChecker/$subsystemName/${motor.name}/BusVoltageVolts", motor.busVoltage.inVolts ) - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "MotorChecker/$subsystemName/${motor.name}/TemperatureCelsius", motor.temperature.inCelsius ) - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "MotorChecker/$subsystemName/${motor.name}/StatorCurrentAmps", motor.statorCurrent.inAmperes ) - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "MotorChecker/$subsystemName/${motor.name}/SupplyCurrentAmps", motor.supplyCurrent.inAmperes ) - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "MotorChecker/$subsystemName/${motor.name}/CurrentLimitStage", motor.currentLimitStage.name ) - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "MotorChecker/$subsystemName/${motor.name}/BaseCurrentLimitAmps", motor.baseCurrentLimit.inAmperes ) - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "MotorChecker/$subsystemName/${motor.name}/FirstStageTemperatureLimitCelsius", motor.firstStageTemperatureLimit.inCelsius ) - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "MotorChecker/$subsystemName/${motor.name}/FirstStageCurrentLimitAmps", motor.firstStageCurrentLimit.inAmperes ) - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "MotorChecker/$subsystemName/${motor.name}/MotorShutDownThresholdCelsius", motor.motorShutDownThreshold.inCelsius ) - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "MotorChecker/$subsystemName/${motor.name}/CurrentLimitInUseAmps", motor.currentLimitInUse.inAmperes ) - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "MotorChecker/$subsystemName/${motor.name}/MotorID", motor.id.toLong() ) - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "MotorChecker/$subsystemName/${motor.name}/Errors", motor.errors.toTypedArray() ) - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "MotorChecker/$subsystemName/${motor.name}/Warnings", motor.warnings.toTypedArray() ) - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "MotorChecker/$subsystemName/${motor.name}/Info", motor.info.toTypedArray() ) - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "MotorChecker/$subsystemName/${motor.name}/StickyFaults", motor.stickyFaults.toTypedArray() ) } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt index de7268fb..1712ac7a 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt @@ -5,7 +5,7 @@ import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.FeederConstants import com.team4099.robot2023.subsystems.superstructure.Request -import com.team4099.robot2023.util.DebugLogger +import com.team4099.robot2023.util.CustomLogger import edu.wpi.first.math.filter.Debouncer import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.SubsystemBase @@ -146,7 +146,7 @@ class Feeder(val io: FeederIO) : SubsystemBase() { Logger.recordOutput( "Feeder/isAtCommandedState", currentState.equivalentToRequest(currentRequest) ) - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "Feeder/timeProfileGeneratedAt", timeProfileGeneratedAt.inSeconds ) Logger.recordOutput("Feeder/feederVoltageTarget", feederTargetVoltage.inVolts) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIOTalon.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIOTalon.kt index 7c9101b9..a2b1030f 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIOTalon.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIOTalon.kt @@ -19,7 +19,7 @@ import com.team4099.robot2023.config.constants.FlywheelConstants import com.team4099.robot2023.subsystems.falconspin.Falcon500 import com.team4099.robot2023.subsystems.falconspin.MotorChecker import com.team4099.robot2023.subsystems.falconspin.MotorCollection -import com.team4099.robot2023.util.DebugLogger +import com.team4099.robot2023.util.CustomLogger import org.littletonrobotics.junction.Logger import org.team4099.lib.units.AngularVelocity import org.team4099.lib.units.Velocity @@ -200,10 +200,10 @@ object FlywheelIOTalon : FlywheelIO { override fun setFlywheelVelocity(velocity: AngularVelocity, feedforward: ElectricalPotential) { val error = velocity - flywheelLeftSensor.velocity - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "Flywheel/premotionTargetedVelocity", velocity.inRotationsPerMinute ) - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "Flywheel/motionTargetedVelocity", flywheelLeftSensor.velocityToRawUnits(velocity) ) // flywheelRightTalon.setControl( diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt index 015ae4ef..04bbbd9e 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt @@ -5,7 +5,7 @@ import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.IntakeConstants import com.team4099.robot2023.subsystems.superstructure.Request -import com.team4099.robot2023.util.DebugLogger +import com.team4099.robot2023.util.CustomLogger import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.SubsystemBase import org.littletonrobotics.junction.Logger @@ -109,7 +109,7 @@ class Intake(val io: IntakeIO) : SubsystemBase() { Logger.recordOutput( "Intake/isAtCommandedState", currentState.equivalentToRequest(currentRequest) ) - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "Intake/timeProfileGeneratedAt", timeProfileGeneratedAt.inSeconds ) Logger.recordOutput("Intake/rollerVoltageTarget", rollerVoltageTarget.inVolts) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt index 4a5d674b..eaf5f4cb 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt @@ -1,7 +1,8 @@ package com.team4099.robot2023.subsystems.led +import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.LEDConstants -import com.team4099.robot2023.util.DebugLogger +import com.team4099.robot2023.util.CustomLogger import com.team4099.robot2023.util.FMSData import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj.RobotController @@ -15,8 +16,10 @@ class Leds(val io: LedIO) { var subsystemsAtPosition = false var isAutoAiming = false var isAmping = false + var isPreping = false var seesGamePiece = false var seesTag = true + var isTuningDebugging = Constants.Tuning.TUNING_MODE || Constants.Tuning.DEBUGING_MODE var state = LEDConstants.CandleState.NO_NOTE set(value) { @@ -32,6 +35,8 @@ class Leds(val io: LedIO) { state = if (io.batteryVoltage < 12.3.volts) { LEDConstants.CandleState.LOW_BATTERY_WARNING + } else if (isTuningDebugging) { + LEDConstants.CandleState.TUNING_MODE_WARNING } else { LEDConstants.CandleState.GOLD } @@ -50,7 +55,7 @@ class Leds(val io: LedIO) { } else { state = LEDConstants.CandleState.CAN_SHOOT } - } else if (isAmping) { + } else if (isAmping || isPreping) { if (subsystemsAtPosition) { state = LEDConstants.CandleState.CAN_SHOOT } else { @@ -66,6 +71,6 @@ class Leds(val io: LedIO) { } Logger.processInputs("LED", inputs) - DebugLogger.recordDebugOutput("LED/state", state.name) + CustomLogger.recordDebugOutput("LED/state", state.name) } } 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 bc4eb6af..2f758847 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt @@ -37,7 +37,6 @@ import org.team4099.lib.units.derived.rotations import org.team4099.lib.units.derived.volts import org.team4099.lib.units.inRotationsPerMinute import org.team4099.lib.units.perMinute -import org.team4099.lib.units.perSecond class Superstructure( private val intake: Intake, @@ -150,7 +149,7 @@ class Superstructure( notes.forEach { it.currentState = NoteSimulation.NoteStates.STAGED } notes[0].currentState = NoteSimulation.NoteStates.IN_ROBOT - notes[7].currentState = NoteSimulation.NoteStates.OFF_FIELD + // notes[7].currentState = NoteSimulation.NoteStates.OFF_FIELD noteHoldingID = 0 } } @@ -161,6 +160,10 @@ class Superstructure( leds.isAmping = (currentState == SuperstructureStates.WRIST_AMP_PREP) || (currentState == SuperstructureStates.ELEVATOR_AMP_PREP) + leds.isPreping = + (currentState == SuperstructureStates.PASSING_SHOT_PREP) || + (currentState == SuperstructureStates.SCORE_SPEAKER_LOW_PREP) || + (currentState == SuperstructureStates.SCORE_SPEAKER_HIGH_PREP) leds.subsystemsAtPosition = wrist.isAtTargetedPosition && flywheel.isAtTargetedVelocity && elevator.isAtTargetedPosition leds.seesGamePiece = limelight.inputs.gamePieceTargets.size > 0 @@ -286,8 +289,6 @@ class Superstructure( } SuperstructureStates.IDLE -> { - intake.currentRequest = Request.IntakeRequest.OpenLoop(0.0.volts, 0.0.volts) - feeder.currentRequest = Request.FeederRequest.OpenLoopIntake(Feeder.TunableFeederStates.idleVoltage.get()) @@ -313,6 +314,9 @@ class Superstructure( Intake.TunableIntakeStates.idleCenterWheelVoltage.get() ) } else { + + intake.currentRequest = Request.IntakeRequest.OpenLoop(0.0.volts, 0.0.volts) + wrist.currentRequest = Request.WristRequest.TargetingPosition(Wrist.TunableWristStates.idleAngle.get()) } @@ -482,14 +486,8 @@ class Superstructure( Logger.recordOutput("AutoAim/FlywheelSpeed", targetFlywheelSpeed.inRotationsPerMinute) Logger.recordOutput("AutoAim/WristAngle", targetWristAngle.inDegrees) - if (drivetrain.fieldVelocity.magnitude.absoluteValue < 0.1.meters.perSecond) { - flywheel.currentRequest = Request.FlywheelRequest.TargetingVelocity(targetFlywheelSpeed) - wrist.currentRequest = Request.WristRequest.TargetingPosition(targetWristAngle) - } else { - flywheel.currentRequest = Request.FlywheelRequest.TargetingVelocity(targetFlywheelSpeed) - wrist.currentRequest = - Request.WristRequest.TargetingPosition(targetWristAngle, 2.0.degrees) - } + flywheel.currentRequest = Request.FlywheelRequest.TargetingVelocity(targetFlywheelSpeed) + wrist.currentRequest = Request.WristRequest.TargetingPosition(targetWristAngle) when (currentRequest) { is Request.SuperstructureRequest.Idle -> { @@ -718,7 +716,7 @@ class Superstructure( SuperstructureStates.MANUAL_SCORE_SPEAKER_PREP -> { flywheel.currentRequest = Request.FlywheelRequest.TargetingVelocity(flywheelToShootAt) wrist.currentRequest = - Request.WristRequest.TargetingPosition(wristAngleToShootAt, 1.5.degrees) + Request.WristRequest.TargetingPosition(wristAngleToShootAt, 0.5.degrees) if (wrist.isAtTargetedPosition && flywheel.isAtTargetedVelocity && currentRequest is Request.SuperstructureRequest.ScoreSpeaker diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt index 28f48e95..9b3bfbef 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt @@ -114,7 +114,7 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { PhotonUtils.calculateDistanceToTargetMeters( cameraPoses[instance].translation.z.inMeters, 57.125.inches.inMeters, - 22.42.degrees.inRadians, + 24.02.degrees.inRadians, tag.pitch.degrees.inRadians ) .meters + 4.inches diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/Wrist.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/Wrist.kt index 69d11d8a..c2f50ed1 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/Wrist.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/Wrist.kt @@ -5,7 +5,7 @@ import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.WristConstants import com.team4099.robot2023.subsystems.superstructure.Request -import com.team4099.robot2023.util.DebugLogger +import com.team4099.robot2023.util.CustomLogger import edu.wpi.first.wpilibj.RobotBase import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.Commands @@ -417,7 +417,7 @@ class Wrist(val io: WristIO) : SubsystemBase() { } } - DebugLogger.recordDebugOutput("Wrist/profileIsOutOfBounds", isOutOfBounds(setPoint.velocity)) + CustomLogger.recordDebugOutput("Wrist/profileIsOutOfBounds", isOutOfBounds(setPoint.velocity)) Logger.recordOutput("Wrist/armFeedForward", feedforward.inVolts) Logger.recordOutput("Wrist/armTargetPosition", setPoint.position.inDegrees) Logger.recordOutput("Wrist/armTargetVelocity", setPoint.velocity.inDegreesPerSecond) diff --git a/src/main/kotlin/com/team4099/robot2023/util/DebugLogger.kt b/src/main/kotlin/com/team4099/robot2023/util/CustomLogger.kt similarity index 61% rename from src/main/kotlin/com/team4099/robot2023/util/DebugLogger.kt rename to src/main/kotlin/com/team4099/robot2023/util/CustomLogger.kt index 35e41368..e52956c4 100644 --- a/src/main/kotlin/com/team4099/robot2023/util/DebugLogger.kt +++ b/src/main/kotlin/com/team4099/robot2023/util/CustomLogger.kt @@ -7,9 +7,90 @@ import edu.wpi.first.util.struct.Struct import edu.wpi.first.util.struct.StructSerializable import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d import org.littletonrobotics.junction.Logger +import org.littletonrobotics.junction.inputs.LoggableInputs -class DebugLogger { +class CustomLogger { companion object { + inline fun processInputs(key: String, inputs: LoggableInputs) { + Logger.processInputs(key, inputs) + } + + inline fun > recordOutput(key: String, value: E) { + Logger.recordOutput(key, value) + } + + inline fun recordOutput(key: String, value: T) { + Logger.recordOutput(key, value) + } + + inline fun > recordOutput(key: String, value: Measure) { + Logger.recordOutput(key, value) + } + + inline fun recordOutput(key: String, value: Struct) { + Logger.recordOutput(key, value) + } + + inline fun recordOutput(key: String, vararg value: T) { + Logger.recordOutput(key, *value) + } + + inline fun recordOutput(key: String, value: Mechanism2d) { + Logger.recordOutput(key, value) + } + + inline fun recordOutput(key: String, value: Array) { + Logger.recordOutput(key, value) + } + + inline fun recordOutput(key: String, value: BooleanArray) { + Logger.recordOutput(key, value) + } + + inline fun recordOutput(key: String, value: Boolean) { + Logger.recordOutput(key, value) + } + + inline fun recordOutput(key: String, value: ByteArray) { + Logger.recordOutput(key, value) + } + + inline fun recordOutput(key: String, value: DoubleArray) { + Logger.recordOutput(key, value) + } + + inline fun recordOutput(key: String, value: Double) { + Logger.recordOutput(key, value) + } + + inline fun recordOutput(key: String, value: Float) { + Logger.recordOutput(key, value) + } + + inline fun recordOutput(key: String, value: FloatArray) { + Logger.recordOutput(key, value) + } + + inline fun recordOutput(key: String, value: Int) { + Logger.recordOutput(key, value) + } + + inline fun recordOutput(key: String, value: IntArray) { + Logger.recordOutput(key, value) + } + + inline fun recordOutput(key: String, value: Long) { + Logger.recordOutput(key, value) + } + + inline fun recordOutput(key: String, value: LongArray) { + Logger.recordOutput(key, value) + } + + inline fun recordOutput(key: String, value: String) { + Logger.recordOutput(key, value) + } + inline fun > recordDebugOutput(key: String, value: E) { if (Constants.Tuning.DEBUGING_MODE) { Logger.recordOutput(key, value) diff --git a/src/main/kotlin/com/team4099/robot2023/util/NoteSimulation.kt b/src/main/kotlin/com/team4099/robot2023/util/NoteSimulation.kt index d1ce504c..37a2ec71 100644 --- a/src/main/kotlin/com/team4099/robot2023/util/NoteSimulation.kt +++ b/src/main/kotlin/com/team4099/robot2023/util/NoteSimulation.kt @@ -110,14 +110,14 @@ class NoteSimulation( var flywheelAngularVelocitySupplier: () -> AngularVelocity = { 0.0.rotations.perMinute } fun periodic() { - DebugLogger.recordDebugOutput("NoteData/$id/inclinationAngle", inclinationAngleTheta.inDegrees) - DebugLogger.recordDebugOutput("NoteData/$id/azimuthalAngle", azimuthalAnglePhi.inDegrees) - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput("NoteData/$id/inclinationAngle", inclinationAngleTheta.inDegrees) + CustomLogger.recordDebugOutput("NoteData/$id/azimuthalAngle", azimuthalAnglePhi.inDegrees) + CustomLogger.recordDebugOutput( "NoteData/$id/launchVelocityMetersPerSecond", launchVelocity.inMetersPerSecond ) - DebugLogger.recordDebugOutput("NoteData/$id/startPose", launchStartPose.pose3d) - DebugLogger.recordDebugOutput("NoteData/$id/state", currentState.name) - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput("NoteData/$id/startPose", launchStartPose.pose3d) + CustomLogger.recordDebugOutput("NoteData/$id/state", currentState.name) + CustomLogger.recordDebugOutput( "NoteSimulation/IntakePose", poseSupplier().transformBy(IntakeConstants.INTAKE_TRANSFORM).pose2d ) diff --git a/src/main/kotlin/com/team4099/robot2023/util/driver/Jessika.kt b/src/main/kotlin/com/team4099/robot2023/util/driver/Jessika.kt new file mode 100644 index 00000000..b1c5a2d0 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/util/driver/Jessika.kt @@ -0,0 +1,3 @@ +package com.team4099.robot2023.util.driver + +class Jessika : DriverProfile(false, true, 2, 2) diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index e63a4888..8f8e893e 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2024.2.7", + "version": "v2024.3.1", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2024", "mavenUrls": [ @@ -14,7 +14,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2024.2.7", + "version": "v2024.3.1", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -29,7 +29,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2024.2.7", + "version": "v2024.3.1", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -46,12 +46,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2024.2.7" + "version": "v2024.3.1" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2024.2.7" + "version": "v2024.3.1" } ] } \ No newline at end of file