Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Worlds 2024 code #48

Merged
merged 7 commits into from
Jun 19, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 5 additions & 0 deletions simgui-ds.json
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,11 @@
"visible": true
}
},
"System Joysticks": {
"window": {
"visible": false
}
},
"keyboardJoysticks": [
{
"axisConfig": [
Expand Down
24 changes: 20 additions & 4 deletions src/main/deploy/pathplanner/paths/New New New New Path.path
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down
5 changes: 2 additions & 3 deletions src/main/kotlin/com/team4099/robot2023/Robot.kt
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -197,7 +197,7 @@ object Robot : LoggedRobot() {
"LoggedRobot/RemainingRamMB", Runtime.getRuntime().freeMemory() / 1024 / 1024
)

DebugLogger.recordDebugOutput(
CustomLogger.recordDebugOutput(
"LoggedRobot/totalMS", (Clock.realTimestamp - startTime).inMilliseconds
)

Expand All @@ -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()
Expand Down
41 changes: 24 additions & 17 deletions src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
Original file line number Diff line number Diff line change
Expand Up @@ -2,11 +2,8 @@ package com.team4099.robot2023

import com.team4099.lib.logging.LoggedTunableValue
import com.team4099.robot2023.auto.AutonomousSelector
import com.team4099.robot2023.commands.drivetrain.LockDriveCommand
import com.team4099.robot2023.commands.drivetrain.ResetGyroYawCommand
import com.team4099.robot2023.commands.drivetrain.TargetAngleCommand
import com.team4099.robot2023.commands.drivetrain.TargetNoteCommand
import com.team4099.robot2023.commands.drivetrain.TargetSpeakerCommand
import com.team4099.robot2023.commands.drivetrain.TeleopDriveCommand
import com.team4099.robot2023.config.ControlBoard
import com.team4099.robot2023.config.constants.Constants
Expand Down Expand Up @@ -38,7 +35,7 @@ import com.team4099.robot2023.subsystems.vision.camera.CameraIOPhotonvision
import com.team4099.robot2023.subsystems.wrist.Wrist
import com.team4099.robot2023.subsystems.wrist.WristIOSim
import com.team4099.robot2023.subsystems.wrist.WristIOTalon
import com.team4099.robot2023.util.driver.Ryan
import com.team4099.robot2023.util.driver.Jessika
import edu.wpi.first.wpilibj.DriverStation
import edu.wpi.first.wpilibj.RobotBase
import edu.wpi.first.wpilibj2.command.Command
Expand Down Expand Up @@ -113,16 +110,13 @@ object RobotContainer {

drivetrain.defaultCommand =
TeleopDriveCommand(
driver = Ryan(),
driver = Jessika(),
{ ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
{ ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
{ ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) },
{ ControlBoard.slowMode },
drivetrain
)

superstructure.defaultCommand = superstructure.forceIdleIfAutoAimCommand()

/*
module steeing tuning

Expand Down Expand Up @@ -170,11 +164,15 @@ object RobotContainer {
limelight.limelightState = LimelightVision.LimelightStates.TARGETING_GAME_PIECE

ControlBoard.resetGyro.whileTrue(ResetGyroYawCommand(drivetrain))

ControlBoard.intake.whileTrue(superstructure.groundIntakeCommand())

/*
ControlBoard.intake.whileTrue(
ParallelCommandGroup(
superstructure.groundIntakeCommand(),
TargetNoteCommand(
driver = Ryan(),
driver = Jessika(),
{ ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
{ ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
{ ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) },
Expand All @@ -186,11 +184,13 @@ object RobotContainer {
)
)

*/

ControlBoard.prepAmp.whileTrue(superstructure.prepAmpCommand())

ControlBoard.passingShotAlignment.whileTrue(
TargetAngleCommand(
driver = Ryan(),
driver = Jessika(),
{ ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
{ ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
{ ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) },
Expand All @@ -214,7 +214,7 @@ object RobotContainer {
ParallelCommandGroup(
superstructure.prepSpeakerMidCommand(),
TargetAngleCommand(
driver = Ryan(),
driver = Jessika(),
{ ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
{ ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
{ ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) },
Expand All @@ -240,9 +240,10 @@ object RobotContainer {
ControlBoard.passingShot.whileTrue(superstructure.passingShotCommand())
ControlBoard.underStagePassingShot.whileTrue(superstructure.underStageCommand())

/*
ControlBoard.targetAmp.whileTrue(
TargetAngleCommand(
driver = Ryan(),
driver = Jessika(),
{ ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
{ ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
{ ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) },
Expand All @@ -252,6 +253,8 @@ object RobotContainer {
)
)

*/

ControlBoard.climbAlignFar.whileTrue(
runOnce({
setClimbAngle =
Expand Down Expand Up @@ -286,7 +289,7 @@ object RobotContainer {
)
ControlBoard.climbAutoAlign.whileTrue(
TargetAngleCommand(
driver = Ryan(),
driver = Jessika(),
{ ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
{ ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
{ ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) },
Expand All @@ -297,7 +300,7 @@ object RobotContainer {
)
// ControlBoard.climbAlignLeft.whileTrue(
// TargetAngleCommand(
// driver = Ryan(),
// driver = Jessika(),
// { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
// { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
// { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) },
Expand All @@ -313,7 +316,7 @@ object RobotContainer {
//
// ControlBoard.climbAlignRight.whileTrue(
// TargetAngleCommand(
// driver = Ryan(),
// driver = Jessika(),
// { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
// { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
// { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) },
Expand All @@ -327,10 +330,12 @@ object RobotContainer {
// )
// )

/*

ControlBoard.targetSpeaker.whileTrue(
ParallelCommandGroup(
TargetSpeakerCommand(
driver = Ryan(),
driver = Jessika(),
{ ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
{ ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
{ ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) },
Expand All @@ -343,7 +348,9 @@ object RobotContainer {
)
)

ControlBoard.lockWheels.whileTrue(LockDriveCommand(drivetrain))
*/

// ControlBoard.lockWheels.whileTrue(LockDriveCommand(drivetrain))

// ControlBoard.characterizeSubsystem.whileTrue(
// WheelRadiusCharacterizationCommand(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -128,7 +142,7 @@ object AutonomousSelector {
}

val waitTime: Time
get() = 0.0.seconds
get() = waitBeforeCommandSlider.getDouble(0.0).seconds

val secondaryWaitTime: Time
get() = secondaryWaitInAuto.getDouble(0.0).seconds
Expand Down Expand Up @@ -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({
Expand All @@ -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({
Expand Down Expand Up @@ -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,
Expand Down
Loading
Loading