Skip to content

Commit

Permalink
quals 1-6
Browse files Browse the repository at this point in the history
  • Loading branch information
@MatthewChoulas
MatthewChoulas committed Mar 2, 2024
1 parent f39e75c commit b5ab4ea
Show file tree
Hide file tree
Showing 6 changed files with 100 additions and 18 deletions.
20 changes: 8 additions & 12 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 @@ -2,7 +2,6 @@ package com.team4099.robot2023.auto.mode

import com.team4099.lib.trajectory.FieldWaypoint
import com.team4099.robot2023.commands.drivetrain.DrivePathCommand
import com.team4099.robot2023.commands.drivetrain.ResetPoseCommand
import com.team4099.robot2023.config.constants.FlywheelConstants
import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
import com.team4099.robot2023.subsystems.superstructure.Superstructure
Expand All @@ -23,9 +22,6 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru
addRequirements(drivetrain, superstructure)

addCommands(
ResetPoseCommand(
drivetrain, Pose2d(startingPose.x, startingPose.y, startingPose.rotation)
),
superstructure.scoreCommand(),
WaitCommand(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds),
ParallelCommandGroup(
Expand All @@ -50,7 +46,7 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru
)
)
},
keepTrapping = false
keepTrapping = true
),
WaitCommand(0.5).andThen(superstructure.groundIntakeCommand())
),
Expand All @@ -70,7 +66,7 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru
FieldWaypoint(
Translation2d(
(1.48.meters + 3.inches + 2.41.meters + 0.225.meters) / 2 +
0.25.meters,
0.25.meters,
4.87.meters
)
.translation2d,
Expand All @@ -86,7 +82,7 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru
FieldWaypoint(
Translation2d(
(1.48.meters + 3.inches + 2.41.meters + 0.225.meters) / 2 +
0.25.meters,
0.25.meters,
4.77.meters
)
.translation2d,
Expand All @@ -101,7 +97,7 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru
)
)
},
keepTrapping = false
keepTrapping = true
),
WaitCommand(1.0).andThen(superstructure.groundIntakeCommand())
),
Expand All @@ -121,7 +117,7 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru
FieldWaypoint(
Translation2d(
((1.48.meters + 3.inches) + (2.34.meters + 0.3.meters)) / 2 +
0.25.meters,
0.25.meters,
5.55.meters
)
.translation2d,
Expand All @@ -137,7 +133,7 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru
FieldWaypoint(
Translation2d(
((1.48.meters + 3.inches) + (2.34.meters + 0.3.meters)) / 2 +
0.25.meters,
0.25.meters,
5.45.meters
)
.translation2d,
Expand All @@ -152,7 +148,7 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru
) // Subwoofer
)
},
keepTrapping = false
keepTrapping = true
),
WaitCommand(0.5).andThen(superstructure.groundIntakeCommand())
),
Expand All @@ -164,4 +160,4 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru
companion object {
val startingPose = Pose2d(Translation2d(1.46.meters, 5.5.meters), 180.degrees)
}
}
}
88 changes: 86 additions & 2 deletions src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteMiddleCenterLine.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,9 +38,19 @@ class FourNoteMiddleCenterLine(val drivetrain: Drivetrain, val superstructure: S
null,
startingPose.rotation.inRotation2ds
),
FieldWaypoint(
Translation2d(2.0.meters, 5.525.meters).translation2d,
null,
180.degrees.inRotation2ds,
),
FieldWaypoint(
Translation2d(2.4.meters, 5.5.meters).translation2d,
0.degrees.inRotation2ds,
null,
180.degrees.inRotation2ds,
),
FieldWaypoint(
Translation2d(2.0.meters, 5.475.meters).translation2d,
null,
180.degrees.inRotation2ds,
),
FieldWaypoint(
Expand All @@ -54,7 +64,81 @@ class FourNoteMiddleCenterLine(val drivetrain: Drivetrain, val superstructure: S
),
WaitCommand(1.0)
.andThen(superstructure.groundIntakeCommand())
)
), superstructure.scoreCommand(),
WaitCommand(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds),
ParallelCommandGroup(
DrivePathCommand.createPathInFieldFrame(
drivetrain,
{
listOf(
FieldWaypoint(
startingPose.translation.translation2d,
null,
startingPose.rotation.inRotation2ds
),
FieldWaypoint(
Translation2d(4.87.meters, 4.12.meters).translation2d,
null,
180.degrees.inRotation2ds,
),
FieldWaypoint(
Translation2d(7.72.meters, 4.12.meters).translation2d,
null,
180.degrees.inRotation2ds,
),
FieldWaypoint(
Translation2d(4.87.meters, 4.12.meters).translation2d,
null,
180.degrees.inRotation2ds,
),
FieldWaypoint(
Translation2d(
1.48.meters + 3.inches, 5.5.meters).translation2d,
null,
180.degrees.inRotation2ds
)
)
}
),WaitCommand(1.0)
.andThen(superstructure.groundIntakeCommand())
), superstructure.scoreCommand(),
WaitCommand(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds),
ParallelCommandGroup(
DrivePathCommand.createPathInFieldFrame(
drivetrain,
{
listOf(
FieldWaypoint(
startingPose.translation.translation2d,
null,
startingPose.rotation.inRotation2ds
),
FieldWaypoint(
Translation2d(4.87.meters, 4.12.meters).translation2d,
null,
180.degrees.inRotation2ds,
),
FieldWaypoint(
Translation2d(7.72.meters, 2.47.meters).translation2d,
null,
180.degrees.inRotation2ds,
),
FieldWaypoint(
Translation2d(4.87.meters, 4.12.meters).translation2d,
null,
180.degrees.inRotation2ds,
),
FieldWaypoint(
Translation2d(
1.48.meters + 3.inches, 5.5.meters).translation2d,
null,
180.degrees.inRotation2ds
)
)
}
),WaitCommand(1.0)
.andThen(superstructure.groundIntakeCommand())
), superstructure.scoreCommand()
)
}
companion object {
Expand Down
2 changes: 1 addition & 1 deletion src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,7 @@ object Constants {

object Tuning {

const val TUNING_MODE = true
const val TUNING_MODE = false
const val DEBUGING_MODE = true
const val SIMULATE_DRIFT = false
const val DRIFT_CONSTANT = 0.001
Expand Down
5 changes: 3 additions & 2 deletions src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,7 @@ object FeederConstants {
val VOLTAGE_COMPENSATION = 12.0.volts
val FEEDER_CURRENT_LIMIT = 40.0.amps

val BEAM_BREAK_WAIT_TIME = 0.2.seconds
val BEAM_BREAK_WAIT_TIME = 0.3.seconds

val FEEDER_GEAR_RATIO = 24.0 / 12.0
val FEEDER_INERTIA = 0.0017672509.kilo.grams * 1.0.meters.squared
Expand All @@ -33,5 +33,6 @@ object FeederConstants {

val OUTTAKE_NOTE_VOLTAGE = (-6).volts

val beamBreakFilterTime = 0.04.seconds
val beamBreakFilterTime = 0.1.seconds

}
1 change: 1 addition & 0 deletions src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,6 +50,7 @@ import org.team4099.lib.units.perSecond
import java.util.concurrent.locks.Lock
import java.util.concurrent.locks.ReentrantLock
import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest
import com.team4099.robot2023.util.rotateBy

class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemBase() {
private val gyroNotConnectedAlert =
Expand Down
2 changes: 1 addition & 1 deletion src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -302,7 +302,7 @@ class Superstructure(
}
SuperstructureStates.GROUND_INTAKE_PREP -> {
wrist.currentRequest =
Request.WristRequest.TargetingPosition(Wrist.TunableWristStates.intakeAngle.get())
Request.WristRequest.OpenLoop(-2.volts)
if (wrist.isAtTargetedPosition) {
nextState = SuperstructureStates.GROUND_INTAKE
}
Expand Down

0 comments on commit b5ab4ea

Please sign in to comment.