Hang yourself

ftc13100 · Dec 8, 2024 · 8d66e18 · 8d66e18
1 parent 1e2dc86
commit 8d66e18
Show file tree

Hide file tree

Showing 3 changed files with 32 additions and 5 deletions.
diff --git a/.../main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/Blue1Plus2V2.kt b/.../main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/Blue1Plus2V2.kt
@@ -14,6 +14,7 @@ import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
 import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeBeltSubsystem
 import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem
 import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem
+import org.firstinspires.ftc.teamcode.utils.roadrunner.trajectorysequence.TrajectorySequence
 
 @Autonomous(name = "V2 Blue 1 + 2", group = "Basket", preselectTeleOp = "MainTeleOp")
 class Blue1Plus2V2: OpMode() {
@@ -31,6 +32,9 @@ class Blue1Plus2V2: OpMode() {
     private lateinit var armSubsystem: ArmSubsystem
     private lateinit var elevatorSubsystem: ElevatorSubsystem
 
+    private lateinit var toBasket1: TrajectorySequence
+    private lateinit var toBasket2: TrajectorySequence
+
     override fun init() {
         telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry)
 
@@ -49,7 +53,7 @@ class Blue1Plus2V2: OpMode() {
         armSubsystem = ArmSubsystem(armRight, armLeft)
         elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft, armSubsystem::armAngle)
 
-        val toBasket1 = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_LEFT.startPose)
+        toBasket1 = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_LEFT.startPose)
             .splineToLinearHeading(Pose2d(54.0, 56.0, Math.toRadians(225.0)), Math.toRadians(0.0))
             .addTemporalMarker(0.1) {
                 intakeSubsystem.intake()
@@ -175,6 +179,9 @@ class Blue1Plus2V2: OpMode() {
             }
             .splineToSplineHeading(Pose2d(28.0, 10.0, Math.toRadians(0.0)), 90.0)
             .back(5.0)
+            .addDisplacementMarker {
+                driveSubsystem.followTrajectorySequenceAsync(toBasket2)
+            }
             .build()
         driveSubsystem.poseEstimate = AutoStartPose.BLUE_LEFT.startPose
         driveSubsystem.followTrajectorySequenceAsync(toBasket1)

diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/teleOp/MainTeleOp.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/teleOp/MainTeleOp.kt
@@ -130,6 +130,16 @@ class MainTeleOp : CommandOpMode() {
             InstantCommand({ intakeBeltSubsystem.decreasePos() })
         )
 
+        driver.getGamepadButton(GamepadKeys.Button.Y)
+            .whenPressed(
+                RunCommand({
+                    slidesSubsystem.disable()
+                    slidesSubsystem.spinDown()
+                    armSubsystem.anticlockwise()
+                })
+            )
+
+
         driveSubsystem.defaultCommand = driveCommand
 
         RunCommand({

diff --git a/...ode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/ElevatorSubsystem.kt b/...ode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/ElevatorSubsystem.kt
@@ -7,14 +7,12 @@ import com.arcrobotics.ftclib.hardware.motors.Motor
 import com.arcrobotics.ftclib.hardware.motors.MotorGroup
 import com.arcrobotics.ftclib.trajectory.TrapezoidProfile
 import com.arcrobotics.ftclib.util.MathUtils.clamp
-import com.qualcomm.robotcore.hardware.DcMotor
-import com.qualcomm.robotcore.hardware.DcMotorEx
 import org.firstinspires.ftc.teamcode.constants.SlidesConstants
 import kotlin.math.sin
 
 @Config
 class ElevatorSubsystem(
-    private val elevatorRight : Motor,
+    elevatorRight : Motor,
     elevatorLeft : Motor,
     private val slideAngle: () -> Double
 ) : SubsystemBase() {
@@ -51,8 +49,12 @@ class ElevatorSubsystem(
     init {
         elevatorRight.inverted = true
         elevatorRight.encoder.setDirection(Motor.Direction.REVERSE)
+
         extendMotors.resetEncoder()
         extendMotors.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE)
+
+        setpoint = 0.0
+        enabled = true
     }
 
     override fun periodic() {
@@ -68,8 +70,16 @@ class ElevatorSubsystem(
         enabled != enabled
     }
 
+    fun disable() {
+        enabled = false
+    }
+
     fun spinUp() {
-        extendMotors.set(1.0);
+        extendMotors.set(1.0)
+    }
+
+    fun spinDown() {
+        extendMotors.set(-1.0)
     }
 
     fun stop() {