diff --git a/PathPlanning/src/main/kotlin/com/example/pathplanning/blue/basket/BlueHighBasket2.kt b/PathPlanning/src/main/kotlin/com/example/pathplanning/blue/basket/BlueHighBasket2.kt index f2a4b8a..869bf51 100644 --- a/PathPlanning/src/main/kotlin/com/example/pathplanning/blue/basket/BlueHighBasket2.kt +++ b/PathPlanning/src/main/kotlin/com/example/pathplanning/blue/basket/BlueHighBasket2.kt @@ -4,10 +4,6 @@ import com.acmerobotics.roadrunner.geometry.Pose2d import com.noahbres.meepmeep.MeepMeep import com.noahbres.meepmeep.core.toRadians import com.noahbres.meepmeep.roadrunner.DefaultBotBuilder -import java.awt.Image -import java.io.File -import java.io.IOException -import javax.imageio.ImageIO object BlueHighBasket2 { @JvmStatic @@ -23,19 +19,10 @@ object BlueHighBasket2 { .waitSeconds(2.0) .build() } - var img: Image? = null - try { - img = ImageIO.read(File("/Users/ishaanghaskadbi/Downloads/field.png")) - } catch (_ : IOException) { - - } - - if (img != null) { - meepMeep.setBackground(img) - .setDarkMode(true) - .setBackgroundAlpha(0.95f) - .addEntity(myBot) - .start() - } + meepMeep.setBackground(MeepMeep.Background.FIELD_CENTERSTAGE_JUICE_DARK) + .setDarkMode(true) + .setBackgroundAlpha(0.95f) + .addEntity(myBot) + .start() } } \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/SpinDownCommand.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/SpinDownCommand.kt index e6b99a8..695a894 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/SpinDownCommand.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/SpinDownCommand.kt @@ -5,10 +5,10 @@ package org.firstinspires.ftc.teamcode.commands.elevator >>>>>>>> 509c48b (Arm PID infra:):TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/SpinDownCommand.kt import com.arcrobotics.ftclib.command.CommandBase -import org.firstinspires.ftc.teamcode.subsystems.slides.SlidesSubsystem +import org.firstinspires.ftc.teamcode.subsystems.slides.OpenSlidesSubsystem class SpinDownCommand( - private val subsystem: SlidesSubsystem + private val subsystem: OpenSlidesSubsystem ) : CommandBase() { diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/SpinUpCommand.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/SpinUpCommand.kt index abe5bde..10f87e8 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/SpinUpCommand.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/SpinUpCommand.kt @@ -5,10 +5,10 @@ package org.firstinspires.ftc.teamcode.commands.elevator >>>>>>>> 509c48b (Arm PID infra:):TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/SpinUpCommand.kt import com.arcrobotics.ftclib.command.CommandBase -import org.firstinspires.ftc.teamcode.subsystems.slides.SlidesSubsystem +import org.firstinspires.ftc.teamcode.subsystems.slides.OpenSlidesSubsystem class SpinUpCommand( - private val subsystem: SlidesSubsystem + private val subsystem: OpenSlidesSubsystem ) : CommandBase() { override fun execute() { diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/BlueHighBasetLeft.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/BlueHighBasetLeft.kt new file mode 100644 index 0000000..1285eb6 --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/BlueHighBasetLeft.kt @@ -0,0 +1,72 @@ +package org.firstinspires.ftc.teamcode.opModes.auto.blue.basket + +import com.arcrobotics.ftclib.hardware.motors.CRServo +import com.arcrobotics.ftclib.hardware.motors.Motor +import com.qualcomm.robotcore.eventloop.opmode.Autonomous +import com.qualcomm.robotcore.eventloop.opmode.OpMode +import org.firstinspires.ftc.teamcode.constants.AutoStartPose +import org.firstinspires.ftc.teamcode.constants.ControlBoard +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.SlidesSubsytem + +@Autonomous +class BlueHighBasetLeft() : OpMode() { + private lateinit var armLeft: Motor + private lateinit var armRight: Motor + private lateinit var elevatorLeft: Motor + private lateinit var elevatorRight: Motor + + private lateinit var intake: CRServo + + private lateinit var driveSubsystem: DriveSubsystem + private lateinit var intakeSubsystem: IntakeSubsystem + private lateinit var armSubsystem: ArmSubsystem + private lateinit var elevatorSubsystem: SlidesSubsytem + + override fun init() { + intake = CRServo(hardwareMap, "intake") + + armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName) + armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName) + + elevatorLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName) + elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName) + + driveSubsystem = DriveSubsystem(hardwareMap) + intakeSubsystem = IntakeSubsystem(intake) + armSubsystem = ArmSubsystem(armLeft, armRight) + elevatorSubsystem = SlidesSubsytem(elevatorLeft, elevatorRight) + + val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_LEFT.startPose) + .turn(Math.toRadians(90.0)) + .forward(60.0) + .addTemporalMarker(2.0) { + armSubsystem.setpoint = Math.toRadians(150.0) + } + .waitSeconds(1.0) + .addTemporalMarker(3.0) { + elevatorSubsystem.setpoint = 2.0 + } + .waitSeconds(2.0) + .addTemporalMarker(5.0){ + intakeSubsystem.outtake() + } + .waitSeconds(1.0) + .addTemporalMarker(6.0) { + elevatorSubsystem.setpoint = 0.0 + } + .waitSeconds(2.0) + .addTemporalMarker(8.0) { + armSubsystem.setpoint = Math.toRadians(0.0) + } + .build() + + driveSubsystem.followTrajectorySequenceAsync(trajectory) + } + + override fun loop() { + driveSubsystem.update() + } +} \ No newline at end of file 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 index 177b696..1208843 100644 --- 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 @@ -13,7 +13,7 @@ import org.firstinspires.ftc.teamcode.commands.elevator.SpinUpCommand import org.firstinspires.ftc.teamcode.constants.ControlBoard import org.firstinspires.ftc.teamcode.subsystems.arm.OpenArmSubsystem import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem -import org.firstinspires.ftc.teamcode.subsystems.slides.SlidesSubsystem +import org.firstinspires.ftc.teamcode.subsystems.slides.OpenSlidesSubsystem @TeleOp class MainTeleOp : CommandOpMode() { @@ -22,7 +22,7 @@ class MainTeleOp : CommandOpMode() { private lateinit var armLeft: Motor private lateinit var armRight: Motor - private lateinit var slidesSubsystem: SlidesSubsystem + private lateinit var slidesSubsystem: OpenSlidesSubsystem private lateinit var armSubsystem: OpenArmSubsystem private lateinit var driveSubsystem: DriveSubsystem @@ -45,7 +45,7 @@ class MainTeleOp : CommandOpMode() { armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName, Motor.GoBILDA.RPM_435) armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName, Motor.GoBILDA.RPM_435) - slidesSubsystem = SlidesSubsystem(elevatorRight, elevatorLeft) + slidesSubsystem = OpenSlidesSubsystem(elevatorRight, elevatorLeft) armSubsystem = OpenArmSubsystem(armRight, armLeft) driveSubsystem = DriveSubsystem(hardwareMap) diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/SlidesSubsystem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/OpenSlidesSubsystem.kt similarity index 96% rename from TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/SlidesSubsystem.kt rename to TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/OpenSlidesSubsystem.kt index 34734be..82caf39 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/SlidesSubsystem.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/OpenSlidesSubsystem.kt @@ -4,7 +4,7 @@ import com.arcrobotics.ftclib.command.SubsystemBase import com.arcrobotics.ftclib.hardware.motors.Motor import com.arcrobotics.ftclib.hardware.motors.MotorGroup -class SlidesSubsystem( +class OpenSlidesSubsystem( //sets them as a private variable thats a motor (same name as in driver hub) private val elevatorLeft: Motor, private val elevatorRight: Motor, diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/SlidesSubsytemPID.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/SlidesSubsytem.kt similarity index 98% rename from TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/SlidesSubsytemPID.kt rename to TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/SlidesSubsytem.kt index 0ea55b2..38ff79b 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/SlidesSubsytemPID.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/SlidesSubsytem.kt @@ -9,7 +9,7 @@ import org.firstinspires.ftc.teamcode.utils.PIDSubsystem import kotlin.math.PI @Config -class SlidesSubsytemPID( +class SlidesSubsytem( elevatorLeft : Motor, elevatorRight : Motor ) : PIDSubsystem(