diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/ArmConstants.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/ArmConstants.kt index 44fb0b8..50971b8 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/ArmConstants.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/ArmConstants.kt @@ -4,5 +4,5 @@ enum class ArmConstants(val value: Double) { kP(1.0), kI(0.0), kD(0.08), - kCos(0.0045) + kCos(0.01) } \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/ControlBoard.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/ControlBoard.kt index f2bf6c0..3d31684 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/ControlBoard.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/ControlBoard.kt @@ -20,8 +20,8 @@ enum class ControlBoard(val deviceName: String) { SLIDES_RIGHT("rightSlideString"), //Intake - INTAKE(""), + INTAKE("rollerIntake"), + INTAKE_BELT("intakeBelt") + - // Camera - CAMERA("lifeCam") } diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/BlueHighBasketLeft.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/BlueHighBasketLeft.kt index 3530290..461264b 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/BlueHighBasketLeft.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/BlueHighBasketLeft.kt @@ -26,7 +26,7 @@ class BlueHighBasketLeft: OpMode() { private lateinit var elevatorSubsystem: ElevatorSubsystem override fun init() { -// intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) + intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName) armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName) @@ -35,29 +35,31 @@ class BlueHighBasketLeft: OpMode() { elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName) driveSubsystem = DriveSubsystem(hardwareMap) -// intakeSubsystem = IntakeSubsystem(intake) + intakeSubsystem = IntakeSubsystem(intake) armSubsystem = ArmSubsystem(armRight, armLeft) elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft, armSubsystem::armAngle) val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_LEFT.startPose) .turn(Math.toRadians(-90.0)) .back(60.0) + .turn(Math.toRadians(20.0)) + .back(5.0) .addTemporalMarker(3.0) { armSubsystem.setpoint = Math.toRadians(95.0) } .waitSeconds(1.0) - .addTemporalMarker(6.0) { - elevatorSubsystem.setpoint = 1000.0 + .addTemporalMarker(5.0) { + elevatorSubsystem.setpoint = 1900.0 } .waitSeconds(2.0) -// .addTemporalMarker(5.0){ -// intakeSubsystem.outtake() -// } + .addTemporalMarker(6.0){ + intakeSubsystem.outtake() + } .waitSeconds(1.0) - .addTemporalMarker(10.0) { - elevatorSubsystem.setpoint = 50.0 + .addTemporalMarker(9.0) { + elevatorSubsystem.setpoint = 0.0 } - .waitSeconds(2.0) + .waitSeconds(5.0) .addTemporalMarker(13.0) { armSubsystem.setpoint = Math.toRadians(5.0) } diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/chamber/left/BlueHighChamberLeft.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/chamber/left/BlueHighChamberLeft.kt index eee7f89..6b537df 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/chamber/left/BlueHighChamberLeft.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/chamber/left/BlueHighChamberLeft.kt @@ -44,7 +44,7 @@ class BlueHighChamberLeft() : OpMode() { .forward(28.0) .strafeLeft(11.0) .addTemporalMarker(2.0) { - armSubsystem.setpoint = Math.toRadians(150.0) + armSubsystem.setpoint = Math.toRadians(.0) } .waitSeconds(1.0) .addTemporalMarker(3.0) { diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesPIDTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesPIDTuner.kt index 815923d..7bd5966 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesPIDTuner.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesPIDTuner.kt @@ -43,6 +43,7 @@ class SlidesPIDTuner : CommandOpMode() { telemetry.addData("Slides Position", slidesSubsystem.slidePos) telemetry.addData("Setpoint", target) telemetry.addData("arm angle", armSubsystem.armAngle) + telemetry.addData("slides Velocity", slidesSubsystem.slideVelocity) telemetry.update() }).perpetually().schedule() diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/intake/IntakeBeltSubsytem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/intake/IntakeBeltSubsytem.kt new file mode 100644 index 0000000..198aff4 --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/intake/IntakeBeltSubsytem.kt @@ -0,0 +1,12 @@ +package org.firstinspires.ftc.teamcode.subsystems.intake + +import com.arcrobotics.ftclib.command.SubsystemBase +import com.qualcomm.robotcore.hardware.Servo + +class IntakeBeltSubsytem( + private val intakeBelt: Servo +) : SubsystemBase() { + fun intake() { intakeBelt.position = 0.5 } + + fun outtake() { intakeBelt.position = 0.0 } +} \ No newline at end of file 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 b3141c9..4bab4f8 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 @@ -21,8 +21,8 @@ class ElevatorSubsystem( SlidesConstants.kI.value, SlidesConstants.kD.value, TrapezoidProfile.Constraints( - 2000.0, - 1000.0 + 1600.0, + 700.0 ) ) @@ -45,7 +45,7 @@ class ElevatorSubsystem( elevatorRight.encoder.setDirection(Motor.Direction.REVERSE) extendMotors.resetEncoder() - extendMotors.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE) + extendMotors.setZeroPowerBehavior(Motor.ZeroPowerBehavior.FLOAT) } override fun periodic() { @@ -71,7 +71,8 @@ class ElevatorSubsystem( companion object { @JvmField - var kP = 0.014 + var kP = 0.01 + // kP = 0.01 @JvmField var kI = 0.0 @@ -81,5 +82,8 @@ class ElevatorSubsystem( @JvmField var kG = 0.115 + // kG = 0.115 + } -} \ No newline at end of file +} +