From f6e176bb3e8bde90ba8582bbad06893e73b315a4 Mon Sep 17 00:00:00 2001 From: 5190laptop Date: Tue, 19 Feb 2019 23:22:18 -0500 Subject: [PATCH] Bag and Tag --- .../kotlin/org/ghrobotics/frc2019/Network.kt | 24 +- .../kotlin/org/ghrobotics/frc2019/Robot.kt | 7 +- .../org/ghrobotics/frc2019/auto/Autonomous.kt | 10 +- .../auto/routines/SideCargoShipRoutine.kt | 216 +++++++++--------- .../frc2019/subsystems/Superstructure.kt | 2 +- .../subsystems/drive/ManualDriveCommand.kt | 13 +- 6 files changed, 141 insertions(+), 131 deletions(-) diff --git a/src/main/kotlin/org/ghrobotics/frc2019/Network.kt b/src/main/kotlin/org/ghrobotics/frc2019/Network.kt index fca90b9..ef88012 100644 --- a/src/main/kotlin/org/ghrobotics/frc2019/Network.kt +++ b/src/main/kotlin/org/ghrobotics/frc2019/Network.kt @@ -22,9 +22,9 @@ object Network { val startingPositionChooser = enumSendableChooser() val autoModeChooser = enumSendableChooser() - val cargoShip1Chooser = enumSendableChooser() - val cargoShip2Chooser = enumSendableChooser() - val cargoShip3Chooser = enumSendableChooser() +// val cargoShip1Chooser = enumSendableChooser() +// val cargoShip2Chooser = enumSendableChooser() +// val cargoShip3Chooser = enumSendableChooser() private val mainShuffleboardDisplay: ShuffleboardTab = Shuffleboard.getTab("5190") @@ -103,15 +103,15 @@ object Network { "Starting Position", startingPositionChooser ) - autoLayout.add( - "Cargo Ship 1", cargoShip1Chooser - ) - autoLayout.add( - "Cargo Ship 2", cargoShip2Chooser - ) - autoLayout.add( - "Cargo Ship 3", cargoShip3Chooser - ) +// autoLayout.add( +// "Cargo Ship 1", cargoShip1Chooser +// ) +// autoLayout.add( +// "Cargo Ship 2", cargoShip2Chooser +// ) +// autoLayout.add( +// "Cargo Ship 3", cargoShip3Chooser +// ) //mainShuffleboardDisplay.add(VisionProcessing.cameraSource).withPosition(3, 2).withSize(3, 3) } diff --git a/src/main/kotlin/org/ghrobotics/frc2019/Robot.kt b/src/main/kotlin/org/ghrobotics/frc2019/Robot.kt index 13835c2..4f57a9c 100644 --- a/src/main/kotlin/org/ghrobotics/frc2019/Robot.kt +++ b/src/main/kotlin/org/ghrobotics/frc2019/Robot.kt @@ -14,7 +14,6 @@ import org.ghrobotics.frc2019.subsystems.climb.ClimbSubsystem import org.ghrobotics.frc2019.subsystems.drive.DriveSubsystem import org.ghrobotics.frc2019.subsystems.elevator.ElevatorSubsystem import org.ghrobotics.frc2019.subsystems.intake.IntakeSubsystem -import org.ghrobotics.frc2019.vision.VisionProcessing import org.ghrobotics.lib.commands.FalconSubsystem import org.ghrobotics.lib.mathematics.units.millisecond import org.ghrobotics.lib.wrappers.FalconRobot @@ -38,9 +37,9 @@ object Robot : FalconRobot() { Network Autonomous -// CameraServer.getInstance().startAutomaticCapture() -// .setVideoMode(VideoMode.PixelFormat.kMJPEG, 320, 240, 15) - VisionProcessing + CameraServer.getInstance().startAutomaticCapture() + .setVideoMode(VideoMode.PixelFormat.kMJPEG, 320, 240, 15) +// VisionProcessing } override fun periodic() { diff --git a/src/main/kotlin/org/ghrobotics/frc2019/auto/Autonomous.kt b/src/main/kotlin/org/ghrobotics/frc2019/auto/Autonomous.kt index 53ead7c..56dbf59 100644 --- a/src/main/kotlin/org/ghrobotics/frc2019/auto/Autonomous.kt +++ b/src/main/kotlin/org/ghrobotics/frc2019/auto/Autonomous.kt @@ -27,9 +27,9 @@ object Autonomous { // Starting position of the robot val startingPosition = { Network.startingPositionChooser.selected } - val cargoShipGamePiece1 = { Network.cargoShip1Chooser.selected } - val cargoShipGamePiece2 = { Network.cargoShip2Chooser.selected } - val cargoShipGamePiece3 = { Network.cargoShip3Chooser.selected } +// val cargoShipGamePiece1 = { Network.cargoShip1Chooser.selected } +// val cargoShipGamePiece2 = { Network.cargoShip2Chooser.selected } +// val cargoShipGamePiece3 = { Network.cargoShip3Chooser.selected } // Stores whether the current config is valid. private var configValid = Source(true) @@ -80,7 +80,7 @@ object Autonomous { Mode.BASELINE, BaselineRoutine() ) - state(Mode.SIDE_CARGO_SHIP, SideCargoShipRoutine()) +// state(Mode.SIDE_CARGO_SHIP, SideCargoShipRoutine()) state( Mode.TEST_TRAJECTORIES, @@ -128,5 +128,5 @@ object Autonomous { enum class GamePiece { HATCH, CARGO } - enum class Mode { TEST_TRAJECTORIES, HIGH_HATCHES_ROCKET, HATCH_AND_CARGO_ROCKET, FORWARD_CARGO_SHIP, SIDE_CARGO_SHIP, BASELINE } + enum class Mode { TEST_TRAJECTORIES, HIGH_HATCHES_ROCKET, HATCH_AND_CARGO_ROCKET, FORWARD_CARGO_SHIP, BASELINE } } diff --git a/src/main/kotlin/org/ghrobotics/frc2019/auto/routines/SideCargoShipRoutine.kt b/src/main/kotlin/org/ghrobotics/frc2019/auto/routines/SideCargoShipRoutine.kt index 1b16a99..0d2e408 100644 --- a/src/main/kotlin/org/ghrobotics/frc2019/auto/routines/SideCargoShipRoutine.kt +++ b/src/main/kotlin/org/ghrobotics/frc2019/auto/routines/SideCargoShipRoutine.kt @@ -1,108 +1,108 @@ -package org.ghrobotics.frc2019.auto.routines - -import org.ghrobotics.frc2019.auto.Autonomous -import org.ghrobotics.frc2019.auto.paths.TrajectoryFactory -import org.ghrobotics.frc2019.subsystems.Superstructure -import org.ghrobotics.frc2019.subsystems.arm.ArmSubsystem -import org.ghrobotics.frc2019.subsystems.arm.ClosedLoopArmCommand -import org.ghrobotics.frc2019.subsystems.drive.DriveSubsystem -import org.ghrobotics.frc2019.subsystems.intake.IntakeCargoCommand -import org.ghrobotics.frc2019.subsystems.intake.IntakeHatchCommand -import org.ghrobotics.frc2019.subsystems.intake.IntakeSubsystem -import org.ghrobotics.lib.commands.* -import org.ghrobotics.lib.mathematics.twodim.trajectory.types.duration -import org.ghrobotics.lib.mathematics.units.Time -import org.ghrobotics.lib.mathematics.units.degree -import org.ghrobotics.lib.mathematics.units.second -import org.ghrobotics.lib.utils.map -import org.ghrobotics.lib.utils.withEquals - -class SideCargoShipRoutine : AutoRoutine() { - - private val path1 = TrajectoryFactory.sideStartToCargoShipS1 - private val path2 = Autonomous.cargoShipGamePiece2.withEquals(Autonomous.GamePiece.CARGO) - .map(TrajectoryFactory.cargoShipS1ToDepot, TrajectoryFactory.cargoShipS1ToLoadingStation) - private val path3 = Autonomous.cargoShipGamePiece2.withEquals(Autonomous.GamePiece.CARGO) - .map(TrajectoryFactory.depotToCargoShipS2, TrajectoryFactory.loadingStationToCargoShipS2) - - val pathMirrored = Autonomous.startingPosition.withEquals(Autonomous.StartingPositions.LEFT) - - override val duration: Time - get() = path1.duration - - override val routine: FalconCommand - get() = sequential { - - +ConditionalCommand( - Autonomous.cargoShipGamePiece1.withEquals(Autonomous.GamePiece.CARGO), - IntakeCargoCommand(IntakeSubsystem.Direction.HOLD), - IntakeHatchCommand(IntakeSubsystem.Direction.HOLD) - ) - - +parallel { - +DriveSubsystem.followTrajectory(path1) - +sequential { - +DelayCommand(path1.duration - 3.second) - +ConditionalCommand( - Autonomous.cargoShipGamePiece1.withEquals(Autonomous.GamePiece.CARGO), - Superstructure.kFrontCargoFromLoadingStation.withTimeout(2.second), - Superstructure.kFrontHatchFromLoadingStation.withTimeout(2.second) - ) - } - } - - +ConditionalCommand( - Autonomous.cargoShipGamePiece1.withEquals(Autonomous.GamePiece.CARGO), - IntakeCargoCommand(IntakeSubsystem.Direction.RELEASE), - IntakeHatchCommand(IntakeSubsystem.Direction.RELEASE) - ).withTimeout(0.5.second) - - - +parallel { - +DriveSubsystem.followTrajectory(path2, pathMirrored) - +sequential { - +DelayCommand(0.2.second) - +parallel { - +ConditionalCommand( - Autonomous.cargoShipGamePiece2.withEquals(Autonomous.GamePiece.CARGO), - sequential { - +ConditionCommand { ArmSubsystem.armPosition > 105.degree } - +IntakeCargoCommand(IntakeSubsystem.Direction.HOLD) - }) - +ConditionalCommand( - Autonomous.cargoShipGamePiece2.withEquals(Autonomous.GamePiece.CARGO), - Superstructure.kBackCargoFromLoadingStation.withTimeout(2.second), - Superstructure.kBackHatchFromLoadingStation.withTimeout(2.second) - ) - } - } - } - - +ConditionalCommand( - Autonomous.cargoShipGamePiece2.withEquals(Autonomous.GamePiece.HATCH), - IntakeHatchCommand(IntakeSubsystem.Direction.HOLD) - ) - - +parallel { - +DriveSubsystem.followTrajectory(path3) - +sequential { - +executeFor( - 4.second, - Superstructure.kFrontCargoFromLoadingStation - ) - +ConditionalCommand( - Autonomous.cargoShipGamePiece1.withEquals(Autonomous.GamePiece.CARGO), - Superstructure.kFrontCargoFromLoadingStation.withTimeout(2.second), - Superstructure.kFrontHatchFromLoadingStation.withTimeout(2.second) - ) - } - } - - +ConditionalCommand( - Autonomous.cargoShipGamePiece2.withEquals(Autonomous.GamePiece.CARGO), - IntakeCargoCommand(IntakeSubsystem.Direction.RELEASE), - IntakeHatchCommand(IntakeSubsystem.Direction.RELEASE) - ).withTimeout(0.5.second) - } - -} +//package org.ghrobotics.frc2019.auto.routines +// +//import org.ghrobotics.frc2019.auto.Autonomous +//import org.ghrobotics.frc2019.auto.paths.TrajectoryFactory +//import org.ghrobotics.frc2019.subsystems.Superstructure +//import org.ghrobotics.frc2019.subsystems.arm.ArmSubsystem +//import org.ghrobotics.frc2019.subsystems.arm.ClosedLoopArmCommand +//import org.ghrobotics.frc2019.subsystems.drive.DriveSubsystem +//import org.ghrobotics.frc2019.subsystems.intake.IntakeCargoCommand +//import org.ghrobotics.frc2019.subsystems.intake.IntakeHatchCommand +//import org.ghrobotics.frc2019.subsystems.intake.IntakeSubsystem +//import org.ghrobotics.lib.commands.* +//import org.ghrobotics.lib.mathematics.twodim.trajectory.types.duration +//import org.ghrobotics.lib.mathematics.units.Time +//import org.ghrobotics.lib.mathematics.units.degree +//import org.ghrobotics.lib.mathematics.units.second +//import org.ghrobotics.lib.utils.map +//import org.ghrobotics.lib.utils.withEquals +// +//class SideCargoShipRoutine : AutoRoutine() { +// +// private val path1 = TrajectoryFactory.sideStartToCargoShipS1 +// private val path2 = Autonomous.cargoShipGamePiece2.withEquals(Autonomous.GamePiece.CARGO) +// .map(TrajectoryFactory.cargoShipS1ToDepot, TrajectoryFactory.cargoShipS1ToLoadingStation) +// private val path3 = Autonomous.cargoShipGamePiece2.withEquals(Autonomous.GamePiece.CARGO) +// .map(TrajectoryFactory.depotToCargoShipS2, TrajectoryFactory.loadingStationToCargoShipS2) +// +// val pathMirrored = Autonomous.startingPosition.withEquals(Autonomous.StartingPositions.LEFT) +// +// override val duration: Time +// get() = path1.duration +// +// override val routine: FalconCommand +// get() = sequential { +// +// +ConditionalCommand( +// Autonomous.cargoShipGamePiece1.withEquals(Autonomous.GamePiece.CARGO), +// IntakeCargoCommand(IntakeSubsystem.Direction.HOLD), +// IntakeHatchCommand(IntakeSubsystem.Direction.HOLD) +// ) +// +// +parallel { +// +DriveSubsystem.followTrajectory(path1) +// +sequential { +// +DelayCommand(path1.duration - 3.second) +// +ConditionalCommand( +// Autonomous.cargoShipGamePiece1.withEquals(Autonomous.GamePiece.CARGO), +// Superstructure.kFrontCargoFromLoadingStation.withTimeout(2.second), +// Superstructure.kFrontHatchFromLoadingStation.withTimeout(2.second) +// ) +// } +// } +// +// +ConditionalCommand( +// Autonomous.cargoShipGamePiece1.withEquals(Autonomous.GamePiece.CARGO), +// IntakeCargoCommand(IntakeSubsystem.Direction.RELEASE), +// IntakeHatchCommand(IntakeSubsystem.Direction.RELEASE) +// ).withTimeout(0.5.second) +// +// +// +parallel { +// +DriveSubsystem.followTrajectory(path2, pathMirrored) +// +sequential { +// +DelayCommand(0.2.second) +// +parallel { +// +ConditionalCommand( +// Autonomous.cargoShipGamePiece2.withEquals(Autonomous.GamePiece.CARGO), +// sequential { +// +ConditionCommand { ArmSubsystem.armPosition > 105.degree } +// +IntakeCargoCommand(IntakeSubsystem.Direction.HOLD) +// }) +// +ConditionalCommand( +// Autonomous.cargoShipGamePiece2.withEquals(Autonomous.GamePiece.CARGO), +// Superstructure.kBackCargoFromLoadingStation.withTimeout(2.second), +// Superstructure.kBackHatchFromLoadingStation.withTimeout(2.second) +// ) +// } +// } +// } +// +// +ConditionalCommand( +// Autonomous.cargoShipGamePiece2.withEquals(Autonomous.GamePiece.HATCH), +// IntakeHatchCommand(IntakeSubsystem.Direction.HOLD) +// ) +// +// +parallel { +// +DriveSubsystem.followTrajectory(path3) +// +sequential { +// +executeFor( +// 4.second, +// Superstructure.kFrontCargoFromLoadingStation +// ) +// +ConditionalCommand( +// Autonomous.cargoShipGamePiece1.withEquals(Autonomous.GamePiece.CARGO), +// Superstructure.kFrontCargoFromLoadingStation.withTimeout(2.second), +// Superstructure.kFrontHatchFromLoadingStation.withTimeout(2.second) +// ) +// } +// } +// +// +ConditionalCommand( +// Autonomous.cargoShipGamePiece2.withEquals(Autonomous.GamePiece.CARGO), +// IntakeCargoCommand(IntakeSubsystem.Direction.RELEASE), +// IntakeHatchCommand(IntakeSubsystem.Direction.RELEASE) +// ).withTimeout(0.5.second) +// } +// +//} diff --git a/src/main/kotlin/org/ghrobotics/frc2019/subsystems/Superstructure.kt b/src/main/kotlin/org/ghrobotics/frc2019/subsystems/Superstructure.kt index 24186dc..9592b57 100644 --- a/src/main/kotlin/org/ghrobotics/frc2019/subsystems/Superstructure.kt +++ b/src/main/kotlin/org/ghrobotics/frc2019/subsystems/Superstructure.kt @@ -32,7 +32,7 @@ object Superstructure { val kBackLowRocketCargo get() = goToHeightWithAngle(25.inch, 135.degree) val kFrontHatchFromLoadingStation get() = goToHeightWithAngle(16.inch, 0.degree) - val kBackHatchFromLoadingStation get() = goToHeightWithAngle(16.inch, 180.degree) + val kBackHatchFromLoadingStation get() = goToHeightWithAngle(17.inch, 180.degree) val kFrontCargoIntake get() = elevatorAndArmHeight(0.inch, (-20).degree) val kBackCargoIntake get() = elevatorAndArmHeight(0.inch, (-160).degree) diff --git a/src/main/kotlin/org/ghrobotics/frc2019/subsystems/drive/ManualDriveCommand.kt b/src/main/kotlin/org/ghrobotics/frc2019/subsystems/drive/ManualDriveCommand.kt index 1a63c78..66e0949 100644 --- a/src/main/kotlin/org/ghrobotics/frc2019/subsystems/drive/ManualDriveCommand.kt +++ b/src/main/kotlin/org/ghrobotics/frc2019/subsystems/drive/ManualDriveCommand.kt @@ -6,8 +6,12 @@ package org.ghrobotics.frc2019.subsystems.drive import edu.wpi.first.wpilibj.GenericHID +import org.ghrobotics.frc2019.Constants import org.ghrobotics.frc2019.Controls +import org.ghrobotics.frc2019.subsystems.arm.ArmSubsystem +import org.ghrobotics.frc2019.subsystems.elevator.ElevatorSubsystem import org.ghrobotics.lib.commands.FalconCommand +import org.ghrobotics.lib.mathematics.units.degree import org.ghrobotics.lib.utils.withDeadband import org.ghrobotics.lib.wrappers.hid.getRawButton import org.ghrobotics.lib.wrappers.hid.getX @@ -20,14 +24,21 @@ class ManualDriveCommand : FalconCommand(DriveSubsystem) { executeFrequency = 50 } + private var armStowedRange = (90.degree - Constants.kArmFlipTolerance)..(90.degree + Constants.kArmFlipTolerance) + override suspend fun execute() { // DriveSubsystem.tankDrive( // -leftSource(), // -rightSource() // ) + var rotation = rotationSource() +// if (ArmSubsystem.armPosition !in armStowedRange) { +// rotation *= 0.5 +// } + // TODO DriveSubsystem.curvatureDrive( -speedSource(), - rotationSource(), + rotation, quickTurnSource() ) }