main teleOp 1

ftc13100 · Nov 3, 2024 · b2184aa · b2184aa
1 parent dbc195d
commit b2184aa
Show file tree

Hide file tree

Showing 7 changed files with 77 additions and 34 deletions.
diff --git a/.idea/misc.xml b/.idea/misc.xml
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
@@ -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()
+    }
+
+
 }
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
@@ -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
@@ -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
     }
 }
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
@@ -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
@@ -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
@@ -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)
     }
 
-
 }