From 16a0d1c57b3e5cac41af691183d2bb3259ac3446 Mon Sep 17 00:00:00 2001 From: Ishaan Ghaskadbi Date: Sun, 1 Dec 2024 14:56:33 -0500 Subject: [PATCH] part of auto --- .../pathplanning/blue/main/BlueAuto.kt | 7 +- .../auto/blue/basket/left/Blue1Plus1.kt | 102 ++++++++++-------- .../ftc/teamcode/opModes/teleOp/MainTeleOp.kt | 25 +++-- .../teamcode/opModes/tuning/IntakeTuner.kt | 8 +- .../subsystems/intake/IntakeBeltSubsystem.kt | 4 +- .../subsystems/intake/IntakeSubsystem.kt | 4 +- .../subsystems/slides/ElevatorSubsystem.kt | 10 +- 7 files changed, 90 insertions(+), 70 deletions(-) diff --git a/PathPlanning/src/main/kotlin/com/example/pathplanning/blue/main/BlueAuto.kt b/PathPlanning/src/main/kotlin/com/example/pathplanning/blue/main/BlueAuto.kt index eaf5203..2ee8232 100644 --- a/PathPlanning/src/main/kotlin/com/example/pathplanning/blue/main/BlueAuto.kt +++ b/PathPlanning/src/main/kotlin/com/example/pathplanning/blue/main/BlueAuto.kt @@ -2,7 +2,6 @@ package com.example.pathplanning.blue.main 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 @@ -19,13 +18,15 @@ object BlueAuto { DefaultBotBuilder(meepMeep) // Set bot constraints: maxVel, maxAccel, maxAngVel, maxAngAccel, track width .setConstraints(30.0, 30.0, Math.toRadians(170.0), Math.toRadians(170.0), 13.94) .followTrajectorySequence { drive -> - drive.trajectorySequenceBuilder(Pose2d(-12.0, 62.0, (270.0).toRadians())) + drive.trajectorySequenceBuilder(Pose2d( + -12.0, 62.0, Math.toRadians(270.0) + )) .addTemporalMarker(0.1,) { //add the command for the arm to go up //add command for belt servo to go backwards //add the command to add the slides go up } - .splineToLinearHeading(Pose2d(55.0, 55.0, Math.toRadians(225.0)), Math.toRadians(0.0)) + .splineToLinearHeading(Pose2d(53.0, 55.0, Math.toRadians(225.0)), Math.toRadians(0.0)) .addDisplacementMarker { //add command for rollers servo to outtake } diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/Blue1Plus1.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/Blue1Plus1.kt index 889d3f5..98162a7 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/Blue1Plus1.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/Blue1Plus1.kt @@ -1,5 +1,7 @@ package org.firstinspires.ftc.teamcode.opModes.auto.blue.basket.left +import com.acmerobotics.dashboard.FtcDashboard +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry import com.acmerobotics.roadrunner.geometry.Pose2d import com.arcrobotics.ftclib.hardware.motors.Motor import com.qualcomm.robotcore.eventloop.opmode.Autonomous @@ -30,6 +32,8 @@ class Blue1Plus1: OpMode() { private lateinit var elevatorSubsystem: ElevatorSubsystem override fun init() { + telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry) + intake = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE.deviceName) intakeBelt = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE_BELT.deviceName) @@ -48,74 +52,75 @@ class Blue1Plus1: OpMode() { val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_LEFT.startPose) .addTemporalMarker(1.0) { intakeSubsystem.intake() - intakeBeltSubsystem.outtakePos() + intakeBeltSubsystem.intakePos() } - .splineToLinearHeading(Pose2d(55.0, 55.0, Math.toRadians(225.0)), Math.toRadians(0.0)) + .splineToLinearHeading(Pose2d(54.0, 55.0, Math.toRadians(225.0)), Math.toRadians(0.0)) .back(4.0) .addTemporalMarker(3.0) { - armSubsystem.setpoint = Math.toRadians(92.5) + armSubsystem.setpoint = Math.toRadians(97.5) } .waitSeconds(1.0) .addTemporalMarker(4.0) { - elevatorSubsystem.setpoint = 2000.0 + elevatorSubsystem.setpoint = 30.0 + } + .waitSeconds(1.0) + .addTemporalMarker(5.0) { + intakeBeltSubsystem.outtakePos() } .waitSeconds(2.0) .addTemporalMarker(7.0) { - intakeBeltSubsystem.intakePos() - } - .waitSeconds(1.0) - .addTemporalMarker(8.0) { intakeSubsystem.outtake() } + .waitSeconds(0.5) + .addTemporalMarker(7.5) { + intakeBeltSubsystem.intakePos() + elevatorSubsystem.setpoint = 0.0 + } .waitSeconds(1.5) - .forward(4.0) .addTemporalMarker(9.5) { - elevatorSubsystem.setpoint = 0.0 + armSubsystem.setpoint = 0.0 + intakeBeltSubsystem.outtakePos() + } + .waitSeconds(1.0) + .turn(Math.toRadians(38.0)) + .addTemporalMarker(11.0) { + elevatorSubsystem.setpoint = 31.0 } .waitSeconds(2.0) -// .addTemporalMarker(13.0) { -// armSubsystem.setpoint = 10.0 -// } + .addTemporalMarker(13.0) { + intakeBeltSubsystem.intakePos() + } .waitSeconds(1.0) - .turn(Math.toRadians(25.0)) .addTemporalMarker(14.0) { - elevatorSubsystem.setpoint = 1500.0 + intakeSubsystem.intake() } -// .waitSeconds(2.0) -// .addTemporalMarker(16.0) { -// intakeBeltSubsystem.intakePos() -// } -// .waitSeconds(1.0) -// .addTemporalMarker(17.0) { -// intakeSubsystem.intake() -// } -// .waitSeconds(2.0) -// .addTemporalMarker(20.0) { -// elevatorSubsystem.setpoint = 100.0 -// } -// .waitSeconds(2.0) -// .turn(Math.toRadians(-25.0)) -// .addTemporalMarker(24.0) { -// armSubsystem.setpoint = 95.0 + .waitSeconds(1.0) + .addTemporalMarker(15.0) { + elevatorSubsystem.setpoint = 0.0 + } + .waitSeconds(2.0) + .turn(Math.toRadians(-40.0)) + .addTemporalMarker(17.0) { + armSubsystem.setpoint = 97.5 + } + .waitSeconds(1.0) + .back(0.5) + .addTemporalMarker(18.0) { + elevatorSubsystem.setpoint = 33.0 + } +// .waitSeconds(3.0) +// .addTemporalMarker(21.0) { // intakeBeltSubsystem.outtakePos() -// -// } -// .addTemporalMarker(25.0) { -// elevatorSubsystem.setpoint = 2000.0 -// } -// .waitSeconds(2.0) -// .addTemporalMarker(28.0) { -// intakeSubsystem.outtake() -// } -// .waitSeconds(2.0) -// .lineToSplineHeading(Pose2d(36.0, 8.0, Math.toRadians(180.0))) -// .addTemporalMarker(12.0) { -// armSubsystem.setpoint = 50.0 // } // .waitSeconds(1.0) -// .addTemporalMarker(13.0) { -// elevatorSubsystem.setpoint = 1000.0 +//// .lineToSplineHeading(Pose2d(36.0, 8.0, Math.toRadians(180.0))) +// .addTemporalMarker(22.0) { +// intakeSubsystem.outtake() // } +//// .waitSeconds(1.0) +//// .addTemporalMarker(13.0) { +//// elevatorSubsystem.setpoint = 1000.0 +//// } .build() driveSubsystem.poseEstimate = AutoStartPose.BLUE_LEFT.startPose @@ -124,7 +129,12 @@ class Blue1Plus1: OpMode() { override fun loop() { driveSubsystem.update() + elevatorSubsystem.periodic() armSubsystem.periodic() + + telemetry.addData("Arm Position", armSubsystem.armAngle) + telemetry.addData("Slides Position", elevatorSubsystem.slidePos) + telemetry.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 d26b03d..2a402fc 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 @@ -15,15 +15,13 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp import com.qualcomm.robotcore.hardware.Servo import org.firstinspires.ftc.teamcode.commands.arm.OpenArmCommand import org.firstinspires.ftc.teamcode.commands.drive.DriveCommand -import org.firstinspires.ftc.teamcode.commands.elevator.SpinDownCommand -import org.firstinspires.ftc.teamcode.commands.elevator.SpinUpCommand import org.firstinspires.ftc.teamcode.constants.ArmConstants 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.intake.IntakeBeltSubsystem import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem -import org.firstinspires.ftc.teamcode.subsystems.slides.OpenElevatorSubsystem +import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem import kotlin.math.cos @TeleOp @@ -37,14 +35,14 @@ class MainTeleOp : CommandOpMode() { private lateinit var intake: Servo private lateinit var intakeBelt: Servo - private lateinit var slidesSubsystem: OpenElevatorSubsystem + private lateinit var slidesSubsystem: ElevatorSubsystem private lateinit var armSubsystem: OpenArmSubsystem private lateinit var driveSubsystem: DriveSubsystem private lateinit var intakeSubsystem: IntakeSubsystem private lateinit var intakeBeltSubsystem: IntakeBeltSubsystem - private lateinit var spinUpCommand: SpinUpCommand - private lateinit var spinDownCommand: SpinDownCommand + private lateinit var spinUpCommand: InstantCommand + private lateinit var spinDownCommand: InstantCommand private lateinit var armUpCommand: OpenArmCommand private lateinit var armDownCommand: OpenArmCommand @@ -76,15 +74,20 @@ class MainTeleOp : CommandOpMode() { intakeBelt = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE_BELT.deviceName) armSubsystem = OpenArmSubsystem(armRight, armLeft) { 0.0 } - slidesSubsystem = OpenElevatorSubsystem(elevatorRight, elevatorLeft, armSubsystem::armAngle) + slidesSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft, armSubsystem::armAngle) armSubsystem = OpenArmSubsystem(armRight, armLeft, slidesSubsystem::slidePos) driveSubsystem = DriveSubsystem(hardwareMap) intakeSubsystem = IntakeSubsystem(intake) intakeBeltSubsystem = IntakeBeltSubsystem(intakeBelt) - spinUpCommand = SpinUpCommand(slidesSubsystem) - spinDownCommand = SpinDownCommand(slidesSubsystem) + spinUpCommand = InstantCommand({ + slidesSubsystem.setpoint = 30.0 + }) + + spinDownCommand = InstantCommand({ + slidesSubsystem.setpoint = 0.0 + }) armUpCommand = OpenArmCommand(armSubsystem, true) armDownCommand = OpenArmCommand(armSubsystem, false) @@ -103,8 +106,8 @@ class MainTeleOp : CommandOpMode() { driveCommand = DriveCommand(driveSubsystem, driver::getLeftX, driver::getLeftY, driver::getRightX, 0.0) - operator.getGamepadButton(GamepadKeys.Button.DPAD_UP).whileHeld(spinUpCommand) - operator.getGamepadButton(GamepadKeys.Button.DPAD_DOWN).whileHeld(spinDownCommand) + operator.getGamepadButton(GamepadKeys.Button.DPAD_UP).whenPressed(spinUpCommand) + operator.getGamepadButton(GamepadKeys.Button.DPAD_DOWN).whenPressed(spinDownCommand) operator.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER).whileHeld(armUpCommand) operator.getGamepadButton(GamepadKeys.Button.RIGHT_BUMPER).whileHeld(armDownCommand) diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/IntakeTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/IntakeTuner.kt index 6fa8a7a..016ee4a 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/IntakeTuner.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/IntakeTuner.kt @@ -31,20 +31,20 @@ class IntakeTuner : CommandOpMode() { intakeBeltSubsystem = IntakeBeltSubsystem(intakeBelt) RunCommand({ - intakeSubsystem.setSpeed(speed) + intakeSubsystem.setSpeed(claw) }).perpetually().schedule() RunCommand({ - intakeBeltSubsystem.setPos(position) + intakeBeltSubsystem.setPos(belt) }).perpetually().schedule() } companion object { @JvmField - var speed = 0.0 + var claw = 0.75 @JvmField - var position = 0.0 + var belt = 0.0 } } \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/intake/IntakeBeltSubsystem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/intake/IntakeBeltSubsystem.kt index a15f869..8596dd5 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/intake/IntakeBeltSubsystem.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/intake/IntakeBeltSubsystem.kt @@ -9,12 +9,12 @@ class IntakeBeltSubsystem( var beltPos = false fun outtakePos() { - intakeBelt.position = 0.25 + intakeBelt.position = 0.75 beltPos = !beltPos } fun intakePos() { - intakeBelt.position = 0.0 + intakeBelt.position = 0.5 beltPos = !beltPos } 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 index b10037c..dd2541a 100644 --- 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 @@ -9,12 +9,12 @@ class IntakeSubsystem( var intakePos = false fun intake() { - intake.position = 0.6 + intake.position = 1.0 intakePos = !intakePos } fun outtake() { - intake.position = 1.0 + intake.position = 0.6 intakePos = !intakePos } diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/ElevatorSubsystem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/ElevatorSubsystem.kt index 9f07512..63d49b6 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/ElevatorSubsystem.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/ElevatorSubsystem.kt @@ -6,6 +6,7 @@ import com.arcrobotics.ftclib.controller.wpilibcontroller.ProfiledPIDController 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 org.firstinspires.ftc.teamcode.constants.SlidesConstants import kotlin.math.sin @@ -28,8 +29,13 @@ class ElevatorSubsystem( var setpoint = 0.0 set(value) { - controller.goal = TrapezoidProfile.State(value, 0.0) - field = value + val clamped = if (slideAngle.invoke() < Math.toRadians(75.0)) + clamp(value, 0.0, 20.0) + else + value + + controller.goal = TrapezoidProfile.State(clamped, 0.0) + field = clamped } val slidePos: Double