Skip to content

Commit

Permalink
Bob 2024 (#56)
Browse files Browse the repository at this point in the history
* Remove vision

* BOB wrist tuning + wvrox code

* latest bob code

* bob changes

* fix failing build

---------

Co-authored-by: Shom770 <[email protected]>
  • Loading branch information
AlphaPranav9102 and Shom770 authored Nov 18, 2024
1 parent 28b7e21 commit 64c556b
Show file tree
Hide file tree
Showing 12 changed files with 461 additions and 56 deletions.
6 changes: 1 addition & 5 deletions src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,6 @@ import com.team4099.robot2023.subsystems.superstructure.Request
import com.team4099.robot2023.subsystems.superstructure.Superstructure
import com.team4099.robot2023.subsystems.vision.Vision
import com.team4099.robot2023.subsystems.vision.camera.CameraIO
import com.team4099.robot2023.subsystems.vision.camera.CameraIOPhotonvision
import com.team4099.robot2023.subsystems.wrist.Wrist
import com.team4099.robot2023.subsystems.wrist.WristIOSim
import com.team4099.robot2023.subsystems.wrist.WristIOTalon
Expand Down Expand Up @@ -76,7 +75,7 @@ object RobotContainer {
// drivetrain = Drivetrain(object: GyroIO {},object: DrivetrainIO {}

drivetrain = Drivetrain(GyroIOPigeon2, DrivetrainIOReal)
vision = Vision(object : CameraIO {}, CameraIOPhotonvision("parakeet_2"))
vision = Vision(object : CameraIO {})
limelight = LimelightVision(LimelightVisionIOReal)
intake = Intake(IntakeIOFalconNEO)
feeder = Feeder(FeederIONeo)
Expand Down Expand Up @@ -240,7 +239,6 @@ object RobotContainer {
ControlBoard.passingShot.whileTrue(superstructure.passingShotCommand())
ControlBoard.underStagePassingShot.whileTrue(superstructure.underStageCommand())

/*
ControlBoard.targetAmp.whileTrue(
TargetAngleCommand(
driver = Jessika(),
Expand All @@ -253,8 +251,6 @@ object RobotContainer {
)
)

*/

ControlBoard.climbAlignFar.whileTrue(
runOnce({
setClimbAngle =
Expand Down
39 changes: 36 additions & 3 deletions src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,8 @@ package com.team4099.robot2023.auto
import com.team4099.robot2023.auto.mode.ExamplePathAuto
import com.team4099.robot2023.auto.mode.FiveNoteAutoPath
import com.team4099.robot2023.auto.mode.FourNoteAutoPath
import com.team4099.robot2023.auto.mode.FourNoteAutoPathWithFirstCenterSide
import com.team4099.robot2023.auto.mode.FourNoteAutoPathWithFirstSourceSide
import com.team4099.robot2023.auto.mode.FourNoteLeftCenterLine
import com.team4099.robot2023.auto.mode.FourNoteMiddleCenterLine
import com.team4099.robot2023.auto.mode.FourNoteRightCenterLine
Expand Down Expand Up @@ -46,7 +48,18 @@ object AutonomousSelector {
// orientationChooser.addOption("Right", 270.degrees)
// autoTab.add("Starting Orientation", orientationChooser)

autonomousModeChooser.addOption("Four Note Wing Auto", AutonomousMode.FOUR_NOTE_AUTO_PATH)
autonomousModeChooser.addOption(
"Four Note Wing Auto (Amp Side Note First, Default)",
AutonomousMode.FOUR_NOTE_AUTO_PATH_WITH_AMP_SIDE_FIRST
)
autonomousModeChooser.addOption(
"Four Note Wing Auto (Center Wing Note First)",
AutonomousMode.FOUR_NOTE_AUTO_PATH_WITH_CENTER_SIDE_FIRST
)
autonomousModeChooser.addOption(
"Four Note Wing Auto (Source Side Note First)",
AutonomousMode.FOUR_NOTE_AUTO_PATH_WITH_SOURCE_SIDE_FIRST
)

/*
Expand Down Expand Up @@ -169,14 +182,32 @@ object AutonomousSelector {
)
})
.andThen(SixNoteAutoPath(drivetrain, superstructure))
AutonomousMode.FOUR_NOTE_AUTO_PATH ->
AutonomousMode.FOUR_NOTE_AUTO_PATH_WITH_AMP_SIDE_FIRST ->
return WaitCommand(waitTime.inSeconds)
.andThen({
val flippedPose = AllianceFlipUtil.apply(FourNoteAutoPath.startingPose)
drivetrain.tempZeroGyroYaw(flippedPose.rotation)
drivetrain.resetFieldFrameEstimator(flippedPose)
})
.andThen(FourNoteAutoPath(drivetrain, superstructure))
AutonomousMode.FOUR_NOTE_AUTO_PATH_WITH_CENTER_SIDE_FIRST ->
return WaitCommand(waitTime.inSeconds)
.andThen({
val flippedPose =
AllianceFlipUtil.apply(FourNoteAutoPathWithFirstCenterSide.startingPose)
drivetrain.tempZeroGyroYaw(flippedPose.rotation)
drivetrain.resetFieldFrameEstimator(flippedPose)
})
.andThen(FourNoteAutoPathWithFirstCenterSide(drivetrain, superstructure))
AutonomousMode.FOUR_NOTE_AUTO_PATH_WITH_SOURCE_SIDE_FIRST ->
return WaitCommand(waitTime.inSeconds)
.andThen({
val flippedPose =
AllianceFlipUtil.apply(FourNoteAutoPathWithFirstSourceSide.startingPose)
drivetrain.tempZeroGyroYaw(flippedPose.rotation)
drivetrain.resetFieldFrameEstimator(flippedPose)
})
.andThen(FourNoteAutoPathWithFirstSourceSide(drivetrain, superstructure))
AutonomousMode.FOUR_NOTE_RIGHT_AUTO_PATH ->
return WaitCommand(waitTime.inSeconds)
.andThen({
Expand Down Expand Up @@ -321,7 +352,9 @@ object AutonomousSelector {

private enum class AutonomousMode {
TEST_AUTO_PATH,
FOUR_NOTE_AUTO_PATH,
FOUR_NOTE_AUTO_PATH_WITH_AMP_SIDE_FIRST,
FOUR_NOTE_AUTO_PATH_WITH_CENTER_SIDE_FIRST,
FOUR_NOTE_AUTO_PATH_WITH_SOURCE_SIDE_FIRST,
FOUR_NOTE_RIGHT_AUTO_PATH,
FOUR_NOTE_MIDDLE_AUTO_PATH,
FOUR_NOTE_LEFT_AUTO_PATH,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -36,14 +36,13 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru
startingPose.rotation.inRotation2ds
),
FieldWaypoint(
Translation2d(2.9.meters - 0.4.meters, 6.9.meters + 0.1.meters)
.translation2d,
Translation2d(2.91.meters - 0.75.meters, 6.82.meters).translation2d,
null,
180.degrees.inRotation2ds,
),
FieldWaypoint(
Translation2d(2.9.meters, 6.9.meters + 0.1.meters).translation2d,
null,
Translation2d(2.91.meters - 0.25.meters, 7.meters).translation2d,
0.degrees.inRotation2ds,
180.degrees.inRotation2ds,
),
FieldWaypoint(
Expand Down Expand Up @@ -73,13 +72,13 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru
180.degrees.inRotation2ds
), // Subwoofer
FieldWaypoint(
Translation2d(2.41.meters - 0.4.meters, 4.1.meters).translation2d,
Translation2d(2.41.meters - 0.4.meters, 4.4.meters).translation2d,
null,
180.degrees.inRotation2ds
),
FieldWaypoint(
Translation2d(2.41.meters + 0.225.meters, 4.1.meters).translation2d,
null,
Translation2d(2.41.meters, 4.1.meters).translation2d,
0.degrees.inRotation2ds,
180.degrees.inRotation2ds
),
FieldWaypoint(
Expand Down Expand Up @@ -181,6 +180,6 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru
}

companion object {
val startingPose = Pose2d(Translation2d(1.42.meters - 3.inches, 5.535.meters), 180.degrees)
val startingPose = Pose2d(Translation2d(1.42.meters + 1.inches, 5.535.meters), 180.degrees)
}
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,187 @@
package com.team4099.robot2023.auto.mode

import com.team4099.lib.trajectory.FieldWaypoint
import com.team4099.robot2023.commands.drivetrain.DrivePathCommand
import com.team4099.robot2023.config.constants.FlywheelConstants
import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
import com.team4099.robot2023.subsystems.superstructure.Superstructure
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup
import edu.wpi.first.wpilibj2.command.WaitCommand
import org.team4099.lib.geometry.Pose2d
import org.team4099.lib.geometry.Translation2d
import org.team4099.lib.units.base.inSeconds
import org.team4099.lib.units.base.inches
import org.team4099.lib.units.base.meters
import org.team4099.lib.units.derived.degrees
import org.team4099.lib.units.derived.inRotation2ds

class FourNoteAutoPathWithFirstCenterSide(
val drivetrain: Drivetrain,
val superstructure: Superstructure
) : SequentialCommandGroup() {
init {
addRequirements(drivetrain, superstructure)

addCommands(
superstructure.prepSpeakerLowCommand(),
superstructure.scoreCommand().withTimeout(0.5),
WaitCommand(0.5),
ParallelCommandGroup(
DrivePathCommand.createPathInFieldFrame(
drivetrain,
{
listOf(
FieldWaypoint(
startingPose.translation.translation2d,
null,
180.degrees.inRotation2ds
),
FieldWaypoint(
Translation2d(
((1.43.meters) + (2.34.meters + 0.3.meters)) / 2 +
0.25.meters,
5.55.meters
)
.translation2d,
null,
180.degrees.inRotation2ds
),
FieldWaypoint(
Translation2d(2.34.meters + 0.3.meters + 0.25.meters, 5.535.meters)
.translation2d,
null,
180.degrees.inRotation2ds
),
FieldWaypoint(
Translation2d(
((1.43.meters) + (2.34.meters + 0.3.meters)) / 2 +
0.25.meters,
5.45.meters
)
.translation2d,
null,
180.degrees.inRotation2ds
),
FieldWaypoint(
startingPose.translation.translation2d,
null,
180.degrees.inRotation2ds
) // Subwoofer
)
}
)
.withTimeout(3.235 + 0.5),
WaitCommand(0.3).andThen(superstructure.groundIntakeCommand())
),
superstructure.prepSpeakerLowCommand(),
superstructure
.scoreCommand()
.withTimeout(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds + 0.5),
WaitCommand(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds),
ParallelCommandGroup(
DrivePathCommand.createPathInFieldFrame(
drivetrain,
{
listOf(
FieldWaypoint(
startingPose.translation.translation2d,
null,
180.degrees.inRotation2ds
), // Subwoofer
FieldWaypoint(
Translation2d(2.41.meters - 0.4.meters, 4.4.meters).translation2d,
null,
180.degrees.inRotation2ds
),
FieldWaypoint(
Translation2d(2.41.meters, 4.1.meters).translation2d,
0.degrees.inRotation2ds,
180.degrees.inRotation2ds
),
FieldWaypoint(
startingPose.translation.translation2d,
null,
180.degrees.inRotation2ds
)
)
}
)
.withTimeout(3.235 + 0.5),
WaitCommand(0.5).andThen(superstructure.groundIntakeCommand())
),
superstructure.prepSpeakerLowCommand(),
superstructure
.scoreCommand()
.withTimeout(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds + 0.5),
WaitCommand(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds),
ParallelCommandGroup(
DrivePathCommand.createPathInFieldFrame(
drivetrain,
{
listOf(
FieldWaypoint(
startingPose.translation.translation2d,
null,
startingPose.rotation.inRotation2ds
),
FieldWaypoint(
Translation2d(2.91.meters - 0.75.meters, 6.9.meters).translation2d,
null,
180.degrees.inRotation2ds,
),
FieldWaypoint(
Translation2d(2.91.meters - 0.25.meters, 7.1.meters).translation2d,
0.degrees.inRotation2ds,
180.degrees.inRotation2ds,
),
FieldWaypoint(
startingPose.translation.translation2d,
null,
180.degrees.inRotation2ds
)
)
}
)
.withTimeout(3.235 + 0.5),
WaitCommand(0.5).andThen(superstructure.groundIntakeCommand())
),
superstructure.prepSpeakerLowCommand(),
superstructure
.scoreCommand()
.withTimeout(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds + 0.5),
ParallelCommandGroup(
DrivePathCommand.createPathInFieldFrame(
drivetrain,
{
listOf(
FieldWaypoint(
startingPose.translation.translation2d, null, 180.degrees.inRotation2ds
),
FieldWaypoint(
Translation2d(4.35.meters, 4.85.meters).translation2d,
null,
180.degrees.inRotation2ds
),
FieldWaypoint(
Translation2d(5.92.meters, 3.9.meters).translation2d,
null,
180.degrees.inRotation2ds
),
FieldWaypoint(
Translation2d(8.29.meters, 4.09.meters).translation2d,
null,
180.degrees.inRotation2ds
)
)
}
),
WaitCommand(1.0).andThen(superstructure.groundIntakeCommand())
)
)
}

companion object {
val startingPose = Pose2d(Translation2d(1.42.meters + 1.inches, 5.535.meters), 180.degrees)
}
}
Loading

0 comments on commit 64c556b

Please sign in to comment.