diff --git a/src/main/kotlin/com/team4099/lib/trajectory/CustomHolonomicDriveController.kt b/src/main/kotlin/com/team4099/lib/trajectory/CustomHolonomicDriveController.kt index c56a39c1..6f7f8689 100644 --- a/src/main/kotlin/com/team4099/lib/trajectory/CustomHolonomicDriveController.kt +++ b/src/main/kotlin/com/team4099/lib/trajectory/CustomHolonomicDriveController.kt @@ -1,7 +1,6 @@ package com.team4099.lib.trajectory import edu.wpi.first.math.controller.PIDController -import edu.wpi.first.math.controller.ProfiledPIDController import edu.wpi.first.math.geometry.Pose2d import edu.wpi.first.math.geometry.Rotation2d import edu.wpi.first.math.kinematics.ChassisSpeeds diff --git a/src/main/kotlin/com/team4099/robot2023/Robot.kt b/src/main/kotlin/com/team4099/robot2023/Robot.kt index d13cf4bf..ea1281cd 100644 --- a/src/main/kotlin/com/team4099/robot2023/Robot.kt +++ b/src/main/kotlin/com/team4099/robot2023/Robot.kt @@ -3,7 +3,6 @@ package com.team4099.robot2023 import com.team4099.lib.hal.Clock import com.team4099.robot2023.auto.AutonomousSelector import com.team4099.robot2023.auto.PathStore -import com.team4099.robot2023.commands.drivetrain.ResetGyroYawCommand import com.team4099.robot2023.config.ControlBoard import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.subsystems.falconspin.MotorChecker @@ -24,7 +23,6 @@ import edu.wpi.first.wpilibj.simulation.DriverStationSim import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.CommandScheduler import edu.wpi.first.wpilibj2.command.Commands.runOnce -import edu.wpi.first.wpilibj2.command.WaitCommand import org.ejml.EjmlVersion.BUILD_DATE import org.ejml.EjmlVersion.DIRTY import org.ejml.EjmlVersion.GIT_BRANCH @@ -130,15 +128,15 @@ object Robot : LoggedRobot() { // Set the scheduler to log events for command initialize, interrupt, finish CommandScheduler.getInstance().onCommandInitialize { command: Command -> - DebugLogger.recordDebugOutput("/ActiveCommands/${command.name}", true) + Logger.recordOutput("/ActiveCommands/${command.name}", true) } CommandScheduler.getInstance().onCommandFinish { command: Command -> - DebugLogger.recordDebugOutput("/ActiveCommands/${command.name}", false) + Logger.recordOutput("/ActiveCommands/${command.name}", false) } CommandScheduler.getInstance().onCommandInterrupt { command: Command -> - DebugLogger.recordDebugOutput("/ActiveCommands/${command.name}", false) + Logger.recordOutput("/ActiveCommands/${command.name}", false) } val autoTab = Shuffleboard.getTab("Pre-match") @@ -153,8 +151,8 @@ object Robot : LoggedRobot() { override fun autonomousInit() { RobotContainer.setSteeringCoastMode() - val autonCommandWithWait = runOnce({ RobotContainer.zeroSensors(isInAutonomous = true)}) - .andThen(autonomousCommand) + val autonCommandWithWait = + runOnce({ RobotContainer.zeroSensors(isInAutonomous = true) }).andThen(autonomousCommand) autonCommandWithWait?.schedule() } diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index 742a418f..522ad6ac 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -2,6 +2,7 @@ package com.team4099.robot2023 import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.auto.AutonomousSelector +import com.team4099.robot2023.commands.drivetrain.LockDrivetrainCommand import com.team4099.robot2023.commands.drivetrain.ResetGyroYawCommand import com.team4099.robot2023.commands.drivetrain.TargetAngleCommand import com.team4099.robot2023.commands.drivetrain.TargetNoteCommand @@ -15,7 +16,7 @@ import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOSim import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIO import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIOPigeon2 import com.team4099.robot2023.subsystems.elevator.Elevator -import com.team4099.robot2023.subsystems.elevator.ElevatorIO +import com.team4099.robot2023.subsystems.elevator.ElevatorIONEO import com.team4099.robot2023.subsystems.elevator.ElevatorIOSim import com.team4099.robot2023.subsystems.feeder.Feeder import com.team4099.robot2023.subsystems.feeder.FeederIONeo @@ -48,8 +49,6 @@ import org.team4099.lib.units.derived.Angle import org.team4099.lib.units.derived.degrees import org.team4099.lib.units.derived.inDegrees import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest -import com.team4099.robot2023.subsystems.elevator.ElevatorIONEO -import com.team4099.robot2023.subsystems.intake.IntakeIO object RobotContainer { private val drivetrain: Drivetrain @@ -171,42 +170,46 @@ object RobotContainer { limelight.limelightState = LimelightVision.LimelightStates.TARGETING_GAME_PIECE ControlBoard.resetGyro.whileTrue(ResetGyroYawCommand(drivetrain)) - ControlBoard.intake.whileTrue(ParallelCommandGroup( + ControlBoard.intake.whileTrue( + ParallelCommandGroup( superstructure.groundIntakeCommand(), - TargetNoteCommand( + TargetNoteCommand( + driver = Ryan(), + { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, + { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, + { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) }, + { ControlBoard.slowMode }, + drivetrain, + limelight, + feeder + ) + ) + ) + + ControlBoard.prepAmp.whileTrue(superstructure.prepAmpCommand()) + + ControlBoard.passingShotAlignment.whileTrue( + TargetAngleCommand( driver = Ryan(), { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) }, { ControlBoard.slowMode }, - drivetrain, limelight, feeder) - ) + drivetrain, + { + if (DriverStation.getAlliance().isPresent && + DriverStation.getAlliance().get() == DriverStation.Alliance.Red + ) + 225.degrees + 180.degrees + else if (DriverStation.getAlliance().isPresent && + DriverStation.getAlliance().get() == DriverStation.Alliance.Blue + ) + 135.degrees + else 135.degrees + }, + ) ) - ControlBoard.prepAmp.whileTrue(superstructure.prepAmpCommand()) - - ControlBoard.passingShotAlignment.whileTrue(TargetAngleCommand( - driver = Ryan(), - { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, - { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, - { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) }, - { ControlBoard.slowMode }, - drivetrain, - { - if (DriverStation.getAlliance().isPresent && - DriverStation.getAlliance().get() == DriverStation.Alliance.Red - ) - 225.degrees + 180.degrees - else if (DriverStation.getAlliance().isPresent && - DriverStation.getAlliance().get() == DriverStation.Alliance.Blue - ) - 135.degrees - else - 135.degrees - }, - )) - - ControlBoard.prepHighProtected.whileTrue( ParallelCommandGroup( superstructure.prepSpeakerMidCommand(), @@ -237,17 +240,16 @@ object RobotContainer { ControlBoard.passingShot.whileTrue(superstructure.passingShotCommand()) ControlBoard.underStagePassingShot.whileTrue(superstructure.underStageCommand()) - ControlBoard.targetAmp.whileTrue( - TargetAngleCommand( - driver = Ryan(), - { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, - { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, - { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) }, - { ControlBoard.slowMode }, - drivetrain, - ampAngle - ) + TargetAngleCommand( + driver = Ryan(), + { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, + { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, + { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) }, + { ControlBoard.slowMode }, + drivetrain, + ampAngle + ) ) ControlBoard.climbAlignFar.whileTrue( @@ -341,6 +343,8 @@ object RobotContainer { ) ) + ControlBoard.lockWheels.whileTrue(LockDrivetrainCommand(drivetrain)) + // ControlBoard.characterizeSubsystem.whileTrue( // WheelRadiusCharacterizationCommand( // drivetrain, WheelRadiusCharacterizationCommand.Companion.Direction.CLOCKWISE @@ -377,7 +381,7 @@ object RobotContainer { fun getAutonomousLoadingCommand() = AutonomousSelector.getLoadingCommand(drivetrain) - fun resetGyroYawCommand(angle: Angle) : Command = ResetGyroYawCommand(drivetrain, angle) + fun resetGyroYawCommand(angle: Angle): Command = ResetGyroYawCommand(drivetrain, angle) fun mapTunableCommands() {} } diff --git a/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt b/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt index bb480e9a..5c11ff5c 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt @@ -46,6 +46,8 @@ object AutonomousSelector { autonomousModeChooser.addOption("Four Note Wing Auto", AutonomousMode.FOUR_NOTE_AUTO_PATH) + /* + autonomousModeChooser.addOption( "Four Note Right Auto(1 Wing + 2 Centerline)", AutonomousMode.FOUR_NOTE_RIGHT_AUTO_PATH ) @@ -55,9 +57,14 @@ object AutonomousSelector { autonomousModeChooser.addOption( "Four Note LEFT Auto(1 Wing + 2 Centerline)", AutonomousMode.FOUR_NOTE_LEFT_AUTO_PATH ) + + */ + autonomousModeChooser.addOption( "Five Note Auto from Center Subwoofer", AutonomousMode.FIVE_NOTE_AUTO_PATH ) + + /* autonomousModeChooser.addOption( "Two Note Centerline Auto from Source Side of Subwoofer", AutonomousMode.TWO_NOTE_CENTERLINE_FROM_SOURCE @@ -67,8 +74,11 @@ object AutonomousSelector { "Two Note Centerline Auto from Amp Side of Subwoofer", AutonomousMode.TWO_NOTE_CENTERLINE_FROM_AMP ) + + */ + autonomousModeChooser.addOption( - "Three Note Centerline Auto from Amp Side of Subwoofer", + "Three Note + Pickup Centerline Auto from Amp Side of Subwoofer", AutonomousMode.THREE_NOTE_CENTERLINE_FROM_AMP ) autonomousModeChooser.addOption( @@ -88,10 +98,14 @@ object AutonomousSelector { AutonomousMode.PRELOAD_AND_LEAVE_CENTER_SUBWOOFER ) + /* + autonomousModeChooser.addOption("Six Note Path", AutonomousMode.SIX_NOTE_AUTO_PATH) autonomousModeChooser.addOption( "Six Note Path with Pickup", AutonomousMode.SIX_NOTE_WITH_PICKUP_PATH ) + + */ autonomousModeChooser.addOption("Test Auto Path", AutonomousMode.TEST_AUTO_PATH) // autonomousModeChooser.addOption("Characterize Elevator", // AutonomousMode.ELEVATOR_CHARACTERIZE) diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/FiveNoteAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/FiveNoteAutoPath.kt index 492879e4..47dc8d7e 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/mode/FiveNoteAutoPath.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/FiveNoteAutoPath.kt @@ -114,7 +114,8 @@ class FiveNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru ) } ), - superstructure.groundIntakeCommand() + superstructure + .groundIntakeCommand() .andThen( superstructure.prepManualSpeakerCommand( -3.degrees, 3000.rotations.perMinute, 0.7.degrees 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 0bd6da21..f52427eb 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(0.5), + WaitCommand(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds), ParallelCommandGroup( DrivePathCommand.createPathInFieldFrame( drivetrain, @@ -56,8 +56,9 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru .withTimeout(3.235 + 0.5), WaitCommand(0.5) .andThen(superstructure.groundIntakeCommand()) + .andThen(WaitCommand(1.75)) + .andThen(superstructure.prepSpeakerLowCommand()) ), - superstructure.prepSpeakerLowCommand(), superstructure .scoreCommand() .withTimeout(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds + 0.5), @@ -93,8 +94,9 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru .withTimeout(3.235 + 0.5), WaitCommand(0.5) .andThen(superstructure.groundIntakeCommand()) + .andThen(WaitCommand(1.75)) + .andThen(superstructure.prepSpeakerLowCommand()) ), - superstructure.prepSpeakerLowCommand(), superstructure .scoreCommand() .withTimeout(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds + 0.5), @@ -146,11 +148,41 @@ 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) + .withTimeout(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds + 0.5), + WaitCommand(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + startingPose.translation.translation2d, null, 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(4.45.meters, 4.89.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(5.92.meters, 4.0.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(8.29.meters, 4.09.meters).translation2d, + null, + 180.degrees.inRotation2ds + ) + ) + } + ), + WaitCommand(1.0).andThen(superstructure.groundIntakeCommand()) + ) ) } 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..b3c255b9 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,101 +44,217 @@ 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()) ), - 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 - ), - 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 + 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 + ) + ) + } ), - FieldWaypoint( - Translation2d( - (4.33.meters + 8.32.meters) / 2, - (2.44 + 0.78).meters / 2 + 0.1.meters + 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 ) - .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() ), - WaitCommand(1.0) - .andThen(superstructure.groundIntakeCommand()) + { superstructure.holdingNote } ), - 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 - ), - FieldWaypoint( - Translation2d(4.84.meters, 4.09.meters).translation2d, - null, - 180.degrees.inRotation2ds + 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 + ) + ) + } ), - FieldWaypoint( - Translation2d(8.32.meters, 4.12.meters).translation2d, - null, - 180.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 ) ) - } - ), - WaitCommand(1.5).andThen(superstructure.groundIntakeCommand()) + ) + .andThen(superstructure.scoreCommand()), + { superstructure.holdingNote } ) ) } @@ -146,4 +262,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/ThreeNoteCenterlineFromAmpAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteCenterlineFromAmpAutoPath.kt index 25a7738b..cc1b75c0 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,8 +19,9 @@ import org.team4099.lib.units.perMinute class ThreeNoteCenterlineFromAmpAutoPath( val drivetrain: Drivetrain, - val superstructure: Superstructure + val superstructure: Superstructure, ) : SequentialCommandGroup() { + init { addRequirements(drivetrain) addCommands( @@ -46,161 +47,230 @@ class ThreeNoteCenterlineFromAmpAutoPath( null, 180.degrees.inRotation2ds ), - FieldWaypoint( - Translation2d(8.27.meters + 2.feet, 7.45.meters).translation2d, - null, - 180.degrees.inRotation2ds - ) - ) - } - ), - WaitCommand(2.25).andThen(superstructure.groundIntakeCommand()) - ), - 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, + Translation2d(7.0.meters, 6.65.meters).translation2d, null, - (180 + 13.856).degrees.inRotation2ds - ), - ) - } - ), - WaitCommand(1.0) - .andThen( - superstructure - .prepManualSpeakerCommand( - -7.degrees, 4000.rotations.perMinute + 180.degrees.inRotation2ds ) - .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()) + WaitCommand(2.25).andThen(superstructure.groundIntakeCommand()) ), - ParallelCommandGroup( - DrivePathCommand.createPathInFieldFrame( - drivetrain, - { - listOf( - FieldWaypoint( - Translation2d(8.27.meters, 5.78.meters).translation2d, - null, - 160.degrees.inRotation2ds - ), - FieldWaypoint( - Translation2d((3.9 + 8.27).meters / 2, (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 - ), + 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) ) - } - ), - WaitCommand(1.0) + ) + .andThen(superstructure.scoreCommand().withTimeout(0.5)) .andThen( - 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 + 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 + ) + ) + } ), + 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() ), - WaitCommand(1.0).andThen(superstructure.groundIntakeCommand()) + { superstructure.holdingNote } ), - 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 + 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)) + .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 + ) + ) + } ), + WaitCommand(1.0) + .andThen(superstructure.groundIntakeCommand()) + .andThen(WaitCommand(2.0)) + .andThen( + superstructure.prepManualSpeakerCommand( + -2.0.degrees, 4400.rotations.perMinute + ) + ) ) - } - ), - WaitCommand(1.0) - .andThen( - superstructure.prepManualSpeakerCommand(-5.5.degrees, 4000.rotations.perMinute) - ) - ), - superstructure.scoreCommand() + .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 + ) + ) + ) + .andThen(superstructure.scoreCommand()), + { superstructure.holdingNote } + ) ) } 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/commands/drivetrain/DrivePathCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt index a192a163..7549882c 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt @@ -24,8 +24,6 @@ import edu.wpi.first.wpilibj.RobotBase import edu.wpi.first.wpilibj2.command.Command import org.littletonrobotics.junction.Logger import org.team4099.lib.controller.PIDController -import org.team4099.lib.controller.ProfiledPIDController -import org.team4099.lib.controller.TrapezoidProfile import org.team4099.lib.geometry.Pose2d import org.team4099.lib.geometry.Pose2dWPILIB import org.team4099.lib.geometry.Transform2d @@ -194,12 +192,7 @@ private constructor( xPID = PIDController(poskP.get(), poskI.get(), poskD.get()) yPID = PIDController(poskP.get(), poskI.get(), poskD.get()) - thetaPID = - PIDController( - thetakP.get(), - thetakI.get(), - thetakD.get() - ) + thetaPID = PIDController(thetakP.get(), thetakI.get(), thetakD.get()) thetaPID.enableContinuousInput(-PI.radians, PI.radians) diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/LockDriveCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/LockDriveCommand.kt new file mode 100644 index 00000000..03a04ff5 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/LockDriveCommand.kt @@ -0,0 +1,17 @@ +package com.team4099.robot2023.commands.drivetrain + +import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.subsystems.superstructure.Request +import edu.wpi.first.wpilibj2.command.Command + +class LockDriveCommand(val drivetrain: Drivetrain) : Command() { + + override fun execute() { + drivetrain.defaultCommand.end(true) + drivetrain.currentRequest = Request.DrivetrainRequest.LockWheels() + } + + override fun isFinished(): Boolean { + return false + } +} 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 33c46017..b83fcdd6 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetNoteCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetNoteCommand.kt @@ -1,7 +1,6 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.lib.logging.LoggedTunableValue -import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import com.team4099.robot2023.subsystems.feeder.Feeder @@ -125,20 +124,18 @@ class TargetNoteCommand( if (!feeder.hasNote && limelight.inputs.gamePieceTargets.size > 0) { val driveVector = driver.driveSpeedClampedSupplier(driveX, driveY, slowMode) - var autoDriveVector = hypot(driveVector.first.inMetersPerSecond, driveVector.second.inMetersPerSecond) + var autoDriveVector = + hypot(driveVector.first.inMetersPerSecond, driveVector.second.inMetersPerSecond) if (DriverStation.getAlliance().isPresent && - DriverStation.getAlliance().get() == DriverStation.Alliance.Blue) { - autoDriveVector =-hypot(driveVector.first.inMetersPerSecond, driveVector.second.inMetersPerSecond) + DriverStation.getAlliance().get() == DriverStation.Alliance.Blue + ) { + autoDriveVector = + -hypot(driveVector.first.inMetersPerSecond, driveVector.second.inMetersPerSecond) } drivetrain.currentRequest = Request.DrivetrainRequest.OpenLoop( thetaFeedback, - Pair( - autoDriveVector - .meters - .perSecond, - 0.0.meters.perSecond - ), + Pair(autoDriveVector.meters.perSecond, 0.0.meters.perSecond), fieldOriented = false ) } else { @@ -147,7 +144,6 @@ class TargetNoteCommand( drivetrain.currentRequest = Request.DrivetrainRequest.OpenLoop(rotation, speed) } - } override fun isFinished(): Boolean { 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 a6e60a0e..31b9d0da 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetSpeakerCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetSpeakerCommand.kt @@ -3,7 +3,6 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.lib.logging.LoggedTunableNumber import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.config.constants.DrivetrainConstants -import com.team4099.robot2023.config.constants.FieldConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import com.team4099.robot2023.subsystems.superstructure.Request import com.team4099.robot2023.subsystems.superstructure.Superstructure @@ -135,7 +134,6 @@ class TargetSpeakerCommand( val odomTSpeaker = drivetrain.odomTSpeaker val robotTSpeaker = odomTRobot.inverse().transformBy(odomTSpeaker) - desiredAngle = atan2( robotTSpeaker.y.inMeters - @@ -144,7 +142,8 @@ class TargetSpeakerCommand( robotTSpeaker.translation.magnitude.absoluteValue / 7 ) .value, - robotTSpeaker.x.inMeters - 10.inches.inMeters - + robotTSpeaker.x.inMeters - + 10.inches.inMeters - ( drivetrain.robotVelocity.x * robotTSpeaker.translation.magnitude.absoluteValue / 7 diff --git a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt index 152419d4..89c0b871 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt @@ -55,7 +55,7 @@ object ControlBoard { val prepHigh = Trigger { operator.yButton } val passingShot = Trigger { operator.leftShoulderButton } val passingShotAlignment = Trigger { driver.yButton } - val underStagePassingShot = Trigger { operator.rightShoulderButton} + val underStagePassingShot = Trigger { operator.rightShoulderButton } val extendClimb = Trigger { operator.dPadUp } val retractClimb = Trigger { operator.dPadDown } @@ -73,5 +73,7 @@ object ControlBoard { val targetSpeaker = Trigger { driver.xButton } // TODO: switch back to climbAlignLeft val climbAutoAlign = Trigger { driver.bButton } + val lockWheels = Trigger { driver.selectButton } + // week0 controls } 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 7eec930c..21fab9c7 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt @@ -86,8 +86,8 @@ object DrivetrainConstants { val FRONT_LEFT_MODULE_ZERO = 3.285.radians + PI.radians // good val FRONT_RIGHT_MODULE_ZERO = 2.91.radians + PI.radians // good - val BACK_LEFT_MODULE_ZERO = 5.665.radians + PI.radians// good - val BACK_RIGHT_MODULE_ZERO = 3.50.radians + PI.radians// good + val BACK_LEFT_MODULE_ZERO = 5.665.radians + PI.radians // good + val BACK_RIGHT_MODULE_ZERO = 3.50.radians + PI.radians // good val STEERING_COMPENSATION_VOLTAGE = 10.volts val DRIVE_COMPENSATION_VOLTAGE = 12.volts 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 9cc1d348..65a05646 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt @@ -13,7 +13,7 @@ object IntakeConstants { val CENTER_WHEEL_INERTIA = 0.002459315 val VOLTAGE_COMPENSATION = 12.0.volts - val SIM_INTAKE_DISTANCE = 8.inches + val SIM_INTAKE_DISTANCE = 16.inches val INTAKE_TRANSFORM = Transform2d(Translation2d(-18.0.inches, 0.0.inches), 0.0.degrees) 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 201189fb..c17767dd 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/LEDConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/LEDConstants.kt @@ -1,7 +1,6 @@ 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 diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/SuperstructureConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/SuperstructureConstants.kt index 7a0eaeae..9cce5850 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/SuperstructureConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/SuperstructureConstants.kt @@ -51,7 +51,6 @@ object SuperstructureConstants { Pair(220.1.inches, -2.degrees) ) - val highDistanceFlywheelSpeedTableReal = listOf( Pair(71.2.inches, 2500.rotations.perMinute), 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 9a9a0b05..8f4d6f13 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.5.degrees - 1.1.degrees - // add to drop angle - 1.degrees - 96.3.degrees + 1.degrees - + 96.3.degrees ) * ABSOLUTE_ENCODER_TO_MECHANISM_GEAR_RATIO val WRIST_LENGTH = 18.6.inches val WRIST_INERTIA = 0.7181257183.kilo.grams * 1.0.meters.squared diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/Flywheel.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/Flywheel.kt index 70da9afe..d8c4f3f6 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/Flywheel.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/Flywheel.kt @@ -5,7 +5,6 @@ import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.FlywheelConstants import com.team4099.robot2023.subsystems.superstructure.Request -import com.team4099.robot2023.util.DebugLogger import edu.wpi.first.wpilibj.RobotBase import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.SubsystemBase 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 cbd3f128..7c9101b9 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIOTalon.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIOTalon.kt @@ -28,7 +28,6 @@ import org.team4099.lib.units.base.celsius import org.team4099.lib.units.base.inAmperes import org.team4099.lib.units.base.inSeconds import org.team4099.lib.units.ctreAngularMechanismSensor -import org.team4099.lib.units.derived.AccelerationFeedforward import org.team4099.lib.units.derived.DerivativeGain import org.team4099.lib.units.derived.ElectricalPotential import org.team4099.lib.units.derived.IntegralGain @@ -105,16 +104,16 @@ object FlywheelIOTalon : FlywheelIO { // // flywheelSensor.velocityFeedforwardToRawUnits(FlywheelConstantsConstants.PID.flywheel_KFF) - flywheelLeftConfiguration.CurrentLimits.SupplyCurrentLimit = - FlywheelConstants.LEFT_FLYWHEEL_SUPPLY_CURRENT_LIMIT.inAmperes - flywheelLeftConfiguration.CurrentLimits.SupplyCurrentThreshold = - FlywheelConstants.LEFT_FLYWHEEL_THRESHOLD_CURRENT_LIMIT.inAmperes - flywheelLeftConfiguration.CurrentLimits.SupplyTimeThreshold = - FlywheelConstants.LEFT_flywheel_TRIGGER_THRESHOLD_TIME.inSeconds - flywheelLeftConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true - flywheelLeftConfiguration.CurrentLimits.StatorCurrentLimit = - FlywheelConstants.LEFT_FLYWHEEL_STATOR_CURRENT_LIMIT.inAmperes - flywheelLeftConfiguration.CurrentLimits.StatorCurrentLimitEnable = false + flywheelLeftConfiguration.CurrentLimits.SupplyCurrentLimit = + FlywheelConstants.LEFT_FLYWHEEL_SUPPLY_CURRENT_LIMIT.inAmperes + flywheelLeftConfiguration.CurrentLimits.SupplyCurrentThreshold = + FlywheelConstants.LEFT_FLYWHEEL_THRESHOLD_CURRENT_LIMIT.inAmperes + flywheelLeftConfiguration.CurrentLimits.SupplyTimeThreshold = + FlywheelConstants.LEFT_flywheel_TRIGGER_THRESHOLD_TIME.inSeconds + flywheelLeftConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true + flywheelLeftConfiguration.CurrentLimits.StatorCurrentLimit = + FlywheelConstants.LEFT_FLYWHEEL_STATOR_CURRENT_LIMIT.inAmperes + flywheelLeftConfiguration.CurrentLimits.StatorCurrentLimitEnable = false flywheelLeftConfiguration.MotorOutput.Inverted = InvertedValue.Clockwise_Positive @@ -220,16 +219,12 @@ object FlywheelIOTalon : FlywheelIO { // ) // ) - var acceleration = flywheelLeftSensor.accelerationToRawUnits(20000.rotations.perSecond.perSecond) + var acceleration = + flywheelLeftSensor.accelerationToRawUnits(20000.rotations.perSecond.perSecond) - if (velocity.absoluteValue < 50.rotations.perMinute){ - flywheelLeftTalon.setControl( - VoltageOut( - 0.0.volts.inVolts - ) - ) - } - else { + if (velocity.absoluteValue < 50.rotations.perMinute) { + flywheelLeftTalon.setControl(VoltageOut(0.0.volts.inVolts)) + } else { flywheelLeftTalon.setControl( MotionMagicVelocityVoltage( flywheelLeftSensor.velocityToRawUnits(velocity), diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt index 30881f9e..04453836 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt @@ -5,7 +5,6 @@ import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.LEDConstants import org.littletonrobotics.junction.Logger import org.team4099.lib.units.derived.ElectricalPotential -import org.team4099.lib.units.derived.inVolts import org.team4099.lib.units.derived.volts import kotlin.math.absoluteValue 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 57681f9e..4a5d674b 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt @@ -29,11 +29,12 @@ class Leds(val io: LedIO) { if (DriverStation.getAlliance().isEmpty) { io.batteryVoltage = RobotController.getBatteryVoltage().volts - state = if (io.batteryVoltage < 12.3.volts) { - LEDConstants.CandleState.LOW_BATTERY_WARNING - } else { - LEDConstants.CandleState.GOLD - } + state = + if (io.batteryVoltage < 12.3.volts) { + LEDConstants.CandleState.LOW_BATTERY_WARNING + } else { + LEDConstants.CandleState.GOLD + } } else if (DriverStation.isDisabled() && DriverStation.getAlliance().isPresent) { if (FMSData.isBlue) { state = LEDConstants.CandleState.BLUE diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/AutoAim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/AutoAim.kt index 712f2c64..1ca6483c 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/AutoAim.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/AutoAim.kt @@ -1,13 +1,10 @@ package com.team4099.robot2023.subsystems.superstructure import com.team4099.lib.logging.LoggedTunableValue -import com.team4099.robot2023.config.constants.FieldConstants import com.team4099.robot2023.config.constants.SuperstructureConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import com.team4099.robot2023.subsystems.vision.Vision -import com.team4099.robot2023.util.AllianceFlipUtil import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap -import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj.RobotBase import org.littletonrobotics.junction.Logger import org.team4099.lib.geometry.Translation2d @@ -27,14 +24,15 @@ import org.team4099.lib.units.derived.rotations import org.team4099.lib.units.inRotationsPerMinute import org.team4099.lib.units.perMinute import kotlin.math.absoluteValue -import kotlin.math.hypot class AutoAim(val drivetrain: Drivetrain, val vision: Vision) { val flywheelSpeedRPMInterpolationTable: InterpolatingDoubleTreeMap = InterpolatingDoubleTreeMap() val wristAngleDegreesInterpolationTable: InterpolatingDoubleTreeMap = InterpolatingDoubleTreeMap() - val highFlywheelSpeedRPMInterpolationTable: InterpolatingDoubleTreeMap = InterpolatingDoubleTreeMap() - val highWristAngleDegreesInterpolationTable: InterpolatingDoubleTreeMap = InterpolatingDoubleTreeMap() + val highFlywheelSpeedRPMInterpolationTable: InterpolatingDoubleTreeMap = + InterpolatingDoubleTreeMap() + val highWristAngleDegreesInterpolationTable: InterpolatingDoubleTreeMap = + InterpolatingDoubleTreeMap() val tunableFlywheelInterpolationTable: List, LoggedTunableValue>>> @@ -43,11 +41,10 @@ class AutoAim(val drivetrain: Drivetrain, val vision: Vision) { List, LoggedTunableValue>> val tunableHighFlywheelInterpolationTable: - List, LoggedTunableValue>>> + List, LoggedTunableValue>>> val tunableHighWristInterpolationTable: - List, LoggedTunableValue>> - + List, LoggedTunableValue>> val interpolationTestDistance = LoggedTunableValue("AutoAim/TestDistance", 0.0.meters, Pair({ it.inInches }, { it.inches })) @@ -86,9 +83,6 @@ class AutoAim(val drivetrain: Drivetrain, val vision: Vision) { ) ) } - - - } else { tunableFlywheelInterpolationTable = SuperstructureConstants.distanceFlywheelSpeedTableSim.mapIndexed { i, it -> @@ -121,9 +115,6 @@ class AutoAim(val drivetrain: Drivetrain, val vision: Vision) { ) ) } - - - } tunableHighFlywheelInterpolationTable = @@ -208,10 +199,10 @@ class AutoAim(val drivetrain: Drivetrain, val vision: Vision) { } fun calculateHighWristAngle(): Angle { - return highWristAngleDegreesInterpolationTable.get(calculateDistanceFromSpeaker().inMeters).degrees + return highWristAngleDegreesInterpolationTable.get(calculateDistanceFromSpeaker().inMeters) + .degrees } - fun updateFlywheelInterpolationTable() { flywheelSpeedRPMInterpolationTable.clear() tunableFlywheelInterpolationTable.forEach { @@ -240,24 +231,26 @@ class AutoAim(val drivetrain: Drivetrain, val vision: Vision) { fun updateHighWristInterpolationTable() { highWristAngleDegreesInterpolationTable.clear() tunableHighWristInterpolationTable.forEach { - highWristAngleDegreesInterpolationTable.put(it.first.get().inMeters, it.second.get().inDegrees) + highWristAngleDegreesInterpolationTable.put( + it.first.get().inMeters, it.second.get().inDegrees + ) } } fun calculateDistanceFromSpeaker(): Length { val distance = - Translation2d( - vision.robotTSpeaker.y - - (drivetrain.robotVelocity.y * vision.robotTSpeaker.norm.absoluteValue / 7) - .value - .meters, - vision.robotTSpeaker.x - - (drivetrain.robotVelocity.x * vision.robotTSpeaker.norm.absoluteValue / 7) - .value - .meters - ) - .magnitude - .meters + Translation2d( + vision.robotTSpeaker.y - + (drivetrain.robotVelocity.y * vision.robotTSpeaker.norm.absoluteValue / 7) + .value + .meters, + vision.robotTSpeaker.x - + (drivetrain.robotVelocity.x * vision.robotTSpeaker.norm.absoluteValue / 7) + .value + .meters + ) + .magnitude + .meters Logger.recordOutput("AutoAim/currentDistanceInches", distance.inInches) return distance } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt index 8a2ae78a..649a07ec 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt @@ -50,7 +50,7 @@ sealed interface Request { class PassingShot() : SuperstructureRequest - class UnderStageShot(): SuperstructureRequest + class UnderStageShot() : SuperstructureRequest class Tuning() : SuperstructureRequest } 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 833a1663..bc4eb6af 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt @@ -1,7 +1,6 @@ package com.team4099.robot2023.subsystems.superstructure import com.team4099.lib.hal.Clock -import com.team4099.robot2023.config.constants.ElevatorConstants import com.team4099.robot2023.config.constants.FieldConstants import com.team4099.robot2023.config.constants.FlywheelConstants import com.team4099.robot2023.config.constants.WristConstants @@ -20,7 +19,6 @@ import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj.RobotBase import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.SubsystemBase -import java.awt.Robot import org.littletonrobotics.junction.Logger import org.team4099.lib.geometry.Pose3d import org.team4099.lib.geometry.Rotation3d @@ -39,6 +37,7 @@ 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, @@ -87,6 +86,14 @@ class Superstructure( var notes = mutableListOf() + var holdingNote = false + get() { + return feeder.hasNote || + currentState == SuperstructureStates.GROUND_INTAKE_CLEAN_UP_PUSH_BACK || + currentState == SuperstructureStates.GROUND_INTAKE_CLEAN_UP_PUSH_FORWARD || + (RobotBase.isSimulation() && noteHoldingID != -1) + } + fun toDoubleArray(somePose: Pose3d): DoubleArray { return doubleArrayOf( somePose.x.inMeters, @@ -108,6 +115,7 @@ class Superstructure( notes.add( NoteSimulation( notes.size, + stagedPose = Pose3d( it?.x ?: 0.0.inches, it?.y ?: 0.0.inches, @@ -122,6 +130,7 @@ class Superstructure( notes.add( NoteSimulation( notes.size, + stagedPose = Pose3d( it?.x ?: 0.0.inches, it?.y ?: 0.0.inches, @@ -135,11 +144,15 @@ class Superstructure( notes.forEach { it.poseSupplier = { drivetrain.fieldTRobot } } notes.forEach { it.wristAngleSupplier = { wrist.inputs.wristPosition } } notes.forEach { it.elevatorHeightSupplier = { elevator.inputs.elevatorPosition } } - notes.forEach { it.flywheelAngularVelocitySupplier = { flywheel.inputs.rightFlywheelVelocity } } + notes.forEach { + it.flywheelAngularVelocitySupplier = { flywheel.inputs.rightFlywheelVelocity } + } + notes.forEach { it.currentState = NoteSimulation.NoteStates.STAGED } + notes[0].currentState = NoteSimulation.NoteStates.IN_ROBOT + notes[7].currentState = NoteSimulation.NoteStates.OFF_FIELD noteHoldingID = 0 } - } override fun periodic() { @@ -270,15 +283,10 @@ class Superstructure( if (elevator.isHomed) { nextState = SuperstructureStates.IDLE } - } SuperstructureStates.IDLE -> { - intake.currentRequest = - Request.IntakeRequest.OpenLoop( - Intake.TunableIntakeStates.idleRollerVoltage.get(), - Intake.TunableIntakeStates.idleCenterWheelVoltage.get() - ) + intake.currentRequest = Request.IntakeRequest.OpenLoop(0.0.volts, 0.0.volts) feeder.currentRequest = Request.FeederRequest.OpenLoopIntake(Feeder.TunableFeederStates.idleVoltage.get()) @@ -298,6 +306,12 @@ class Superstructure( Request.WristRequest.TargetingPosition( Wrist.TunableWristStates.idleAngleHasGamepiece.get() ) + + intake.currentRequest = + Request.IntakeRequest.OpenLoop( + Intake.TunableIntakeStates.idleRollerVoltage.get(), + Intake.TunableIntakeStates.idleCenterWheelVoltage.get() + ) } else { wrist.currentRequest = Request.WristRequest.TargetingPosition(Wrist.TunableWristStates.idleAngle.get()) @@ -321,10 +335,10 @@ class Superstructure( nextState = SuperstructureStates.EJECT_GAME_PIECE_PREP } is Request.SuperstructureRequest.PrepScoreAmp -> { - nextState = SuperstructureStates.ELEVATOR_AMP_PREP + nextState = SuperstructureStates.ELEVATOR_AMP_PREP } is Request.SuperstructureRequest.ScoreSpeaker -> { - if (feeder.hasNote) { + if (feeder.hasNote || (RobotBase.isSimulation() && noteHoldingID != -1)) { nextState = SuperstructureStates.SCORE_SPEAKER_LOW_PREP } } @@ -367,7 +381,9 @@ class Superstructure( } SuperstructureStates.GROUND_INTAKE_PREP -> { wrist.currentRequest = - Request.WristRequest.TargetingPosition(Wrist.TunableWristStates.intakeAngle.get(), 1.0.degrees) + Request.WristRequest.TargetingPosition( + Wrist.TunableWristStates.intakeAngle.get(), 1.0.degrees + ) if (wrist.isAtTargetedPosition) { nextState = SuperstructureStates.GROUND_INTAKE } @@ -466,8 +482,14 @@ class Superstructure( Logger.recordOutput("AutoAim/FlywheelSpeed", targetFlywheelSpeed.inRotationsPerMinute) Logger.recordOutput("AutoAim/WristAngle", targetWristAngle.inDegrees) - flywheel.currentRequest = Request.FlywheelRequest.TargetingVelocity(targetFlywheelSpeed) - wrist.currentRequest = Request.WristRequest.TargetingPosition(targetWristAngle) + 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) + } when (currentRequest) { is Request.SuperstructureRequest.Idle -> { @@ -484,7 +506,6 @@ class Superstructure( } } } - SuperstructureStates.HIGH_AIM -> { val targetFlywheelSpeed = aimer.calculateHighFlywheelSpeed() @@ -506,7 +527,6 @@ class Superstructure( shootStartTime = Clock.fpgaTime } } - } } SuperstructureStates.ELEVATOR_AMP_PREP -> { @@ -515,7 +535,9 @@ class Superstructure( Elevator.TunableElevatorHeights.shootAmpFeederPosition.get() ) wrist.currentRequest = - Request.WristRequest.TargetingPosition(Wrist.TunableWristStates.ampScoreAngle.get(), 2.0.degrees) + Request.WristRequest.TargetingPosition( + Wrist.TunableWristStates.ampScoreAngle.get(), 2.0.degrees + ) if (elevator.isAtTargetedPosition && wrist.isAtTargetedPosition && @@ -687,14 +709,16 @@ class Superstructure( when (currentRequest) { is Request.SuperstructureRequest.Idle -> { nextState = SuperstructureStates.IDLE - } is Request.SuperstructureRequest.AutoAim -> { + } + is Request.SuperstructureRequest.AutoAim -> { nextState = SuperstructureStates.HIGH_AIM } } } SuperstructureStates.MANUAL_SCORE_SPEAKER_PREP -> { flywheel.currentRequest = Request.FlywheelRequest.TargetingVelocity(flywheelToShootAt) - wrist.currentRequest = Request.WristRequest.TargetingPosition(wristAngleToShootAt, 1.5.degrees) + wrist.currentRequest = + Request.WristRequest.TargetingPosition(wristAngleToShootAt, 1.5.degrees) if (wrist.isAtTargetedPosition && flywheel.isAtTargetedVelocity && currentRequest is Request.SuperstructureRequest.ScoreSpeaker @@ -785,7 +809,9 @@ class Superstructure( } SuperstructureStates.EJECT_GAME_PIECE_PREP -> { wrist.currentRequest = - Request.WristRequest.TargetingPosition(Wrist.TunableWristStates.ejectAngle.get(), 2.0.degrees) + Request.WristRequest.TargetingPosition( + Wrist.TunableWristStates.ejectAngle.get(), 2.0.degrees + ) if (wrist.isAtTargetedPosition) { nextState = SuperstructureStates.EJECT_GAME_PIECE @@ -799,7 +825,9 @@ class Superstructure( } SuperstructureStates.PASSING_SHOT_PREP -> { wrist.currentRequest = - Request.WristRequest.TargetingPosition(Wrist.TunableWristStates.passingShotAngle.get(), 2.0.degrees) + Request.WristRequest.TargetingPosition( + Wrist.TunableWristStates.passingShotAngle.get(), 2.0.degrees + ) flywheel.currentRequest = Request.FlywheelRequest.TargetingVelocity( Flywheel.TunableFlywheelStates.passingShotVelocity.get() @@ -819,10 +847,11 @@ class Superstructure( } } } - SuperstructureStates.UNDER_STAGE_SHOT_PREP -> { wrist.currentRequest = - Request.WristRequest.TargetingPosition(Wrist.TunableWristStates.underStageShotAngle.get(), 2.0.degrees) + Request.WristRequest.TargetingPosition( + Wrist.TunableWristStates.underStageShotAngle.get(), 2.0.degrees + ) flywheel.currentRequest = Request.FlywheelRequest.TargetingVelocity( Flywheel.TunableFlywheelStates.underStageShotVelocity.get() @@ -830,7 +859,8 @@ class Superstructure( shootStartTime = Clock.fpgaTime - if (flywheel.isAtTargetedVelocity && wrist.isAtTargetedPosition && + if (flywheel.isAtTargetedVelocity && + wrist.isAtTargetedPosition && currentRequest is Request.SuperstructureRequest.ScoreSpeaker ) { nextState = SuperstructureStates.SCORE_SPEAKER @@ -874,13 +904,12 @@ class Superstructure( return returnCommand } - fun forceIdleIfAutoAimCommand() : Command { - val returnCommand = - run { - if (currentState == SuperstructureStates.AUTO_AIM) { - currentRequest = Request.SuperstructureRequest.Idle() - } + fun forceIdleIfAutoAimCommand(): Command { + val returnCommand = run { + if (currentState == SuperstructureStates.AUTO_AIM) { + currentRequest = Request.SuperstructureRequest.Idle() } + } returnCommand.name = "forceIdleIfAutoAim" return returnCommand } @@ -979,7 +1008,8 @@ class Superstructure( val returnCommand = run { if (currentState == SuperstructureStates.ELEVATOR_AMP_PREP || - currentState == SuperstructureStates.WRIST_AMP_PREP || currentState == SuperstructureStates.SCORE_ELEVATOR_AMP + currentState == SuperstructureStates.WRIST_AMP_PREP || + currentState == SuperstructureStates.SCORE_ELEVATOR_AMP ) { currentRequest = Request.SuperstructureRequest.ScoreAmp() } else if (currentState == SuperstructureStates.SCORE_TRAP_PREP) { @@ -993,11 +1023,9 @@ class Superstructure( return returnCommand } - fun noNoteDeadlineCommand() : Command { - val returnCommand = - run { + fun noNoteDeadlineCommand(): Command { + val returnCommand = run {}.until { !feeder.hasNote } - }.until {!feeder.hasNote} returnCommand.name = "noNoteDeadlineCommand" return returnCommand } @@ -1038,8 +1066,6 @@ class Superstructure( return returnCommand } - - fun climbExtendCommand(): Command { val returnCommand = runOnce { currentRequest = Request.SuperstructureRequest.ClimbExtend() }.until { @@ -1060,8 +1086,7 @@ class Superstructure( fun autoAimCommand(): Command { val returnCommand = - run { - currentRequest = Request.SuperstructureRequest.AutoAim() }.until { + run { currentRequest = Request.SuperstructureRequest.AutoAim() }.until { isAtRequestedState && currentState == SuperstructureStates.AUTO_AIM } returnCommand.name = "AutoAim" 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 2ec95887..28f48e95 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt @@ -73,7 +73,7 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { this.speakerVisionConsumer = speakerVisionMeasurementConsumer } - fun getShotConfidence(): Boolean{ + fun getShotConfidence(): Boolean { return Clock.realTimestamp - lastDetectionTime < 4.seconds && trustedRobotDistanceToTarget < 210.inches } @@ -112,12 +112,12 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { robotDistanceToTarget = PhotonUtils.calculateDistanceToTargetMeters( - cameraPoses[instance].translation.z.inMeters, - 57.125.inches.inMeters, - 22.42.degrees.inRadians, - tag.pitch.degrees.inRadians - ) - .meters + 4.inches + cameraPoses[instance].translation.z.inMeters, + 57.125.inches.inMeters, + 22.42.degrees.inRadians, + tag.pitch.degrees.inRadians + ) + .meters + 4.inches lastDetectionTime = Clock.realTimestamp @@ -133,8 +133,7 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { ) ) - robotTSpeaker = - Translation3d(cameraTspeaker2d.x, cameraTspeaker2d.y, 0.meters) + robotTSpeaker = Translation3d(cameraTspeaker2d.x, cameraTspeaker2d.y, 0.meters) val timestampedTrigVisionUpdate = TimestampedTrigVisionUpdate(