Skip to content

Commit

Permalink
tune swerve turning
Browse files Browse the repository at this point in the history
  • Loading branch information
@MatthewChoulas
MatthewChoulas committed May 17, 2024
1 parent a430254 commit 52d87f3
Show file tree
Hide file tree
Showing 10 changed files with 115 additions and 114 deletions.
35 changes: 23 additions & 12 deletions src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,8 +4,6 @@ import com.team4099.lib.logging.LoggedTunableValue
import com.team4099.robot2023.auto.AutonomousSelector
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 @@ -37,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 @@ -112,7 +110,7 @@ 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) },
Expand Down Expand Up @@ -166,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 @@ -182,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 @@ -210,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 @@ -236,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 @@ -248,6 +253,8 @@ object RobotContainer {
)
)
*/

ControlBoard.climbAlignFar.whileTrue(
runOnce({
setClimbAngle =
Expand Down Expand Up @@ -282,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 @@ -293,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 @@ -309,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 @@ -323,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 @@ -339,6 +348,8 @@ object RobotContainer {
)
)
*/

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

// ControlBoard.characterizeSubsystem.whileTrue(
Expand Down
15 changes: 5 additions & 10 deletions src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPath.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -55,8 +55,7 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru
}
)
.withTimeout(3.235 + 0.5),
WaitCommand(0.5)
.andThen(superstructure.groundIntakeCommand())
WaitCommand(0.5).andThen(superstructure.groundIntakeCommand())
),
superstructure.prepSpeakerLowCommand(),
superstructure
Expand Down Expand Up @@ -92,8 +91,7 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru
}
)
.withTimeout(3.235 + 0.5),
WaitCommand(0.5)
.andThen(superstructure.groundIntakeCommand())
WaitCommand(0.5).andThen(superstructure.groundIntakeCommand())
),
superstructure.prepSpeakerLowCommand(),
superstructure
Expand All @@ -113,7 +111,7 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru
FieldWaypoint(
Translation2d(
((1.43.meters) + (2.34.meters + 0.3.meters)) / 2 +
0.25.meters,
0.25.meters,
5.55.meters
)
.translation2d,
Expand All @@ -129,7 +127,7 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru
FieldWaypoint(
Translation2d(
((1.43.meters) + (2.34.meters + 0.3.meters)) / 2 +
0.25.meters,
0.25.meters,
5.45.meters
)
.translation2d,
Expand All @@ -145,8 +143,7 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru
}
)
.withTimeout(3.235 + 0.5),
WaitCommand(0.3)
.andThen(superstructure.groundIntakeCommand())
WaitCommand(0.3).andThen(superstructure.groundIntakeCommand())
),
superstructure.prepSpeakerLowCommand(),
superstructure
Expand Down Expand Up @@ -187,5 +184,3 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru
val startingPose = Pose2d(Translation2d(1.42.meters - 3.inches, 5.535.meters), 180.degrees)
}
}


6 changes: 2 additions & 4 deletions ...ain/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteAndPickupCenterlineSourceAutoPath.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,6 @@ 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
Expand Down Expand Up @@ -110,8 +109,7 @@ class ThreeNoteAndPickupCenterlineSourceAutoPath(
)
}
),
WaitCommand(1.0)
.andThen(superstructure.groundIntakeCommand())
WaitCommand(1.0).andThen(superstructure.groundIntakeCommand())
),
superstructure.prepManualSpeakerCommand(-4.5.degrees, 4000.rotations.perMinute),
superstructure.scoreCommand(),
Expand Down Expand Up @@ -146,4 +144,4 @@ class ThreeNoteAndPickupCenterlineSourceAutoPath(
companion object {
val startingPose = Pose2d(0.63.meters, 4.44.meters, 120.degrees)
}
}
}
145 changes: 72 additions & 73 deletions ...eam4099/robot2023/auto/mode/ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,6 @@ 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
Expand Down Expand Up @@ -40,87 +39,87 @@ class ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath(
null,
180.degrees.inRotation2ds
),
FieldWaypoint(
Translation2d(8.32.meters, 2.44.meters).translation2d,
null,
220.degrees.inRotation2ds
)
FieldWaypoint(
Translation2d(8.32.meters, 2.44.meters).translation2d,
null,
220.degrees.inRotation2ds
)
)
}
),
WaitCommand(1.5).andThen(superstructure.groundIntakeCommand())
),
ParallelCommandGroup(
DrivePathCommand.createPathInFieldFrame(
drivetrain,
{
listOf(
FieldWaypoint(
Translation2d(8.32.meters, 2.44.meters).translation2d,
null,
220.degrees.inRotation2ds
),
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(-4.5.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(4.33.meters, 1.67.meters).translation2d,
null,
(180 - 43.37583640633171).degrees.inRotation2ds
)
)
}
ParallelCommandGroup(
DrivePathCommand.createPathInFieldFrame(
drivetrain,
{
listOf(
FieldWaypoint(
Translation2d(8.32.meters, 2.44.meters).translation2d,
null,
220.degrees.inRotation2ds
),
WaitCommand(1.5)
.andThen(superstructure.groundIntakeCommand())
.andThen(WaitCommand(2.0))
.andThen(
superstructure.prepManualSpeakerCommand(
-4.5.degrees, 4000.rotations.perMinute
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(-4.5.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(4.33.meters, 1.67.meters).translation2d,
null,
(180 - 43.37583640633171).degrees.inRotation2ds
)
)
)
.andThen(superstructure.scoreCommand())
}
),
WaitCommand(1.5)
.andThen(superstructure.groundIntakeCommand())
.andThen(WaitCommand(2.0))
.andThen(
superstructure.prepManualSpeakerCommand(
-4.5.degrees, 4000.rotations.perMinute
)
)
)
.andThen(superstructure.scoreCommand())
)
)
}

Expand Down
Loading

0 comments on commit 52d87f3

Please sign in to comment.