Skip to content

Commit

Permalink
Worlds 2024 code (#48)
Browse files Browse the repository at this point in the history
* worlds load in debugging

* worlds q1-19

* code day 1

* adjust amp center line without first note

* fried at worlds but its over?

* tune swerve turning

* Custom logger (#49)

* add recordOutput method to DebugLogger

* rename to CustomLogger

* fix photonlib.json (now builds)

* add processInputs as a method under CustomLogger

---------

Co-authored-by: Matthew Choulas <[email protected]>
Co-authored-by: AlphaPranav9102 <[email protected]>
Co-authored-by: 00magikarp <[email protected]>
  • Loading branch information
4 people authored Jun 19, 2024
1 parent d1421ed commit 28b7e21
Show file tree
Hide file tree
Showing 44 changed files with 807 additions and 563 deletions.
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

0 comments on commit 28b7e21

Please sign in to comment.