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 87f70a3..eaf5203 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 @@ -29,70 +29,66 @@ object BlueAuto { .addDisplacementMarker { //add command for rollers servo to outtake } - .turn(Math.toRadians(30.0)) // - .addTemporalMarker(6.0) { - //add command for arm to go down - //add command for slides to go out - //add command to belt servo to go at [x] angle - } - .addDisplacementMarker { - //add command to intake - } - .turn(Math.toRadians(-30.0)) // - .addTemporalMarker() { - //add command to bring slides in - //add command to bring arm up - //add command to put slides out - //add command to turn belt servo - } - .addDisplacementMarker { - //add command to outtake - } - .turn(Math.toRadians(55.0)) // - .addTemporalMarker() { - //add command to bring slides in - //add command to bring arm down - //add command to put slides out - //add command to turn belt servo - } - .addDisplacementMarker { - //add command to intake - } - .turn(Math.toRadians(-55.0)) // - .addTemporalMarker() { - //add command to bring slides in - //add command to bring arm up - //add command to put slides out - //add command to turn belt servo - } - .addDisplacementMarker { - //add command to outtake - } - .turn(Math.toRadians(80.0)) // - .addTemporalMarker() { - //add command to bring slides in - //add command to bring arm down - //add command to put slides out - //add command to turn belt servo - } - .addDisplacementMarker { - //add command to intake - } - .turn(Math.toRadians(-80.0)) // - .addTemporalMarker() { - //add command to bring slides in - //add command to bring arm up - //add command to put slides out - //add command to turn belt servo - } - .addDisplacementMarker { - //add command to outtake - } - .splineToSplineHeading(Pose2d(36.0, 12.0, Math.toRadians(-180.0)), Math.toRadians(-90.0)) - .addDisplacementMarker { - //add command to bring arm up - //add command to bring slides out - } +// .turn(Math.toRadians(30.0)) // +// .addTemporalMarker(6.0) { +// //add command for arm to go down +// //add command for slides to go out +// //add command to belt servo to go at [x] angle +// } +// .addDisplacementMarker { +// //add command to intake +// } +// .turn(Math.toRadians(-30.0)) // +// .addTemporalMarker() { +// //add command to bring slides in +// //add command to bring arm up +// //add command to put slides out +// //add command to turn belt servo +// } +// .addDisplacementMarker { +// //add command to outtake +// } +// .turn(Math.toRadians(55.0)) // +// .addTemporalMarker() { +// //add command to bring slides in +// //add command to bring arm down +// //add command to put slides out +// //add command to turn belt servo +// } +// .addDisplacementMarker { +// //add command to intake +// } +// .turn(Math.toRadians(-55.0)) // +// .addTemporalMarker() { +// //add command to bring slides in +// //add command to bring arm up +// //add command to put slides out +// //add command to turn belt servo +// } +// .addDisplacementMarker { +// //add command to outtake +// } +// .turn(Math.toRadians(80.0)) // +// .addTemporalMarker() { +// //add command to bring slides in +// //add command to bring arm down +// //add command to put slides out +// //add command to turn belt servo +// } +// .addDisplacementMarker { +// //add command to intake +// } +// .turn(Math.toRadians(-80.0)) // +// .addTemporalMarker() { +// //add command to bring slides in +// //add command to bring arm up +// //add command to put slides out +// //add command to turn belt servo +// } +// .addDisplacementMarker { +// //add command to outtake +// } + .lineToSplineHeading(Pose2d(36.0, 8.0, Math.toRadians(180.0))) @@ -101,7 +97,7 @@ object BlueAuto { } var img: Image? = null try { - img = ImageIO.read(File("C://Users//arava//Downloads//field.png/")) + img = ImageIO.read(File("/Users/ishaanghaskadbi/Downloads/field.png")) } catch (_ : IOException) { } diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/intake/IntakeBeltCommand.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/intake/IntakeBeltCommand.kt new file mode 100644 index 0000000..9d0f345 --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/intake/IntakeBeltCommand.kt @@ -0,0 +1,22 @@ +package org.firstinspires.ftc.teamcode.commands.intake + +import com.arcrobotics.ftclib.command.CommandBase +import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeBeltSubsystem + +class IntakeBeltCommand( + private val intakeBelt: Boolean, + private val intakeBeltSubsystem: IntakeBeltSubsystem +) : CommandBase() { + override fun execute() { + if (intakeBelt) { + intakeBeltSubsystem.intakePos() + } + else { + intakeBeltSubsystem.outtakePos() + } + } + + /* override fun end(interrupted: Boolean) { + intakeBeltSubsystem.stop() + } +*/} \ No newline at end of file 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 50971b8..2bb65f2 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 @@ -1,8 +1,8 @@ package org.firstinspires.ftc.teamcode.constants enum class ArmConstants(val value: Double) { - kP(1.0), + kP(2.0), kI(0.0), - kD(0.08), + kD(0.0), 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 3d31684..07234d5 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,7 +20,7 @@ enum class ControlBoard(val deviceName: String) { SLIDES_RIGHT("rightSlideString"), //Intake - INTAKE("rollerIntake"), + INTAKE("intakeRoller"), INTAKE_BELT("intakeBelt") 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 95048f3..3f7a4f7 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 @@ -5,14 +5,16 @@ 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 com.qualcomm.robotcore.hardware.Servo 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.IntakeBeltSubsystem import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem -@Autonomous(name = "Blue 1 + 3 High Basket", group = "Basket", preselectTeleOp = "MainTeleOp") +@Autonomous(name = "Blue 1 + 1 High Basket", group = "Basket", preselectTeleOp = "MainTeleOp") class Blue1Plus1: OpMode() { private lateinit var armLeft: Motor private lateinit var armRight: Motor @@ -20,14 +22,17 @@ class Blue1Plus1: OpMode() { private lateinit var elevatorRight: Motor private lateinit var intake: CRServo + private lateinit var intakeBelt: Servo private lateinit var driveSubsystem: DriveSubsystem private lateinit var intakeSubsystem: IntakeSubsystem + private lateinit var intakeBeltSubsystem: IntakeBeltSubsystem private lateinit var armSubsystem: ArmSubsystem private lateinit var elevatorSubsystem: ElevatorSubsystem override fun init() { -// intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) + intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) + intakeBelt = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE_BELT.deviceName) armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName) armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName) @@ -36,48 +41,82 @@ class Blue1Plus1: OpMode() { elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName) driveSubsystem = DriveSubsystem(hardwareMap) -// intakeSubsystem = IntakeSubsystem(intake) + intakeSubsystem = IntakeSubsystem(intake) + intakeBeltSubsystem = IntakeBeltSubsystem(intakeBelt) armSubsystem = ArmSubsystem(armRight, armLeft) elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft, armSubsystem::armAngle) val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_LEFT.startPose) - .addTemporalMarker(0.1) { - armSubsystem.setpoint = Math.toRadians(95.0) - } - .waitSeconds(0.25) .splineToLinearHeading(Pose2d(55.0, 55.0, Math.toRadians(225.0)), Math.toRadians(0.0)) - .waitSeconds(0.25) + .back(4.3) .addTemporalMarker(3.0) { - elevatorSubsystem.setpoint = 1700.0 - //turn belt servo + armSubsystem.setpoint = Math.toRadians(97.5) } - //outtake .waitSeconds(1.0) - .turn(Math.toRadians(30.0)) + .addTemporalMarker(4.0) { + elevatorSubsystem.setpoint = 2000.0 + } + .waitSeconds(2.0) .addTemporalMarker(7.0) { + intakeBeltSubsystem.intakePos() + } + .waitSeconds(1.0) + .addTemporalMarker(8.0) { + intakeSubsystem.outtake() + } + .waitSeconds(0.5) + .addTemporalMarker(8.5) { + intakeSubsystem.stop() + } + .waitSeconds(1.0) + .forward(4.0) + .addTemporalMarker(9.5) { elevatorSubsystem.setpoint = 0.0 - //belt servo turn } -// .addTemporalMarker(8.0) { -// armSubsystem.setpoint = Math.toRadians(0.0) +// .waitSeconds(2.0) +// .addTemporalMarker(13.0) { +// armSubsystem.setpoint = 0.0 +// } +// .waitSeconds(1.0) +// .turn(Math.toRadians(25.0)) +// .addTemporalMarker(14.0) { +// elevatorSubsystem.setpoint = 1500.0 // } -// .addTemporalMarker(9.0) { -// elevatorSubsystem.setpoint = 1700.0 -// //intake +// .waitSeconds(2.0) +// .addTemporalMarker(16.0) { +// intakeBeltSubsystem.intakePos() +// } +// .waitSeconds(1.0) +// .addTemporalMarker(17.0) { +// intakeSubsystem.intake() // } -// .addTemporalMarker(12.0) { -// elevatorSubsystem.setpoint = 0.0 +// .waitSeconds(2.0) +// .addTemporalMarker(20.0) { +// elevatorSubsystem.setpoint = 100.0 // } -// .addTemporalMarker(15.0) { +// .waitSeconds(2.0) +// .turn(Math.toRadians(-25.0)) +// .addTemporalMarker(24.0) { // armSubsystem.setpoint = 95.0 -// //turn belt servo +// intakeBeltSubsystem.outtakePos() +// // } -// .addTemporalMarker(18.0) { -// //outtake +// .addTemporalMarker(25.0) { +// elevatorSubsystem.setpoint = 2000.0 // } // .waitSeconds(2.0) -// .splineToSplineHeading(Pose2d(36.0, 12.0, Math.toRadians(-180.0)), Math.toRadians(-90.0)) -// .turn(Math.toRadians(-30.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 + } .build() driveSubsystem.poseEstimate = AutoStartPose.BLUE_LEFT.startPose 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 6167ae3..2245b19 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 @@ -4,16 +4,20 @@ import com.arcrobotics.ftclib.command.CommandOpMode import com.arcrobotics.ftclib.command.RunCommand import com.arcrobotics.ftclib.gamepad.GamepadEx import com.arcrobotics.ftclib.gamepad.GamepadKeys +import com.arcrobotics.ftclib.hardware.motors.CRServo import com.arcrobotics.ftclib.hardware.motors.Motor 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.commands.intake.IntakeBeltCommand import org.firstinspires.ftc.teamcode.commands.intake.IntakeCommand 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 @@ -23,21 +27,29 @@ class MainTeleOp : CommandOpMode() { private lateinit var elevatorRight: Motor private lateinit var armLeft: Motor private lateinit var armRight: Motor - //private lateinit var intake: CRServo + + private lateinit var intake: CRServo + private lateinit var intakeBelt: Servo private lateinit var slidesSubsystem: OpenElevatorSubsystem 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 armUpCommand: OpenArmCommand private lateinit var armDownCommand: OpenArmCommand + private lateinit var driveCommand: DriveCommand + private lateinit var intakeCommand: IntakeCommand private lateinit var outtakeCommand: IntakeCommand - + private lateinit var intakeBeltCommand: IntakeBeltCommand + private lateinit var outtakeBeltCommand: IntakeBeltCommand private lateinit var driver: GamepadEx private lateinit var operator: GamepadEx @@ -46,25 +58,30 @@ class MainTeleOp : CommandOpMode() { driver = GamepadEx(gamepad1) operator = GamepadEx(gamepad2) - elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName, Motor.GoBILDA.RPM_435) - elevatorLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName, Motor.GoBILDA.RPM_435) + elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName, Motor.GoBILDA.RPM_1150) + elevatorLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName, Motor.GoBILDA.RPM_1150) - armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName, Motor.GoBILDA.RPM_435) - armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName, Motor.GoBILDA.RPM_435) + armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName, Motor.GoBILDA.RPM_60) + armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName, Motor.GoBILDA.RPM_60) - //intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) + intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) + intakeBelt = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE_BELT.deviceName) armSubsystem = OpenArmSubsystem(armRight, armLeft) slidesSubsystem = OpenElevatorSubsystem(elevatorRight, elevatorLeft, armSubsystem::armAngle) driveSubsystem = DriveSubsystem(hardwareMap) - //intakeSubsystem = IntakeSubsystem(intake) + intakeSubsystem = IntakeSubsystem(intake) + intakeBeltSubsystem = IntakeBeltSubsystem(intakeBelt) + spinUpCommand = SpinUpCommand(slidesSubsystem) spinDownCommand = SpinDownCommand(slidesSubsystem) armUpCommand = OpenArmCommand(armSubsystem, true) armDownCommand = OpenArmCommand(armSubsystem, false) - //intakeCommand = IntakeCommand(true, intakeSubsystem) - //outtakeCommand = IntakeCommand(false, intakeSubsystem) + intakeCommand = IntakeCommand(true, intakeSubsystem) + outtakeCommand = IntakeCommand(false, intakeSubsystem) + intakeBeltCommand = IntakeBeltCommand(true, intakeBeltSubsystem) + outtakeBeltCommand = IntakeBeltCommand(false, intakeBeltSubsystem) driveCommand = DriveCommand(driveSubsystem, driver::getLeftX, driver::getLeftY, driver::getRightX, 0.0) operator.getGamepadButton(GamepadKeys.Button.DPAD_UP).whileHeld(spinUpCommand) @@ -73,8 +90,10 @@ class MainTeleOp : CommandOpMode() { operator.getGamepadButton(GamepadKeys.Button.DPAD_DOWN).whileHeld(spinDownCommand) operator.getGamepadButton(GamepadKeys.Button.RIGHT_BUMPER).whileHeld(armUpCommand) operator.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER).whileHeld(armDownCommand) - //operator.getGamepadButton(GamepadKeys.Button.Y).whenHeld(intakeCommand) - // operator.getGamepadButton(GamepadKeys.Button.A).whileHeld(outtakeCommand) + operator.getGamepadButton(GamepadKeys.Button.Y).whenHeld(intakeCommand) + operator.getGamepadButton(GamepadKeys.Button.A).whileHeld(outtakeCommand) + operator.getGamepadButton(GamepadKeys.Button.X).whileHeld(intakeBeltCommand) + operator.getGamepadButton(GamepadKeys.Button.B).whileHeld(outtakeBeltCommand) driveSubsystem.defaultCommand = driveCommand 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 new file mode 100644 index 0000000..a74aec4 --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/IntakeTuner.kt @@ -0,0 +1,50 @@ +package org.firstinspires.ftc.teamcode.opModes.tuning + +import com.acmerobotics.dashboard.FtcDashboard +import com.acmerobotics.dashboard.config.Config +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry +import com.arcrobotics.ftclib.command.CommandOpMode +import com.arcrobotics.ftclib.command.RunCommand +import com.arcrobotics.ftclib.hardware.motors.CRServo +import com.qualcomm.robotcore.eventloop.opmode.TeleOp +import com.qualcomm.robotcore.hardware.Servo +import org.firstinspires.ftc.teamcode.constants.ControlBoard +import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeBeltSubsystem +import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem + +@TeleOp +@Config +class IntakeTuner : CommandOpMode() { + private lateinit var intake: CRServo + private lateinit var intakeBelt: Servo + + private lateinit var intakeSubsystem: IntakeSubsystem + private lateinit var intakeBeltSubsystem: IntakeBeltSubsystem + + override fun initialize() { + telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry) + + intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) + intakeBelt = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE_BELT.deviceName) + + intakeSubsystem = IntakeSubsystem(intake) + intakeBeltSubsystem = IntakeBeltSubsystem(intakeBelt) + + RunCommand({ + intakeSubsystem.setSpeed(speed) + }).perpetually().schedule() + + RunCommand({ + intakeBeltSubsystem.setPos(position) + }).perpetually().schedule() + + } + + companion object { + @JvmField + var speed = 0.0 + + @JvmField + var position = 1.0 + } +} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/ArmSubsystem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/ArmSubsystem.kt index 2973060..bb2b606 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/ArmSubsystem.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/ArmSubsystem.kt @@ -29,19 +29,36 @@ class ArmSubsystem( val armVelocity: Double get() = turnMotors.velocities[0] / GoBILDA.RPM_60.cpr * PI - private val feedforward = ArmFeedforward(0.0, ArmConstants.kCos.value, 0.0); + private var feedforward = ArmFeedforward(0.0, ArmConstants.kCos.value, 0.0); init { - armRight.inverted = true - armRight.encoder.setDirection(Motor.Direction.REVERSE) + armLeft.inverted = true turnMotors.resetEncoder() turnMotors.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE) } override fun useOutput(output: Double, setpoint: Double) { +// controller.setPIDF(kP, kI, kD, 0.0) +// feedforward = ArmFeedforward(0.0, kCos, 0.0) + turnMotors.set(output + feedforward.calculate(armAngle, armVelocity)) } override fun getMeasurement() = armAngle + + companion object { + @JvmField + var kP = 2.0 + + @JvmField + var kI = 0.0 + + @JvmField + var kD = 0.0 + + @JvmField + var kCos = 0.1 + + } } \ 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 new file mode 100644 index 0000000..a9797db --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/intake/IntakeBeltSubsystem.kt @@ -0,0 +1,19 @@ +package org.firstinspires.ftc.teamcode.subsystems.intake + +import com.arcrobotics.ftclib.command.SubsystemBase +import com.qualcomm.robotcore.hardware.Servo + +class IntakeBeltSubsystem( + private val intakeBelt: Servo +) : SubsystemBase() { + fun intakePos() { + intakeBelt.position = 0.0 + } + fun outtakePos() { + intakeBelt.position = 0.6 + } + + fun setPos(pos: Double) { + intakeBelt.position = pos + } +} \ No newline at end of file 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 deleted file mode 100644 index 198aff4..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/intake/IntakeBeltSubsytem.kt +++ /dev/null @@ -1,12 +0,0 @@ -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/intake/IntakeSubsystem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/intake/IntakeSubsystem.kt index 216211c..1d360fd 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 @@ -10,5 +10,7 @@ class IntakeSubsystem( fun outtake() = intake.set(-0.5) + fun setSpeed(speed: Double) = intake.set(speed) + fun stop() = intake.stop() } \ 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 4bab4f8..4a78f77 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 @@ -45,7 +45,7 @@ class ElevatorSubsystem( elevatorRight.encoder.setDirection(Motor.Direction.REVERSE) extendMotors.resetEncoder() - extendMotors.setZeroPowerBehavior(Motor.ZeroPowerBehavior.FLOAT) + extendMotors.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE) } override fun periodic() {