From edfae15fe0231131872e8b8e8c02a85273069dfe Mon Sep 17 00:00:00 2001 From: Matthew Choulas Date: Wed, 17 Apr 2024 16:59:31 -0400 Subject: [PATCH 1/7] worlds load in debugging --- src/main/kotlin/com/team4099/robot2023/Robot.kt | 1 - .../kotlin/com/team4099/robot2023/RobotContainer.kt | 3 ++- .../com/team4099/robot2023/config/ControlBoard.kt | 2 +- .../team4099/robot2023/config/constants/Constants.kt | 4 ++-- .../config/constants/DrivetrainConstants.kt | 4 ++-- .../robot2023/config/constants/FeederConstants.kt | 2 +- .../robot2023/config/constants/IntakeConstants.kt | 2 +- .../robot2023/config/constants/WristConstants.kt | 8 ++++---- .../subsystems/drivetrain/drive/Drivetrain.kt | 12 ++++++++---- .../subsystems/superstructure/Superstructure.kt | 5 +++-- .../team4099/robot2023/subsystems/vision/Vision.kt | 2 +- 11 files changed, 25 insertions(+), 20 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/Robot.kt b/src/main/kotlin/com/team4099/robot2023/Robot.kt index ea1281cd..e2db1832 100644 --- a/src/main/kotlin/com/team4099/robot2023/Robot.kt +++ b/src/main/kotlin/com/team4099/robot2023/Robot.kt @@ -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..f38de59d 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -343,7 +343,8 @@ 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/config/ControlBoard.kt b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt index 89c0b871..655a17ab 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt @@ -73,7 +73,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..78a47d3a 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt @@ -76,8 +76,8 @@ object DrivetrainConstants { val ALLOWED_STEERING_ANGLE_ERROR = 1.degrees val STEERING_SUPPLY_CURRENT_LIMIT = 20.0.amps - val DRIVE_SUPPLY_CURRENT_LIMIT = 70.0.amps - val DRIVE_THRESHOLD_CURRENT_LIMIT = 70.0.amps + val DRIVE_SUPPLY_CURRENT_LIMIT = 80.0.amps + val DRIVE_THRESHOLD_CURRENT_LIMIT = 80.0.amps val DRIVE_TRIGGER_THRESHOLD_TIME = 0.1.seconds val DRIVE_STATOR_CURRENT_LIMIT = 80.0.amps 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/WristConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt index 8f4d6f13..ad8c533f 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt @@ -38,7 +38,7 @@ object WristConstants { ( 97.72227856659904.degrees - 35.degrees + 1.90.degrees - 0.5.degrees - - 1.1.degrees - // add to drop angle + 1.4.degrees - // add to drop angle 1.degrees - 96.3.degrees ) * ABSOLUTE_ENCODER_TO_MECHANISM_GEAR_RATIO @@ -66,14 +66,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 +99,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..ed682784 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,16 +759,16 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB } fun lockWheels() { - DrivetrainIOReal.getSwerveModules()[1].setOpenLoop( + swerveModules[0].setOpenLoop( DrivetrainConstants.FL_LOCKING_ANGLE, 0.meters.perSecond, true ) - DrivetrainIOReal.getSwerveModules()[1].setOpenLoop( + swerveModules[1].setOpenLoop( DrivetrainConstants.FR_LOCKING_ANGLE, 0.meters.perSecond, true ) - DrivetrainIOReal.getSwerveModules()[1].setOpenLoop( + swerveModules[2].setOpenLoop( DrivetrainConstants.BL_LOCKING_ANGLE, 0.meters.perSecond, true ) - DrivetrainIOReal.getSwerveModules()[1].setOpenLoop( + swerveModules[3].setOpenLoop( DrivetrainConstants.BR_LOCKING_ANGLE, 0.meters.perSecond, true ) } 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..38171a2a 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt @@ -286,8 +286,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 +311,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()) } 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..f78b9819 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, + 23.42.degrees.inRadians, tag.pitch.degrees.inRadians ) .meters + 4.inches From dc499c542aac3e4222b0759e6ef6e45b0fb8faab Mon Sep 17 00:00:00 2001 From: Matthew Choulas Date: Thu, 18 Apr 2024 11:01:32 -0500 Subject: [PATCH 2/7] worlds q1-19 --- simgui-ds.json | 5 + .../com/team4099/robot2023/RobotContainer.kt | 7 +- .../robot2023/auto/AutonomousSelector.kt | 42 +++ .../robot2023/auto/mode/FourNoteAutoPath.kt | 20 +- ...eeNoteAndPickupCenterlineSourceAutoPath.kt | 282 +++++---------- ...enterlineWithoutFirstNoteSourceAutoPath.kt | 265 ++++++++++++++ .../ThreeNoteCenterlineFromAmpAutoPath.kt | 338 +++++++----------- ...teCenterlineWithoutFirstNoteAmpAutoPath.kt | 153 ++++++++ .../commands/drivetrain/TargetNoteCommand.kt | 1 + .../drivetrain/TargetSpeakerCommand.kt | 1 - .../config/constants/DrivetrainConstants.kt | 6 +- .../config/constants/LEDConstants.kt | 2 + .../config/constants/WristConstants.kt | 2 +- .../subsystems/drivetrain/drive/Drivetrain.kt | 16 +- .../team4099/robot2023/subsystems/led/Leds.kt | 7 +- .../superstructure/Superstructure.kt | 17 +- 16 files changed, 717 insertions(+), 447 deletions(-) create mode 100644 src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath.kt create mode 100644 src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteCenterlineWithoutFirstNoteAmpAutoPath.kt 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/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index f38de59d..47530ccb 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -2,7 +2,6 @@ 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 @@ -120,9 +119,6 @@ object RobotContainer { { ControlBoard.slowMode }, drivetrain ) - - superstructure.defaultCommand = superstructure.forceIdleIfAutoAimCommand() - /* module steeing tuning @@ -343,8 +339,7 @@ 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..ddec2ecb 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 @@ -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..d7f72fcf 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPath.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPath.kt @@ -23,7 +23,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, @@ -56,9 +56,8 @@ 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()) ), + superstructure.prepSpeakerLowCommand(), superstructure .scoreCommand() .withTimeout(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds + 0.5), @@ -94,9 +93,8 @@ 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()) ), + superstructure.prepSpeakerLowCommand(), superstructure .scoreCommand() .withTimeout(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds + 0.5), @@ -114,7 +112,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, @@ -130,7 +128,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, @@ -148,20 +146,18 @@ 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()) ), + 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 + FourNoteAutoPath.startingPose.translation.translation2d, null, 180.degrees.inRotation2ds ), FieldWaypoint( Translation2d(4.45.meters, 4.89.meters).translation2d, @@ -190,3 +186,5 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru val startingPose = Pose2d(Translation2d(1.42.meters, 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..1189b094 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteAndPickupCenterlineSourceAutoPath.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteAndPickupCenterlineSourceAutoPath.kt @@ -4,12 +4,12 @@ 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 import org.team4099.lib.geometry.Pose2d import org.team4099.lib.geometry.Translation2d +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 @@ -44,217 +44,101 @@ 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(-2.2.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(-2.2.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()) ) ) } @@ -262,4 +146,4 @@ class ThreeNoteAndPickupCenterlineSourceAutoPath( companion object { val startingPose = Pose2d(0.63.meters, 4.44.meters, 120.degrees) } -} +} \ No newline at end of file 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..70c4bb1e --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath.kt @@ -0,0 +1,265 @@ +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.ConditionalCommand +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, 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 + ) + ) + } + ), + 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 + ) + ) + } + ), + superstructure.groundIntakeCommand() + ), + { superstructure.holdingNote } + ), + 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 + ) + ) + } + ), + 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 + ) + ) + ) + .andThen(superstructure.scoreCommand()), + { superstructure.holdingNote } + ) + ) + } + + 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..25a7738b 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteCenterlineFromAmpAutoPath.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteCenterlineFromAmpAutoPath.kt @@ -4,8 +4,8 @@ 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.ParallelRaceGroup import edu.wpi.first.wpilibj2.command.SequentialCommandGroup import edu.wpi.first.wpilibj2.command.WaitCommand import org.team4099.lib.geometry.Pose2d @@ -19,9 +19,8 @@ import org.team4099.lib.units.perMinute class ThreeNoteCenterlineFromAmpAutoPath( val drivetrain: Drivetrain, - val superstructure: Superstructure, + val superstructure: Superstructure ) : SequentialCommandGroup() { - init { addRequirements(drivetrain) addCommands( @@ -51,226 +50,157 @@ 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.27.meters, 5.78.meters).translation2d, + null, + 160.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.27.meters, 5.78.meters).translation2d, + null, + 160.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() ) } companion object { val startingPose = Pose2d(0.75.meters, 6.70.meters, 240.degrees) } -} +} \ No newline at end of file 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..2bb7ff99 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteCenterlineWithoutFirstNoteAmpAutoPath.kt @@ -0,0 +1,153 @@ +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 + ), + FieldWaypoint( + Translation2d(7.0.meters, 5.7.meters).translation2d, + null, + 160.degrees.inRotation2ds + ) + ) + } + ), + WaitCommand(2.25).andThen(superstructure.groundIntakeCommand()) + ), + 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)) + .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( + (3.9 + 8.27).meters / 2, + (5.7 + 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/drivetrain/TargetNoteCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetNoteCommand.kt index b83fcdd6..6a3a9673 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetNoteCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetNoteCommand.kt @@ -116,6 +116,7 @@ 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) 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/config/constants/DrivetrainConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt index 78a47d3a..ca09845c 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.85 const val OMOMETRY_UPDATE_FREQUENCY = 250.0 @@ -76,8 +76,8 @@ object DrivetrainConstants { val ALLOWED_STEERING_ANGLE_ERROR = 1.degrees val STEERING_SUPPLY_CURRENT_LIMIT = 20.0.amps - val DRIVE_SUPPLY_CURRENT_LIMIT = 80.0.amps - val DRIVE_THRESHOLD_CURRENT_LIMIT = 80.0.amps + val DRIVE_SUPPLY_CURRENT_LIMIT = 70.0.amps + val DRIVE_THRESHOLD_CURRENT_LIMIT = 70.0.amps val DRIVE_TRIGGER_THRESHOLD_TIME = 0.1.seconds val DRIVE_STATOR_CURRENT_LIMIT = 80.0.amps 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 ad8c533f..1e29ae4c 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt @@ -38,7 +38,7 @@ object WristConstants { ( 97.72227856659904.degrees - 35.degrees + 1.90.degrees - 0.5.degrees - - 1.4.degrees - // add to drop angle + 1.0.degrees - // add to drop angle 1.degrees - 96.3.degrees ) * ABSOLUTE_ENCODER_TO_MECHANISM_GEAR_RATIO 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 ed682784..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 @@ -759,18 +759,10 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB } fun lockWheels() { - 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 - ) + 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/led/Leds.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt index 4a5d674b..9ea656ea 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt @@ -1,5 +1,6 @@ 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.FMSData @@ -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 { 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 38171a2a..6709cdb0 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt @@ -150,7 +150,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 +161,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 @@ -483,14 +487,9 @@ 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 -> { From a5429cb657c5e0863763bea257f1373c1e366fc9 Mon Sep 17 00:00:00 2001 From: AlphaPranav9102 <49816864+AlphaPranav9102@users.noreply.github.com> Date: Fri, 19 Apr 2024 00:19:37 -0500 Subject: [PATCH 3/7] code day 1 --- .../paths/New New New New Path.path | 24 +++++- .../robot2023/auto/mode/FourNoteAutoPath.kt | 9 +- ...eeNoteAndPickupCenterlineSourceAutoPath.kt | 4 +- ...enterlineWithoutFirstNoteSourceAutoPath.kt | 86 ++----------------- .../ThreeNoteCenterlineFromAmpAutoPath.kt | 8 +- .../team4099/robot2023/config/ControlBoard.kt | 10 ++- .../config/constants/DrivetrainConstants.kt | 2 +- .../superstructure/Superstructure.kt | 2 +- .../robot2023/subsystems/vision/Vision.kt | 2 +- 9 files changed, 49 insertions(+), 98 deletions(-) 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/auto/mode/FourNoteAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPath.kt index d7f72fcf..46a032e4 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 @@ -157,15 +158,15 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru { listOf( FieldWaypoint( - FourNoteAutoPath.startingPose.translation.translation2d, null, 180.degrees.inRotation2ds + 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 ), @@ -183,7 +184,7 @@ 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 1189b094..a21987b0 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteAndPickupCenterlineSourceAutoPath.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteAndPickupCenterlineSourceAutoPath.kt @@ -67,7 +67,7 @@ class ThreeNoteAndPickupCenterlineSourceAutoPath( ) } ), - superstructure.prepManualSpeakerCommand(-2.2.degrees, 4000.rotations.perMinute), + superstructure.prepManualSpeakerCommand(-2.5.degrees, 4400.rotations.perMinute), superstructure.scoreCommand(), ParallelCommandGroup( DrivePathCommand.createPathInFieldFrame( @@ -113,7 +113,7 @@ class ThreeNoteAndPickupCenterlineSourceAutoPath( WaitCommand(1.0) .andThen(superstructure.groundIntakeCommand()) ), - superstructure.prepManualSpeakerCommand(-2.2.degrees, 4000.rotations.perMinute), + superstructure.prepManualSpeakerCommand(-2.5.degrees, 44000.rotations.perMinute), superstructure.scoreCommand(), ParallelCommandGroup( DrivePathCommand.createPathInFieldFrame( diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath.kt index 70c4bb1e..0410ccad 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath.kt @@ -44,23 +44,22 @@ class ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath( 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(8.32.meters, 0.78.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), FieldWaypoint( Translation2d(6.89.meters, 1.73.meters).translation2d, null, @@ -101,11 +100,6 @@ class ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath( 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 ) ) } @@ -118,39 +112,11 @@ class ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath( 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 - ) - ) - } - ), - superstructure.groundIntakeCommand() - ), - { superstructure.holdingNote } - ), - 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, @@ -215,47 +181,7 @@ class ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath( ) ) .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 - ) - ) - ) - .andThen(superstructure.scoreCommand()), - { superstructure.holdingNote } - ) + ) ) } 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 25a7738b..c6422f21 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteCenterlineFromAmpAutoPath.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteCenterlineFromAmpAutoPath.kt @@ -101,9 +101,9 @@ class ThreeNoteCenterlineFromAmpAutoPath( ((180 + 13.856 + 165).degrees / 2).inRotation2ds ), FieldWaypoint( - Translation2d(8.27.meters, 5.78.meters).translation2d, + Translation2d(8.5.meters, 5.6.meters).translation2d, null, - 160.degrees.inRotation2ds + 150.degrees.inRotation2ds ), ) } @@ -117,9 +117,9 @@ class ThreeNoteCenterlineFromAmpAutoPath( { listOf( FieldWaypoint( - Translation2d(8.27.meters, 5.78.meters).translation2d, + Translation2d(8.5.meters, 5.6.meters).translation2d, null, - 160.degrees.inRotation2ds + 150.degrees.inRotation2ds ), FieldWaypoint( Translation2d((3.9 + 8.27).meters / 2, (6.45 + 7.45).meters / 2) diff --git a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt index 655a17ab..1627d261 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt @@ -28,7 +28,15 @@ object ControlBoard { get() = -driver.leftYAxis val turn: Double - get() = driver.rightXAxis * DrivetrainConstants.TELEOP_TURNING_SPEED_PERCENT + get() { + return if (driver.rightXAxis < 0.85) { + driver.rightXAxis * DrivetrainConstants.TELEOP_TURNING_SPEED_PERCENT + } else { + driver.rightXAxis + } + } + + val slowMode: Boolean get() = driver.leftShoulderButton 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 ca09845c..21fab9c7 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.85 + const val TELEOP_TURNING_SPEED_PERCENT = 0.7 const val OMOMETRY_UPDATE_FREQUENCY = 250.0 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 6709cdb0..296d95e9 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt @@ -718,7 +718,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 f78b9819..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, - 23.42.degrees.inRadians, + 24.02.degrees.inRadians, tag.pitch.degrees.inRadians ) .meters + 4.inches From 68ee786984f2d195c261f5edbe4c7204ff9c481d Mon Sep 17 00:00:00 2001 From: Matthew Choulas Date: Fri, 19 Apr 2024 09:21:54 -0500 Subject: [PATCH 4/7] adjust amp center line without first note --- .../ThreeNoteCenterlineWithoutFirstNoteAmpAutoPath.kt | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteCenterlineWithoutFirstNoteAmpAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteCenterlineWithoutFirstNoteAmpAutoPath.kt index 2bb7ff99..e0008435 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteCenterlineWithoutFirstNoteAmpAutoPath.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteCenterlineWithoutFirstNoteAmpAutoPath.kt @@ -54,11 +54,6 @@ class ThreeNoteCenterlineWithoutFirstNoteAmpAutoPath( 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 ) ) } @@ -71,7 +66,7 @@ class ThreeNoteCenterlineWithoutFirstNoteAmpAutoPath( { listOf( FieldWaypoint( - Translation2d(7.0.meters, 5.7.meters).translation2d, + Translation2d(8.27.meters, 5.78.meters).translation2d, null, 160.degrees.inRotation2ds ), @@ -105,7 +100,7 @@ class ThreeNoteCenterlineWithoutFirstNoteAmpAutoPath( ), FieldWaypoint( Translation2d( - (3.9 + 8.27).meters / 2, (5.7 + 7.45).meters / 2 + (3.9 + 8.27).meters / 2, (5.25 + 7.45).meters / 2 ) .translation2d, null, @@ -119,7 +114,7 @@ class ThreeNoteCenterlineWithoutFirstNoteAmpAutoPath( FieldWaypoint( Translation2d( (3.9 + 8.27).meters / 2, - (5.7 + 7.45).meters / 2 + 0.2.meters + (5.25 + 7.45).meters / 2 + 0.2.meters ) .translation2d, null, From a430254e2a08fe8dccea865a715ad05f87824c0e Mon Sep 17 00:00:00 2001 From: AlphaPranav9102 <49816864+AlphaPranav9102@users.noreply.github.com> Date: Sun, 12 May 2024 22:25:51 -0400 Subject: [PATCH 5/7] fried at worlds but its over? --- .../robot2023/auto/AutonomousSelector.kt | 2 +- ...eeNoteAndPickupCenterlineSourceAutoPath.kt | 4 +- ...enterlineWithoutFirstNoteSourceAutoPath.kt | 83 +++---------------- .../team4099/robot2023/config/ControlBoard.kt | 3 +- .../config/constants/DrivetrainConstants.kt | 2 +- .../config/constants/WristConstants.kt | 6 +- .../robot2023/subsystems/falconspin/Motor.kt | 2 + 7 files changed, 22 insertions(+), 80 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt b/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt index ddec2ecb..f696341c 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt @@ -142,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 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 a21987b0..8d7c54a5 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteAndPickupCenterlineSourceAutoPath.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteAndPickupCenterlineSourceAutoPath.kt @@ -67,7 +67,7 @@ class ThreeNoteAndPickupCenterlineSourceAutoPath( ) } ), - superstructure.prepManualSpeakerCommand(-2.5.degrees, 4400.rotations.perMinute), + superstructure.prepManualSpeakerCommand(-4.5.degrees, 4000.rotations.perMinute), superstructure.scoreCommand(), ParallelCommandGroup( DrivePathCommand.createPathInFieldFrame( @@ -113,7 +113,7 @@ class ThreeNoteAndPickupCenterlineSourceAutoPath( WaitCommand(1.0) .andThen(superstructure.groundIntakeCommand()) ), - superstructure.prepManualSpeakerCommand(-2.5.degrees, 44000.rotations.perMinute), + superstructure.prepManualSpeakerCommand(-4.5.degrees, 4000.rotations.perMinute), superstructure.scoreCommand(), ParallelCommandGroup( DrivePathCommand.createPathInFieldFrame( diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath.kt index 0410ccad..eabcfc2f 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath.kt @@ -40,11 +40,11 @@ class ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath( null, 180.degrees.inRotation2ds ), - FieldWaypoint( - Translation2d(8.32.meters, 0.78.meters).translation2d, - null, - 180.degrees.inRotation2ds - ) + FieldWaypoint( + Translation2d(8.32.meters, 2.44.meters).translation2d, + null, + 220.degrees.inRotation2ds + ) ) } ), @@ -56,9 +56,9 @@ class ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath( { listOf( FieldWaypoint( - Translation2d(8.32.meters, 0.78.meters).translation2d, + Translation2d(8.32.meters, 2.44.meters).translation2d, null, - 180.degrees.inRotation2ds + 220.degrees.inRotation2ds ), FieldWaypoint( Translation2d(6.89.meters, 1.73.meters).translation2d, @@ -73,68 +73,7 @@ class ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath( ) } ), - 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 - ) - ) - } - ), - WaitCommand(1.0).andThen(superstructure.groundIntakeCommand()), - ) - ), - ParallelCommandGroup( - DrivePathCommand.createPathInFieldFrame( - drivetrain, - { - listOf( - FieldWaypoint( - Translation2d(8.32.meters, 2.44.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) + superstructure.prepManualSpeakerCommand(-4.5.degrees, 4000.rotations.perMinute) ) .andThen(superstructure.scoreCommand()) .andThen( @@ -164,9 +103,9 @@ class ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath( 180.degrees.inRotation2ds ), FieldWaypoint( - Translation2d(5.5.meters, 1.85.meters).translation2d, + Translation2d(4.33.meters, 1.67.meters).translation2d, null, - (180 - 35.02902884839783).degrees.inRotation2ds + (180 - 43.37583640633171).degrees.inRotation2ds ) ) } @@ -176,7 +115,7 @@ class ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath( .andThen(WaitCommand(2.0)) .andThen( superstructure.prepManualSpeakerCommand( - -2.0.degrees, 4400.rotations.perMinute + -4.5.degrees, 4000.rotations.perMinute ) ) ) diff --git a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt index 1627d261..8eff7609 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 @@ -29,7 +30,7 @@ object ControlBoard { val turn: Double get() { - return if (driver.rightXAxis < 0.85) { + return if (driver.rightXAxis.absoluteValue < 0.80) { driver.rightXAxis * DrivetrainConstants.TELEOP_TURNING_SPEED_PERCENT } else { driver.rightXAxis 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..97638736 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.5 const val OMOMETRY_UPDATE_FREQUENCY = 250.0 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 1e29ae4c..fe741d10 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,9 @@ object WristConstants { val ABSOLUTE_ENCODER_OFFSET = ( 97.72227856659904.degrees - 35.degrees + 1.90.degrees - - 0.5.degrees - - 1.0.degrees - // add to drop angle - 1.degrees - + 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 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 From 52d87f32aecd0a81829404e0a5de61a6b675633f Mon Sep 17 00:00:00 2001 From: Matthew Choulas Date: Fri, 17 May 2024 18:52:35 -0400 Subject: [PATCH 6/7] tune swerve turning --- .../com/team4099/robot2023/RobotContainer.kt | 35 +++-- .../robot2023/auto/mode/FourNoteAutoPath.kt | 15 +- ...eeNoteAndPickupCenterlineSourceAutoPath.kt | 6 +- ...enterlineWithoutFirstNoteSourceAutoPath.kt | 145 +++++++++--------- .../ThreeNoteCenterlineFromAmpAutoPath.kt | 14 +- .../team4099/robot2023/config/ControlBoard.kt | 4 +- .../config/constants/DrivetrainConstants.kt | 2 +- .../config/constants/WristConstants.kt | 3 +- .../superstructure/Superstructure.kt | 2 - .../team4099/robot2023/util/driver/Jessika.kt | 3 + 10 files changed, 115 insertions(+), 114 deletions(-) create mode 100644 src/main/kotlin/com/team4099/robot2023/util/driver/Jessika.kt diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index 47530ccb..45904c6b 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -4,8 +4,6 @@ import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.auto.AutonomousSelector 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 @@ -37,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 @@ -112,7 +110,7 @@ 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) }, @@ -166,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) }, @@ -182,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) }, @@ -210,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) }, @@ -236,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) }, @@ -248,6 +253,8 @@ object RobotContainer { ) ) + */ + ControlBoard.climbAlignFar.whileTrue( runOnce({ setClimbAngle = @@ -282,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) }, @@ -293,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) }, @@ -309,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) }, @@ -323,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) }, @@ -339,6 +348,8 @@ object RobotContainer { ) ) + */ + // ControlBoard.lockWheels.whileTrue(LockDriveCommand(drivetrain)) // ControlBoard.characterizeSubsystem.whileTrue( 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 46a032e4..a0f21fd5 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPath.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPath.kt @@ -55,8 +55,7 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru } ) .withTimeout(3.235 + 0.5), - WaitCommand(0.5) - .andThen(superstructure.groundIntakeCommand()) + WaitCommand(0.5).andThen(superstructure.groundIntakeCommand()) ), superstructure.prepSpeakerLowCommand(), superstructure @@ -92,8 +91,7 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru } ) .withTimeout(3.235 + 0.5), - WaitCommand(0.5) - .andThen(superstructure.groundIntakeCommand()) + WaitCommand(0.5).andThen(superstructure.groundIntakeCommand()) ), superstructure.prepSpeakerLowCommand(), superstructure @@ -113,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, @@ -129,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, @@ -145,8 +143,7 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru } ) .withTimeout(3.235 + 0.5), - WaitCommand(0.3) - .andThen(superstructure.groundIntakeCommand()) + WaitCommand(0.3).andThen(superstructure.groundIntakeCommand()) ), superstructure.prepSpeakerLowCommand(), superstructure @@ -187,5 +184,3 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru 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 8d7c54a5..35854e93 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteAndPickupCenterlineSourceAutoPath.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteAndPickupCenterlineSourceAutoPath.kt @@ -9,7 +9,6 @@ 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.inches import org.team4099.lib.units.base.meters import org.team4099.lib.units.derived.degrees import org.team4099.lib.units.derived.inRotation2ds @@ -110,8 +109,7 @@ class ThreeNoteAndPickupCenterlineSourceAutoPath( ) } ), - WaitCommand(1.0) - .andThen(superstructure.groundIntakeCommand()) + WaitCommand(1.0).andThen(superstructure.groundIntakeCommand()) ), superstructure.prepManualSpeakerCommand(-4.5.degrees, 4000.rotations.perMinute), superstructure.scoreCommand(), @@ -146,4 +144,4 @@ class ThreeNoteAndPickupCenterlineSourceAutoPath( companion object { val startingPose = Pose2d(0.63.meters, 4.44.meters, 120.degrees) } -} \ No newline at end of file +} diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath.kt index eabcfc2f..79d5f281 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath.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 @@ -40,87 +39,87 @@ class ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath( null, 180.degrees.inRotation2ds ), - FieldWaypoint( - Translation2d(8.32.meters, 2.44.meters).translation2d, - null, - 220.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 - ) - ) - } + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(8.32.meters, 2.44.meters).translation2d, + null, + 220.degrees.inRotation2ds ), - WaitCommand(1.5) - .andThen(superstructure.groundIntakeCommand()) - .andThen(WaitCommand(2.0)) - .andThen( - superstructure.prepManualSpeakerCommand( - -4.5.degrees, 4000.rotations.perMinute + 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 ) ) - ) - .andThen(superstructure.scoreCommand()) + } + ), + WaitCommand(1.5) + .andThen(superstructure.groundIntakeCommand()) + .andThen(WaitCommand(2.0)) + .andThen( + superstructure.prepManualSpeakerCommand( + -4.5.degrees, 4000.rotations.perMinute + ) + ) ) + .andThen(superstructure.scoreCommand()) + ) ) } 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 c6422f21..e1677455 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteCenterlineFromAmpAutoPath.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteCenterlineFromAmpAutoPath.kt @@ -5,7 +5,6 @@ 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.ParallelRaceGroup import edu.wpi.first.wpilibj2.command.SequentialCommandGroup import edu.wpi.first.wpilibj2.command.WaitCommand import org.team4099.lib.geometry.Pose2d @@ -77,9 +76,7 @@ class ThreeNoteCenterlineFromAmpAutoPath( WaitCommand(1.0) .andThen( superstructure - .prepManualSpeakerCommand( - -7.degrees, 4000.rotations.perMinute - ) + .prepManualSpeakerCommand(-7.degrees, 4000.rotations.perMinute) .withTimeout(1.0) ) ), @@ -108,8 +105,7 @@ class ThreeNoteCenterlineFromAmpAutoPath( ) } ), - WaitCommand( - 1.0).andThen(superstructure.groundIntakeCommand()) + WaitCommand(1.0).andThen(superstructure.groundIntakeCommand()) ), ParallelCommandGroup( DrivePathCommand.createPathInFieldFrame( @@ -193,7 +189,9 @@ class ThreeNoteCenterlineFromAmpAutoPath( ), WaitCommand(1.0) .andThen( - superstructure.prepManualSpeakerCommand(-5.5.degrees, 4000.rotations.perMinute) + superstructure.prepManualSpeakerCommand( + -5.5.degrees, 4000.rotations.perMinute + ) ) ), superstructure.scoreCommand() @@ -203,4 +201,4 @@ class ThreeNoteCenterlineFromAmpAutoPath( companion object { val startingPose = Pose2d(0.75.meters, 6.70.meters, 240.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 8eff7609..a77b9930 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt @@ -30,15 +30,13 @@ object ControlBoard { val turn: Double get() { - return if (driver.rightXAxis.absoluteValue < 0.80) { + return if (driver.rightXAxis.absoluteValue < 0.90) { driver.rightXAxis * DrivetrainConstants.TELEOP_TURNING_SPEED_PERCENT } else { driver.rightXAxis } } - - val slowMode: Boolean get() = driver.leftShoulderButton 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 97638736..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.5 + 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/WristConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt index fe741d10..cb60870f 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt @@ -39,7 +39,8 @@ object WristConstants { 97.72227856659904.degrees - 35.degrees + 1.90.degrees - 0.55.degrees - 0.6.degrees - // add to drop angle - 1.degrees - 0.5.degrees - + 1.degrees - + 0.5.degrees - 96.3.degrees ) * ABSOLUTE_ENCODER_TO_MECHANISM_GEAR_RATIO val WRIST_LENGTH = 18.6.inches 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 296d95e9..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, @@ -490,7 +489,6 @@ class Superstructure( flywheel.currentRequest = Request.FlywheelRequest.TargetingVelocity(targetFlywheelSpeed) wrist.currentRequest = Request.WristRequest.TargetingPosition(targetWristAngle) - when (currentRequest) { is Request.SuperstructureRequest.Idle -> { nextState = SuperstructureStates.IDLE 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) From a07407fa87ca29e60b8c3aa9bcb71266b4886971 Mon Sep 17 00:00:00 2001 From: 00magikarp <94652654+00magikarp@users.noreply.github.com> Date: Wed, 19 Jun 2024 11:56:57 -0400 Subject: [PATCH 7/7] Custom logger (#49) * add recordOutput method to DebugLogger * rename to CustomLogger * fix photonlib.json (now builds) * add processInputs as a method under CustomLogger --- .../kotlin/com/team4099/robot2023/Robot.kt | 4 +- .../commands/DriveModuleSteeringCommand.kt | 4 +- .../robot2023/commands/SysIdCommand.kt | 4 +- .../WheelRadiusCharacterizationCommand.kt | 15 ++-- .../commands/drivetrain/DrivePathCommand.kt | 6 +- .../drivetrain/ResetGyroYawCommand.kt | 4 +- .../commands/drivetrain/ResetPoseCommand.kt | 8 +- .../commands/drivetrain/ResetZeroCommand.kt | 4 +- .../commands/drivetrain/TargetAngleCommand.kt | 10 +-- .../commands/drivetrain/TargetNoteCommand.kt | 6 +- .../commands/drivetrain/TeleopDriveCommand.kt | 6 +- .../commands/drivetrain/TestDriveCommand.kt | 4 +- .../commands/drivetrain/ZeroSensorsCommand.kt | 4 +- .../subsystems/falconspin/MotorChecker.kt | 38 ++++----- .../robot2023/subsystems/feeder/Feeder.kt | 4 +- .../subsystems/flywheel/FlywheelIOTalon.kt | 6 +- .../robot2023/subsystems/intake/Intake.kt | 4 +- .../team4099/robot2023/subsystems/led/Leds.kt | 4 +- .../robot2023/subsystems/wrist/Wrist.kt | 4 +- .../util/{DebugLogger.kt => CustomLogger.kt} | 83 ++++++++++++++++++- .../team4099/robot2023/util/NoteSimulation.kt | 12 +-- vendordeps/photonlib.json | 10 +-- 22 files changed, 162 insertions(+), 82 deletions(-) rename src/main/kotlin/com/team4099/robot2023/util/{DebugLogger.kt => CustomLogger.kt} (61%) diff --git a/src/main/kotlin/com/team4099/robot2023/Robot.kt b/src/main/kotlin/com/team4099/robot2023/Robot.kt index e2db1832..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 ) 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 6a3a9673..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 @@ -120,8 +120,8 @@ class TargetNoteCommand( 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/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/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 9ea656ea..eaf5f4cb 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt @@ -2,7 +2,7 @@ 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 @@ -71,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/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/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