From b2184aa08aad9a095a00bf23c540fdc682b6a526 Mon Sep 17 00:00:00 2001 From: Ishaan Ghaskadbi Date: Sun, 3 Nov 2024 10:41:14 -0500 Subject: [PATCH] main teleOp 1 --- .idea/misc.xml | 3 +- .../ftc/teamcode/commands/drive/ArmCommand.kt | 22 +++++++++- .../ftc/teamcode/constants/ControlBoard.kt | 16 ++++---- .../ftc/teamcode/opModes/teleOp/MainTeleOp.kt | 40 ++++++++++++++----- .../teamcode/subsystems/arm/ArmSubsystem.kt | 16 +++++--- .../subsystems/drive/DriveSubsystem.kt | 2 +- .../subsystems/slides/SlidesSubsystem.kt | 12 +++--- 7 files changed, 77 insertions(+), 34 deletions(-) diff --git a/.idea/misc.xml b/.idea/misc.xml index 0ad17cb..b2c751a 100644 --- a/.idea/misc.xml +++ b/.idea/misc.xml @@ -1,7 +1,6 @@ - - + diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/drive/ArmCommand.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/drive/ArmCommand.kt index 413f05b..c590585 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/drive/ArmCommand.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/drive/ArmCommand.kt @@ -1,4 +1,24 @@ package org.firstinspires.ftc.teamcode.commands.drive -class ArmCommand { +import com.arcrobotics.ftclib.command.CommandBase +import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem + +class ArmCommand( + private val subsystem: ArmSubsystem, + private val turn : Boolean, +) : CommandBase() { + + override fun execute() { + if (turn) { + subsystem.clockwise() + } else { + subsystem.anticlockwise() + } + } + + override fun end(interrupted: Boolean) { + subsystem.stop() + } + + } \ 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 ecee851..5392afc 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 @@ -9,16 +9,16 @@ enum class ControlBoard(val deviceName: String) { DRIVE_RIGHT_REAR("rightRear"), // Odometry - ODO_LEFT_ENCODER(""), - ODO_RIGHT_ENCODER(""), - ODO_STRAFE_ENCODER(""), + ODO_LEFT_ENCODER("leftFront"), + ODO_RIGHT_ENCODER("rightFront"), + ODO_STRAFE_ENCODER("leftSlideString"), // Arm - ARM_LEFT(""), - ARM_RIGHT(""), - SLIDES_LEFT(""), - SLIDES_RIGHT(""), + ARM_LEFT("leftSlideAxel"), + ARM_RIGHT("rightSlideAxel"), + SLIDES_LEFT("leftSlideString"), + SLIDES_RIGHT("rightSlideString"), // Camera - CAMERA("lifecam") + CAMERA("lifeCam") } 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 7e02824..218af5e 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 @@ -5,24 +5,33 @@ import com.arcrobotics.ftclib.gamepad.GamepadEx import com.arcrobotics.ftclib.gamepad.GamepadKeys import com.arcrobotics.ftclib.hardware.motors.Motor import com.qualcomm.robotcore.eventloop.opmode.TeleOp +import org.firstinspires.ftc.teamcode.commands.drive.ArmCommand +import org.firstinspires.ftc.teamcode.commands.drive.DriveCommand import org.firstinspires.ftc.teamcode.commands.drive.SpinDownCommand import org.firstinspires.ftc.teamcode.commands.drive.SpinUpCommand 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.slides.SlidesSubsystem @TeleOp class MainTeleOp: CommandOpMode() { - private lateinit var leftSlideString : Motor - private lateinit var rightSlideString : Motor + private lateinit var elevatorLeft : Motor + private lateinit var elevatorRight : Motor + private lateinit var armLeft : Motor + private lateinit var armRight : Motor private lateinit var slidesSubsystem: SlidesSubsystem - + private lateinit var armSubsystem: ArmSubsystem + private lateinit var driveSubsystem: DriveSubsystem private lateinit var spinUpCommand: SpinUpCommand private lateinit var spinDownCommand: SpinDownCommand - + private lateinit var armUpCommand: ArmCommand + private lateinit var armDownCommand: ArmCommand + private lateinit var driveCommand: DriveCommand private lateinit var driver : GamepadEx private lateinit var operator : GamepadEx @@ -31,17 +40,26 @@ class MainTeleOp: CommandOpMode() { driver = GamepadEx(gamepad1) operator = GamepadEx(gamepad2) - rightSlideString = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName) - leftSlideString = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName) - - - slidesSubsystem = SlidesSubsystem(rightSlideString, leftSlideString) + elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName) + elevatorLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName) + armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName) + armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName) + slidesSubsystem = SlidesSubsystem(elevatorRight, elevatorLeft) + armSubsystem = ArmSubsystem(armRight, armLeft) + driveSubsystem = DriveSubsystem(hardwareMap) spinUpCommand = SpinUpCommand(slidesSubsystem) spinDownCommand = SpinDownCommand(slidesSubsystem) + armUpCommand = ArmCommand(armSubsystem, true) + armDownCommand = ArmCommand(armSubsystem, false) + driveCommand = DriveCommand(driveSubsystem, driver::getLeftX, driver::getLeftY, driver::getRightX, 0.1) + + //operator.getGamepadButton(GamepadKeys.Button.DPAD_UP).whileHeld(spinUpCommand) + // 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.DPAD_UP).whileHeld(spinUpCommand) - operator.getGamepadButton(GamepadKeys.Button.DPAD_DOWN).whileHeld(spinDownCommand) + driveSubsystem.defaultCommand = driveCommand } } \ 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 2d10ee0..bd247e7 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 @@ -6,23 +6,29 @@ import com.arcrobotics.ftclib.hardware.motors.MotorGroup class ArmSubsystem( // Here I am just declaring the motors and what they are called on our driver hub. - private val leftSlideAxel : Motor, - private val rightSlideAxel : Motor, + private val armRight : Motor, + private val armLeft : Motor, ) : SubsystemBase() { //Here I am making a motor group, as the arm motors are going to work together to to turn the slides. - private val turnMotors = MotorGroup(leftSlideAxel, rightSlideAxel) + private val turnMotors = MotorGroup(armRight, armLeft) + + init { + armLeft.inverted = true + turnMotors.resetEncoder() + turnMotors.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE) + } // Here are functions that work the motor, and the double is the speed of the motor, 1 being 100%. fun clockwise() { - turnMotors.set(1.0) + turnMotors.set(0.35) } fun anticlockwise() { - turnMotors.set(-1.0) + turnMotors.set(-0.35) } fun stop() { diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/drive/DriveSubsystem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/drive/DriveSubsystem.kt index 4aed5b7..ed6b1bd 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/drive/DriveSubsystem.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/drive/DriveSubsystem.kt @@ -97,7 +97,7 @@ class DriveSubsystem @JvmOverloads constructor( setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, DriveConstants.MOTOR_VELO_PID) } - leftFront.direction = DcMotorSimple.Direction.REVERSE + // leftFront.direction = DcMotorSimple.Direction.REVERSE leftRear.direction = DcMotorSimple.Direction.REVERSE localizer = StandardTrackingWheelLocalizer(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/SlidesSubsystem.kt index f1b7aa4..f6681ce 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/SlidesSubsystem.kt @@ -5,19 +5,20 @@ import com.arcrobotics.ftclib.hardware.motors.Motor import com.arcrobotics.ftclib.hardware.motors.MotorGroup class SlidesSubsystem( - //sets them as a private variable thats a motor (same name as in driver hub) - private val leftSlideString : Motor, - private val rightSlideString : Motor, + private val elevatorLeft : Motor, + private val elevatorRight : Motor, ) : SubsystemBase() { //makes a motor group bc you have to move them at the same time - private val elevatorMotors = MotorGroup(leftSlideString, rightSlideString) + private val elevatorMotors = MotorGroup(elevatorLeft, elevatorRight) + init { + elevatorLeft.inverted = true + } // Functions for moving slides up and down, number being speed, 1 being 100% fun up() { - elevatorMotors.set(1.0) } @@ -29,5 +30,4 @@ class SlidesSubsystem( elevatorMotors.set(0.0) } - } \ No newline at end of file