specimen auto half finished

ftc13100 · Dec 23, 2024 · 88ffa0f · 88ffa0f
1 parent a50756b
commit 88ffa0f
Show file tree

Hide file tree

Showing 5 changed files with 76 additions and 24 deletions.
diff --git a/PathPlanning/src/main/kotlin/com/example/pathplanning/legacy/blue/main/BlueAutoLeft.kt b/PathPlanning/src/main/kotlin/com/example/pathplanning/legacy/blue/main/BlueAutoLeft.kt
@@ -19,28 +19,29 @@ object MeepMeepTesting {
 
         myBot.runAction(
             myBot.drive.actionBuilder(Pose2d(-10.0, 62.0, 90.0.toRadians()))
-                .setTangent(0.0)
-                .splineToLinearHeading(Pose2d(0.0, 35.0, 90.0.toRadians()), 90.0.toRadians())
+                .strafeToConstantHeading(Vector2d(2.0, 31.5))
                 .setTangent(90.0.toRadians())
-                .splineToSplineHeading(Pose2d(-34.0, 35.0, 0.0), -90.0.toRadians())
+                .splineToSplineHeading(Pose2d(-36.0, 35.0, 0.0), -90.0.toRadians())
                 .setTangent(-90.0.toRadians())
                 .splineToSplineHeading(Pose2d(-44.0, 5.0, 0.0.toRadians()), 120.0.toRadians())
                 .strafeTo(Vector2d(-44.0, 56.0))
                 .setTangent(-90.0.toRadians())
                 .splineToSplineHeading( Pose2d(-56.0, 5.0, 0.0.toRadians()), 130.0.toRadians())
                 .strafeTo(Vector2d(-56.0, 56.0))
                 .setTangent(-90.0.toRadians())
-                .splineToSplineHeading(Pose2d(-36.0, 59.0, 180.0.toRadians()), 0.0)
-                .setTangent(180.0.toRadians())
-                .splineToLinearHeading(Pose2d(0.0, 34.0, 90.0.toRadians()), -90.0.toRadians())
-                .setTangent(0.0.toRadians())
-                .splineToSplineHeading(Pose2d(-36.0, 59.0, 180.0.toRadians()), 0.0)
-                .setTangent(180.0.toRadians())
-                .splineToLinearHeading(Pose2d(0.0, 34.0, 90.0.toRadians()), -90.0.toRadians())
-                .setTangent(0.0.toRadians())
-                .splineToSplineHeading(Pose2d(-36.0, 59.0, 180.0.toRadians()), 0.0)
-                .setTangent(180.0.toRadians())
-                .splineToLinearHeading(Pose2d(0.0, 34.0, 90.0.toRadians()), -90.0.toRadians())
+                .splineToSplineHeading(Pose2d(-50.0, 50.0, 90.0.toRadians()), Math.toRadians(90.0))
+                .setTangent(90.0.toRadians())
+                .splineToSplineHeading(Pose2d(-36.0, 59.0, 180.0.toRadians()), Math.toRadians(180.0))
+//                .setTangent(180.0.toRadians())
+//                .splineToLinearHeading(Pose2d(0.0, 34.0, 90.0.toRadians()), -90.0.toRadians())
+//                .setTangent(0.0.toRadians())
+//                .splineToSplineHeading(Pose2d(-36.0, 59.0, 180.0.toRadians()), 0.0)
+//                .setTangent(180.0.toRadians())
+//                .splineToLinearHeading(Pose2d(0.0, 34.0, 90.0.toRadians()), -90.0.toRadians())
+//                .setTangent(0.0.toRadians())
+//                .splineToSplineHeading(Pose2d(-36.0, 59.0, 180.0.toRadians()), 0.0)
+//                .setTangent(180.0.toRadians())
+//                .splineToLinearHeading(Pose2d(0.0, 34.0, 90.0.toRadians()), -90.0.toRadians())
                 .build()
 
         )

diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/ActionCommand.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/ActionCommand.kt
@@ -24,6 +24,7 @@ class ActionCommand(
         action.preview(packet.fieldOverlay())
         finished = !action.run(packet)
 
+        packet.put("Finished", finished)
         FtcDashboard.getInstance().sendTelemetryPacket(packet)
     }
 

diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/Specimen1Plus3.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/Specimen1Plus3.kt
@@ -1,14 +1,18 @@
 package org.firstinspires.ftc.teamcode.opModes.auto
 
+import com.acmerobotics.roadrunner.Action
 import com.acmerobotics.roadrunner.Pose2d
+import com.acmerobotics.roadrunner.Vector2d
 import com.arcrobotics.ftclib.command.CommandOpMode
+import com.arcrobotics.ftclib.command.InstantCommand
 import com.arcrobotics.ftclib.command.ParallelCommandGroup
 import com.arcrobotics.ftclib.command.SequentialCommandGroup
 import com.arcrobotics.ftclib.command.WaitCommand
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous
 import org.firstinspires.ftc.teamcode.commands.ActionCommand
 import org.firstinspires.ftc.teamcode.commands.arm.ArmCommand
 import org.firstinspires.ftc.teamcode.commands.elevator.ElevatorCommand
+import org.firstinspires.ftc.teamcode.commands.intake.IntakeBeltCommand
 import org.firstinspires.ftc.teamcode.commands.intake.IntakeCommand
 import org.firstinspires.ftc.teamcode.constants.AutoStartPose
 import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem
@@ -26,25 +30,69 @@ class Specimen1Plus3 : CommandOpMode() {
 
         val scorePreloaded =
             SequentialCommandGroup(
+                InstantCommand({ DriveSubsystem.pose = AutoStartPose.BLUE_RIGHT.startPose }),
                 ParallelCommandGroup(
                     ActionCommand(
                         DriveSubsystem.actionBuilder(AutoStartPose.BLUE_RIGHT::startPose)
-                            .splineToLinearHeading(
-                                Pose2d(54.0, 56.0, Math.toRadians(225.0)),
-                                Math.toRadians(0.0)
-                            )
+                            .strafeToConstantHeading(Vector2d(2.0, 31.5))
                         .build(),
                         DriveSubsystem
                     ),
-                    ArmCommand(Math.toRadians(90.0), ArmSubsystem),
+                    ArmCommand(Math.toRadians(90.0), ArmSubsystem).withTimeout(2000),
+                    InstantCommand({ IntakeSubsystem.closeClaw() }),
                 ),
-                ElevatorCommand(20.0, ElevatorSubsystem),
-                IntakeCommand(false, IntakeSubsystem),
+                ElevatorCommand(12.0, ElevatorSubsystem).withTimeout(1250),
+                InstantCommand({ IntakeSubsystem.openClaw() }),
             )
 
+        val pushSamples =
+            SequentialCommandGroup(
+                ParallelCommandGroup(
+                    ActionCommand(
+                        DriveSubsystem.actionBuilder { Pose2d(2.0, 31.5, Math.toRadians(90.0)) }
+                            .setTangent(Math.toRadians(90.0))
+                            .splineToSplineHeading(Pose2d(-36.0, 35.0, 0.0), Math.toRadians(-90.0))
+                            .setTangent(Math.toRadians(-90.0))
+                            .splineToSplineHeading(Pose2d(-44.0, 5.0, 0.0), Math.toRadians(120.0))
+                            .strafeTo(Vector2d(-44.0, 56.0))
+                            .setTangent(Math.toRadians(-90.0))
+                            .splineToSplineHeading( Pose2d(-56.0, 5.0, 0.0), Math.toRadians(130.0))
+                            .strafeTo(Vector2d(-56.0, 56.0))
+                            .setTangent(Math.toRadians(-90.0))
+                            .splineToSplineHeading(Pose2d(-36.0, 59.0, Math.toRadians(180.0)), 0.0)
+                        .build(),
+                        DriveSubsystem
+                    ),
+                    ElevatorCommand(0.0, ElevatorSubsystem).withTimeout(1250),
+                    ArmCommand(0.0, ArmSubsystem).withTimeout(2000),
+                    IntakeBeltCommand(Math.toRadians(-60.0), IntakeSubsystem),
+                )
+            )
+
+        val scoreSpecimens =
+            SequentialCommandGroup(
+                ElevatorCommand(5.0, ElevatorSubsystem).withTimeout(500),
+                InstantCommand({IntakeSubsystem.closeClaw()}),
+                ParallelCommandGroup(
+                    ElevatorCommand(0.0, ElevatorSubsystem).withTimeout(500),
+                    ArmCommand(Math.toRadians(90.0), ArmSubsystem).withTimeout(2000),
+                    IntakeBeltCommand(Math.toRadians(90.0), IntakeSubsystem)
+                ),
+                ActionCommand(
+                    DriveSubsystem.actionBuilder { Pose2d(-36.0, 59.0, Math.toRadians(180.0)) }
+                        .setTangent(Math.toRadians(180.0))
+                        .splineToLinearHeading(Pose2d(-2.0, 31.5, Math.toRadians(90.0)), Math.toRadians(-90.0))
+                        .build(),
+                    DriveSubsystem,
+                )
+            )
 
         schedule(
-            scorePreloaded
+            scorePreloaded.andThen(
+                pushSamples.andThen(
+                    scoreSpecimens
+                )
+            )
         )
     }
 

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
@@ -135,6 +135,8 @@ class MainTeleOp : CommandOpMode() {
             telemetry.addData("Slides Position", ElevatorSubsystem.position)
             telemetry.addData("Slides Velocity", ElevatorSubsystem.velocity)
 
+            telemetry.addData("Intake Position", Math.toDegrees(IntakeSubsystem.position))
+
             telemetry.addData("Operator Mode", operatorMode)
             telemetry.addData("Driver Mode", operatorMode)
 

diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/intake/IntakeSubsystem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/intake/IntakeSubsystem.kt
@@ -23,7 +23,7 @@ object IntakeSubsystem : SubsystemBase() {
     private lateinit var intakeEncoder: Encoder
 
     val position
-        get() = intakeEncoder.getPositionAndVelocity().position / 8192.0 * 2 * PI - 1.82
+        get() = intakeEncoder.getPositionAndVelocity().position / 8192.0 * 2 * PI + 0.93
 
     val velocity
         get() = intakeEncoder.getPositionAndVelocity().velocity / 8192.0 * 2 * PI
@@ -70,7 +70,7 @@ object IntakeSubsystem : SubsystemBase() {
     }
 
     fun openClaw() {
-        claw.position = 0.2
+        claw.position = 0.1
     }
 
     fun wristUp() {