Skip to content

Commit

Permalink
added all cone auto command
Browse files Browse the repository at this point in the history
  • Loading branch information
@Shilab66
Shilab66 committed Dec 14, 2023
1 parent 623b871 commit a62ce75
Show file tree
Hide file tree
Showing 2 changed files with 207 additions and 1 deletion.
11 changes: 10 additions & 1 deletion src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@ import com.team4099.robot2023.auto.mode.ConeCubeLowOverChargeStationAuto
import com.team4099.robot2023.auto.mode.ConeCubeMobilityAuto
import com.team4099.robot2023.auto.mode.ConeCubeOverChargeStationAuto
import com.team4099.robot2023.auto.mode.ConeMobilityAuto
import com.team4099.robot2023.auto.mode.LimelightTestingAuto
import com.team4099.robot2023.auto.mode.PreloadOpenLoopChargeStationBalance
import com.team4099.robot2023.auto.mode.ScorePreloadCone
import com.team4099.robot2023.auto.mode.TestAutoPath
Expand Down Expand Up @@ -85,6 +86,10 @@ object AutonomousSelector {

autonomousModeChooser.addOption("1 Cone + Mobility", AutonomousMode.CONE_MOBILITY_AUTO)

autonomousModeChooser.addOption(
"LimeLight Testing", AutonomousMode.CONE_LIMELIGHT
)

// autonomousModeChooser.addOption("1 Cone + Open Loop Charge Station",
// AutonomousMode.CONE_MOBILITY_AUTO)

Expand Down Expand Up @@ -199,6 +204,9 @@ object AutonomousSelector {
return WaitCommand(waitTime.inSeconds)
.andThen(ScorePreloadCone(drivetrain, superstructure))
.andThen(engageCommand)
AutonomousMode.CONE_LIMELIGHT ->
return WaitCommand(waitTime.inSeconds)
.andThen(LimelightTestingAuto(drivetrain, superstructure))
else -> println("ERROR: unexpected auto mode: $mode")
}
return InstantCommand()
Expand All @@ -218,6 +226,7 @@ object AutonomousSelector {
CO_CU_LAUNCH_MIDDLE_AUTO,
CONE_MOBILITY_AUTO,
PRELOAD_SCORE_OPEN_LOOP_CHARGE_STATION_SCORE,
PRELOAD_SCORE_AUTO
PRELOAD_SCORE_AUTO,
CONE_LIMELIGHT
}
}
197 changes: 197 additions & 0 deletions src/main/kotlin/com/team4099/robot2023/auto/mode/LimelightTestingAuto.kt
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,197 @@
package com.team4099.robot2023.auto.mode

import com.team4099.lib.logging.LoggedTunableValue
import com.team4099.lib.trajectory.Waypoint
import com.team4099.robot2023.commands.drivetrain.DrivePathCommand
import com.team4099.robot2023.commands.drivetrain.ResetPoseCommand
import com.team4099.robot2023.config.constants.Constants
import com.team4099.robot2023.config.constants.FieldConstants
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.inMeters
import org.team4099.lib.units.base.meters
import org.team4099.lib.units.derived.degrees
import org.team4099.lib.units.derived.inDegrees
import org.team4099.lib.units.derived.inRotation2ds

class LimelightTestingAuto(val drivetrain: Drivetrain, val superstructure: Superstructure) :
SequentialCommandGroup() {

init {
addCommands(
ResetPoseCommand(
drivetrain, Pose2d(startingPosX.get(), startingPosY.get(), startingPosTheta.get())
),
ParallelCommandGroup(
DrivePathCommand(
drivetrain,
{
listOf(
Waypoint(
Translation2d(1.9.meters, 3.31.meters).translation2d,
null,
0.0.degrees.inRotation2ds
),
Waypoint(
Translation2d(2.9.meters, 4.8.meters).translation2d,
null,
0.0.degrees.inRotation2ds
),
Waypoint(
Translation2d(
FieldConstants.StagingLocations.translations[3]!!.x-intakeOffset.get(),
FieldConstants.StagingLocations.translations[3]!!.y-intakeOffset.get()
)
.translation2d,
0.0.degrees.inRotation2ds,
0.0.degrees.inRotation2ds
),
)
}
),
WaitCommand(1.5).andThen(superstructure.groundIntakeConeCommand())
),
ParallelCommandGroup(
WaitCommand(1.0).andThen(superstructure.requestIdleCommand()),
DrivePathCommand(
drivetrain,
{
listOf(
Waypoint(
Translation2d(
FieldConstants.StagingLocations.translations[3]!!.x- intakeOffset.get(),
FieldConstants.StagingLocations.translations[3]!!.y- intakeOffset.get()
)
.translation2d,
0.0.degrees.inRotation2ds,
0.0.degrees.inRotation2ds
),
Waypoint(
Translation2d(3.139.meters, 4.8.meters).translation2d,
null,
180.degrees.inRotation2ds
),
Waypoint(
Translation2d(
endingPosX.get(),
FieldConstants.Grids.nodeFirstY +
FieldConstants.Grids.nodeSeparationY * 7
)
.translation2d,
null,
180.0.degrees.inRotation2ds
)
)
},
keepTrapping = true
)
),
superstructure.prepScoreCommand(
Constants.Universal.GamePiece.CUBE, Constants.Universal.NodeTier.HIGH
),
superstructure.score(),
WaitCommand(0.5),
ParallelCommandGroup(
DrivePathCommand(
drivetrain,
{
listOf(
Waypoint(
Translation2d(1.9.meters, 3.31.meters).translation2d,
null,
0.0.degrees.inRotation2ds
),
Waypoint(
Translation2d(2.9.meters, 4.8.meters).translation2d,
null,
0.0.degrees.inRotation2ds
),
Waypoint(
Translation2d(
FieldConstants.StagingLocations.translations[2]!!.x-intakeOffset.get(),
FieldConstants.StagingLocations.translations[2]!!.y-intakeOffset.get()
)
.translation2d,
0.0.degrees.inRotation2ds,
0.0.degrees.inRotation2ds
),
)
}
),
WaitCommand(1.5).andThen(superstructure.groundIntakeConeCommand())
),
ParallelCommandGroup(
WaitCommand(1.0).andThen(superstructure.requestIdleCommand()),
DrivePathCommand(
drivetrain,
{
listOf(
Waypoint(
Translation2d(
FieldConstants.StagingLocations.translations[2]!!.x- intakeOffset.get(),
FieldConstants.StagingLocations.translations[2]!!.y- intakeOffset.get()
)
.translation2d,
0.0.degrees.inRotation2ds,
0.0.degrees.inRotation2ds
),
Waypoint(
Translation2d(3.139.meters, 4.8.meters).translation2d,
null,
180.degrees.inRotation2ds
),
Waypoint(
Translation2d(
endingPosX.get(),
FieldConstants.Grids.nodeFirstY +
FieldConstants.Grids.nodeSeparationY * 7
)
.translation2d,
null,
180.0.degrees.inRotation2ds
)
)
},
keepTrapping = true
)
)
)
}

companion object {
val startingPosX =
LoggedTunableValue(
"Drivetrain/startingPosX1", 1.9.meters, Pair({ it.inMeters }, { it.meters })
)
val startingPosY =
LoggedTunableValue(
"Drivetrain/startingPosY1", 3.31.meters, Pair({ it.inMeters }, { it.meters })
)
val startingPosTheta =
LoggedTunableValue(
"Drivetrain/startingPosTheta1", 0.0.degrees, Pair({ it.inDegrees }, { it.degrees })
)

val endingPosX =
LoggedTunableValue(
"Drivetrain/endingPosX1", 1.9.meters, Pair({ it.inMeters }, { it.meters })
)
val endingPosY =
LoggedTunableValue(
"Drivetrain/endingPosY1", 3.31.meters, Pair({ it.inMeters }, { it.meters })
)
val endingPosTheta =
LoggedTunableValue(
"Drivetrain/endingPosTheta1", 180.0.degrees, Pair({ it.inDegrees }, { it.degrees })
)
val intakeOffset=
LoggedTunableValue(
"Drivetrain/intakeOffset", 0.0.meters, Pair({ it.inMeters }, { it.meters })
)
}
}

0 comments on commit a62ce75

Please sign in to comment.