Skip to content

Commit

Permalink
ishaan use this
Browse files Browse the repository at this point in the history
  • Loading branch information
@aravioli108
aravioli108 committed Dec 27, 2024
1 parent 322ab39 commit 3d80ea1
Show file tree
Hide file tree
Showing 8 changed files with 176 additions and 54 deletions.
10 changes: 5 additions & 5 deletions ...Planning/src/main/kotlin/com/example/pathplanning/legacy/blue/main/Sample1Plus3Testing.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,14 +18,14 @@ object Sample1Plus3Testing {
.build()

myBot.runAction(
myBot.drive.actionBuilder(Pose2d(10.0, 62.0, -90.0.toRadians()))
myBot.drive.actionBuilder(Pose2d(38.0, 62.0, -90.0.toRadians()))
.setTangent(-45.0)
.splineToLinearHeading(Pose2d(56.0, 56.0, Math.toRadians(225.0)), Math.toRadians(45.0))
.turn(Math.toRadians(30.0))
.turn(Math.toRadians(-30.0))
.turn(Math.toRadians(55.0))
.turn(Math.toRadians(-55.0))
.turn(Math.toRadians(75.0))
// .turn(Math.toRadians(-30.0))
// .turn(Math.toRadians(55.0))
// .turn(Math.toRadians(-55.0))
// .turn(Math.toRadians(75.0))
.build()

)
Expand Down
2 changes: 1 addition & 1 deletion ...anning/src/main/kotlin/com/example/pathplanning/legacy/blue/main/Specimen1Plus3Testing.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@ object Specimen1Plus3Testing {

val myBot =
DefaultBotBuilder(meepMeep) // Set bot constraints: maxVel, maxAccel, maxAngVel, maxAngAccel, track width
.setConstraints(50.0, 50.0, Math.toRadians(180.0), Math.toRadians(180.0), 4669.698132939092 * 0.002953423066)
.setConstraints(70.0, 70.0, Math.toRadians(180.0), Math.toRadians(180.0), 4669.698132939092 * 0.00297208937)
.build()

myBot.runAction(
Expand Down
2 changes: 1 addition & 1 deletion TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/AutoStartPose.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@ import com.acmerobotics.roadrunner.Pose2d
enum class AutoStartPose(val startPose: Pose2d) {
BLUE_LEFT(
Pose2d(
10.0, 62.0, Math.toRadians(-90.0)
38.0, 62.0, Math.toRadians(-90.0)
)
),

Expand Down
135 changes: 129 additions & 6 deletions TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/Sample1Plus3.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,17 +8,21 @@ 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.hardware.ams.AMSColorSensor.Wait
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.commands.intake.ThrowItBackCommand
import org.firstinspires.ftc.teamcode.constants.AutoStartPose
import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem
import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem
import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem
import java.time.Instant
import java.util.Vector

@Autonomous(name = "Sample 1 + 3")
class Sample1Plus3 : CommandOpMode() {
Expand All @@ -35,20 +39,139 @@ class Sample1Plus3 : CommandOpMode() {
ActionCommand(
DriveSubsystem.actionBuilder(AutoStartPose.BLUE_LEFT::startPose)
.setTangent(-45.0)
.splineToLinearHeading(Pose2d(56.0, 56.0, Math.toRadians(225.0)), Math.toRadians(45.0)) .build(),
.splineToLinearHeading(Pose2d(55.0, 55.0, Math.toRadians(225.0)), Math.toRadians(45.0)) .build(),
DriveSubsystem
),
ArmCommand(Math.toRadians(90.0), ArmSubsystem).withTimeout(2000),
IntakeBeltCommand(Math.toRadians(-60.0),IntakeSubsystem),
ArmCommand(Math.toRadians(91.0), ArmSubsystem).withTimeout(2500),
InstantCommand({ IntakeSubsystem.closeClaw() }),
),
ElevatorCommand(30.0, ElevatorSubsystem).withTimeout(2000),
IntakeBeltCommand(Math.toRadians(90.0), IntakeSubsystem).withTimeout(1000),
ElevatorCommand(31.0, ElevatorSubsystem).withTimeout(2000),
IntakeBeltCommand(Math.toRadians(70.0), IntakeSubsystem).withTimeout(500),
InstantCommand({ IntakeSubsystem.openClaw() }),
WaitCommand(500),
)


val scoreSample1 =
SequentialCommandGroup(
ParallelCommandGroup(
IntakeBeltCommand(0.0, IntakeSubsystem).withTimeout(500),
ElevatorCommand(0.0, ElevatorSubsystem).withTimeout(1000),
),
ArmCommand(0.0, ArmSubsystem).withTimeout(2000),
ParallelCommandGroup(
ActionCommand(
DriveSubsystem.actionBuilder{Pose2d(56.0, 56.0, Math.toRadians(225.0))}
.turn(Math.toRadians(35.0))
.strafeTo(Vector2d(54.0, 53.0))
.build()
),
ElevatorCommand(24.0, ElevatorSubsystem).withTimeout(1000),
),
IntakeBeltCommand(Math.toRadians(-65.0), IntakeSubsystem).withTimeout(700),
WaitCommand(250),
InstantCommand({IntakeSubsystem.closeClaw()}),
WaitCommand(250),
ParallelCommandGroup(
ThrowItBackCommand(IntakeSubsystem).withTimeout(700),
ElevatorCommand(0.0, ElevatorSubsystem).withTimeout(1000),
ActionCommand(
DriveSubsystem.actionBuilder{Pose2d(54.0, 53.0, Math.toRadians(260.0))}
.turn(Math.toRadians(-35.0))
.strafeTo(Vector2d(55.0, 55.0))
.build()
),
),
ArmCommand(Math.toRadians(91.0), ArmSubsystem).withTimeout(2500),
ElevatorCommand(31.0, ElevatorSubsystem).withTimeout(1000),
WaitCommand(100),
InstantCommand({IntakeSubsystem.openClaw()})
)

val scoreSample2 =
SequentialCommandGroup(
ParallelCommandGroup(
IntakeBeltCommand(0.0, IntakeSubsystem).withTimeout(500),
ElevatorCommand(0.0, ElevatorSubsystem).withTimeout(1000),
),
ArmCommand(0.0, ArmSubsystem).withTimeout(2000),
ParallelCommandGroup(
ActionCommand(
DriveSubsystem.actionBuilder{Pose2d(56.0, 56.0, Math.toRadians(225.0))}
.turn(Math.toRadians(55.0))
.strafeTo(Vector2d(56.0, 53.0))
.build()
),
ElevatorCommand(24.0, ElevatorSubsystem).withTimeout(1000),
ThrowItBackCommand(IntakeSubsystem),
),
IntakeBeltCommand(Math.toRadians(-65.0), IntakeSubsystem).withTimeout(700),
WaitCommand(100),
InstantCommand({IntakeSubsystem.closeClaw()}),
WaitCommand(250),
ParallelCommandGroup(
ThrowItBackCommand(IntakeSubsystem).withTimeout(700),
ElevatorCommand(0.0, ElevatorSubsystem).withTimeout(1000),
ActionCommand(
DriveSubsystem.actionBuilder{Pose2d(57.0, 53.0, Math.toRadians(280.0))}
.turn(Math.toRadians(-55.0))
.strafeTo(Vector2d(55.0, 55.0))
.build()
),
),
ArmCommand(Math.toRadians(91.0), ArmSubsystem).withTimeout(2000),
ElevatorCommand(31.0, ElevatorSubsystem).withTimeout(1000),
IntakeBeltCommand(Math.toRadians(70.0), IntakeSubsystem).withTimeout(500),
WaitCommand(100),
InstantCommand({IntakeSubsystem.openClaw()}),
WaitCommand(500),
)

val scoreSample3 =
SequentialCommandGroup(
ParallelCommandGroup(
IntakeBeltCommand(0.0, IntakeSubsystem).withTimeout(500),
ElevatorCommand(0.0, ElevatorSubsystem).withTimeout(1000),
),
ArmCommand(0.0, ArmSubsystem).withTimeout(2000),
ParallelCommandGroup(
ActionCommand(
DriveSubsystem.actionBuilder{Pose2d(56.0, 56.0, Math.toRadians(225.0))}
.turn(Math.toRadians(70.0))
.build()
),
ElevatorCommand(24.0, ElevatorSubsystem).withTimeout(1000),
ThrowItBackCommand(IntakeSubsystem),
),
IntakeBeltCommand(Math.toRadians(-50.0), IntakeSubsystem).withTimeout(700),
WaitCommand(100),
InstantCommand({IntakeSubsystem.closeClaw()}),
WaitCommand(250),
ParallelCommandGroup(
ThrowItBackCommand(IntakeSubsystem).withTimeout(700),
ElevatorCommand(0.0, ElevatorSubsystem).withTimeout(1000),
ActionCommand(
DriveSubsystem.actionBuilder{Pose2d(56.0, 56.0, Math.toRadians(295.0))}
.turn(Math.toRadians(-70.0))
.build()
),
),
ArmCommand(Math.toRadians(91.0), ArmSubsystem).withTimeout(2000),
ElevatorCommand(31.0, ElevatorSubsystem).withTimeout(1000),
WaitCommand(100),
InstantCommand({IntakeSubsystem.openClaw()}),
WaitCommand(500),
ElevatorCommand(0.0, ElevatorSubsystem).withTimeout(1000),
ArmCommand(0.0, ArmSubsystem)
)

schedule(
scorePreloaded
SequentialCommandGroup(
scorePreloaded,
scoreSample1,
scoreSample2,
// scoreSample3
)
)
}

Expand Down
52 changes: 26 additions & 26 deletions TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/Specimen1Plus3.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@ 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.ThrowItBackCommand
import org.firstinspires.ftc.teamcode.constants.AutoStartPose
import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem
import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
Expand All @@ -32,14 +33,14 @@ class Specimen1Plus3 : CommandOpMode() {
ParallelCommandGroup(
ActionCommand(
DriveSubsystem.actionBuilder(AutoStartPose.BLUE_RIGHT::startPose)
.strafeToConstantHeading(Vector2d(2.0, 31.5))
.strafeToConstantHeading(Vector2d(2.0, 32.0))
.build(),
DriveSubsystem
),
ArmCommand(Math.toRadians(90.0), ArmSubsystem).withTimeout(2000),
InstantCommand({ IntakeSubsystem.closeClaw() }),
),
ElevatorCommand(12.0, ElevatorSubsystem).withTimeout(850),
ElevatorCommand(11.0, ElevatorSubsystem).withTimeout(600),
InstantCommand({ IntakeSubsystem.openClaw() }),
)

Expand All @@ -48,7 +49,7 @@ class Specimen1Plus3 : CommandOpMode() {
SequentialCommandGroup(
ParallelCommandGroup(
ActionCommand(
DriveSubsystem.actionBuilder { Pose2d(2.0, 31.5, Math.toRadians(90.0)) }
DriveSubsystem.actionBuilder { Pose2d(2.0, 32.0, Math.toRadians(90.0)) }
.setTangent(Math.toRadians(90.0))
.splineToLinearHeading(
Pose2d(-36.0, 35.0, Math.toRadians(90.0)),
Expand All @@ -68,7 +69,6 @@ class Specimen1Plus3 : CommandOpMode() {
Math.toRadians(130.0)
)
.strafeTo(Vector2d(-58.0, 56.0))

//pick up
.setTangent(Math.toRadians(-90.0))
.splineToLinearHeading(
Expand All @@ -80,7 +80,7 @@ class Specimen1Plus3 : CommandOpMode() {
.build(),
DriveSubsystem
),
ElevatorCommand(0.0, ElevatorSubsystem).withTimeout(850),
ElevatorCommand(0.0, ElevatorSubsystem).withTimeout(800),
ArmCommand(0.0, ArmSubsystem).withTimeout(2000),
)
)
Expand All @@ -91,29 +91,29 @@ class Specimen1Plus3 : CommandOpMode() {
IntakeBeltCommand(Math.toRadians(-60.0), IntakeSubsystem).withTimeout(500),
ElevatorCommand(8.0, ElevatorSubsystem).withTimeout(500),
),
WaitCommand(750),
WaitCommand(100),
InstantCommand({ IntakeSubsystem.closeClaw() }),
WaitCommand(500),
ParallelCommandGroup(
ElevatorCommand(0.0, ElevatorSubsystem).withTimeout(500),
ArmCommand(Math.toRadians(90.0), ArmSubsystem).withTimeout(2000),
IntakeBeltCommand(Math.toRadians(90.0), IntakeSubsystem).withTimeout(500),
ThrowItBackCommand(IntakeSubsystem).withTimeout(500),
ActionCommand(
DriveSubsystem.actionBuilder { Pose2d(-36.0, 59.0, Math.toRadians(180.0)) }
DriveSubsystem.actionBuilder { Pose2d(-34.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))
.splineToLinearHeading(Pose2d(-2.0, 32.0, Math.toRadians(90.0)), Math.toRadians(-90.0))
.build(),
DriveSubsystem,
)
),
ElevatorCommand(12.0, ElevatorSubsystem).withTimeout(1000),
ElevatorCommand(11.0, ElevatorSubsystem).withTimeout(700),
InstantCommand({ IntakeSubsystem.openClaw() }),
ParallelCommandGroup(
ArmCommand(0.0, ArmSubsystem).withTimeout(2000),
ElevatorCommand(0.0, ElevatorSubsystem).withTimeout(1000),
ActionCommand(
DriveSubsystem.actionBuilder { Pose2d(-2.0, 31.5, Math.toRadians(90.0)) }
.splineToLinearHeading(Pose2d(-36.0, 59.0, Math.toRadians(180.0)), 0.0)
DriveSubsystem.actionBuilder { Pose2d(-2.0, 32.0, Math.toRadians(90.0)) }
.splineToLinearHeading(Pose2d(-34.0, 59.0, Math.toRadians(180.0)), 0.0)
.build()
),
)
Expand All @@ -124,29 +124,29 @@ class Specimen1Plus3 : CommandOpMode() {
IntakeBeltCommand(Math.toRadians(-60.0), IntakeSubsystem).withTimeout(500),
ElevatorCommand(8.0, ElevatorSubsystem).withTimeout(500),
),
WaitCommand(750),
WaitCommand(100),
InstantCommand({ IntakeSubsystem.closeClaw() }),
WaitCommand(500),
ParallelCommandGroup(
ElevatorCommand(0.0, ElevatorSubsystem).withTimeout(500),
ArmCommand(Math.toRadians(90.0), ArmSubsystem).withTimeout(2000),
IntakeBeltCommand(Math.toRadians(90.0), IntakeSubsystem).withTimeout(500),
ThrowItBackCommand(IntakeSubsystem).withTimeout(500),
ActionCommand(
DriveSubsystem.actionBuilder { Pose2d(-36.0, 59.0, Math.toRadians(180.0)) }
DriveSubsystem.actionBuilder { Pose2d(-34.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))
.splineToLinearHeading(Pose2d(-4.0, 32.0, Math.toRadians(90.0)), Math.toRadians(-90.0))
.build(),
DriveSubsystem,
)
),
ElevatorCommand(12.0, ElevatorSubsystem).withTimeout(1000),
ElevatorCommand(11.0, ElevatorSubsystem).withTimeout(700),
InstantCommand({ IntakeSubsystem.openClaw() }),
ParallelCommandGroup(
ArmCommand(0.0, ArmSubsystem).withTimeout(2000),
ElevatorCommand(0.0, ElevatorSubsystem).withTimeout(1000),
ActionCommand(
DriveSubsystem.actionBuilder { Pose2d(-2.0, 31.5, Math.toRadians(90.0)) }
.splineToLinearHeading(Pose2d(-36.0, 59.0, Math.toRadians(180.0)), 0.0)
DriveSubsystem.actionBuilder { Pose2d(-4.0, 32.0, Math.toRadians(90.0)) }
.splineToLinearHeading(Pose2d(-34.0, 59.0, Math.toRadians(180.0)), 0.0)
.build()
),
)
Expand All @@ -157,29 +157,29 @@ class Specimen1Plus3 : CommandOpMode() {
IntakeBeltCommand(Math.toRadians(-60.0), IntakeSubsystem).withTimeout(500),
ElevatorCommand(8.0, ElevatorSubsystem).withTimeout(500),
),
WaitCommand(750),
WaitCommand(100),
InstantCommand({ IntakeSubsystem.closeClaw() }),
WaitCommand(500),
ParallelCommandGroup(
ElevatorCommand(0.0, ElevatorSubsystem).withTimeout(500),
ArmCommand(Math.toRadians(90.0), ArmSubsystem).withTimeout(2000),
IntakeBeltCommand(Math.toRadians(90.0), IntakeSubsystem).withTimeout(500),
ThrowItBackCommand(IntakeSubsystem).withTimeout(700),
ActionCommand(
DriveSubsystem.actionBuilder { Pose2d(-36.0, 59.0, Math.toRadians(180.0)) }
DriveSubsystem.actionBuilder { Pose2d(-34.0, 59.0, Math.toRadians(180.0)) }
.setTangent(Math.toRadians(180.0))
.splineToLinearHeading(Pose2d(-4.0, 31.5, Math.toRadians(90.0)), Math.toRadians(-90.0))
.splineToLinearHeading(Pose2d(-6.0, 32.0, Math.toRadians(90.0)), Math.toRadians(-90.0))
.build(),
DriveSubsystem,
)
),
ElevatorCommand(12.0, ElevatorSubsystem).withTimeout(1000),
ElevatorCommand(11.0, ElevatorSubsystem).withTimeout(700),
InstantCommand({ IntakeSubsystem.openClaw() }),
ParallelCommandGroup(
ArmCommand(0.0, ArmSubsystem).withTimeout(2000),
ElevatorCommand(0.0, ElevatorSubsystem).withTimeout(1000),
ActionCommand(
DriveSubsystem.actionBuilder { Pose2d(-6.0, 31.5, Math.toRadians(90.0)) }
.splineToLinearHeading(Pose2d(-36.0, 59.0, Math.toRadians(180.0)), 0.0)
DriveSubsystem.actionBuilder { Pose2d(-6.0, 32.0, Math.toRadians(90.0)) }
.splineToLinearHeading(Pose2d(-34.0, 59.0, Math.toRadians(180.0)), 0.0)
.build()
),
)
Expand Down
Loading

0 comments on commit 3d80ea1

Please sign in to comment.