Merge pull request #4 from STMARobotics/kyen/slide-code

Kyen/slide code
STMARobotics · Oct 9, 2024 · 7bbc3dc · 7bbc3dc
2 parents 4917452 + 3a95cbc
commit 7bbc3dc
Show file tree

Hide file tree

Showing 4 changed files with 194 additions and 2 deletions.
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ArmSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ArmSubsystem.java
@@ -0,0 +1,80 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit;
+
+public class ArmSubsystem {
+    private DcMotor armMotor;
+
+    private HardwareMap hardwareMap;
+    private Telemetry telemetry;
+
+    public static final String ARM_MOTOR = "arm_motor";
+
+    final double ARM_TICKS_PER_DEGREE =
+            28 // number of encoder ticks per rotation of the bare motor
+                    * 250047.0 / 4913.0 // This is the exact gear ratio of the 50.9:1 Yellow Jacket gearbox
+                    * 100.0 / 20.0 // This is the external gear reduction, a 20T pinion gear that drives a 100T hub-mount gear
+                    * 1/360.0; // we want ticks per degree, not per rotation
+    final double ARM_COLLAPSED_INTO_ROBOT = 0;
+    final double ARM_COLLECT              = 15 * ARM_TICKS_PER_DEGREE;
+    final double ARM_CLEAR_BARRIER        = 15 * ARM_TICKS_PER_DEGREE;
+    final double ARM_SCORE_SPECIMEN       = 90 * ARM_TICKS_PER_DEGREE;
+    final double ARM_SCORE_SAMPLE_IN_LOW  = 90 * ARM_TICKS_PER_DEGREE;
+    final double ARM_ATTACH_HANGING_HOOK  = 110 * ARM_TICKS_PER_DEGREE;
+    final double ARM_WINCH_ROBOT          = 10  * ARM_TICKS_PER_DEGREE;
+
+    static double FUDGE_FACTOR_DEGREES = 15;
+
+    public ArmSubsystem(HardwareMap hm, Telemetry telemetry)    {
+        this.hardwareMap = hm;
+        this.telemetry = telemetry;
+
+        armMotor=hm.get(DcMotor.class, ARM_MOTOR);
+        armMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        ((DcMotorEx)armMotor).setCurrentAlert(5, CurrentUnit.AMPS);
+        armMotor.setTargetPosition(0);
+        armMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        armMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+
+        armMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+    }
+
+    public void moveToArmToCollectPosition(double alteration){
+
+        setArmPosition(ARM_COLLECT + getAlterationDegrees(alteration));
+    }
+
+    private double getAlterationDegrees(double alteration) {
+        return FUDGE_FACTOR_DEGREES * ARM_TICKS_PER_DEGREE * alteration;
+    }
+
+    public void moveArmToClearBarrierPosition(double alteration){
+        setArmPosition(ARM_CLEAR_BARRIER + (getAlterationDegrees(alteration)));
+    }
+    public void moveArmToScoreSampleInLowPosition(double alteration){
+        setArmPosition(ARM_SCORE_SAMPLE_IN_LOW + (getAlterationDegrees(alteration)));
+    }
+    public void moveArmToCollapsedIntoRobotPosition(double alteration){
+        setArmPosition(ARM_COLLAPSED_INTO_ROBOT + (getAlterationDegrees(alteration)));
+    }
+    public void moveArmToScoreSpecimenPosition(double alteration){
+        setArmPosition(ARM_SCORE_SPECIMEN + (getAlterationDegrees(alteration)));
+    }
+    public void moveArmToAttachHangingHookPosition(double alteration){
+        setArmPosition(ARM_ATTACH_HANGING_HOOK + (getAlterationDegrees(alteration)));
+    }
+    public void moveArmToWinchRobotPosition(double alteration){
+        setArmPosition(ARM_WINCH_ROBOT + (getAlterationDegrees(alteration)));
+    }
+    private void setArmPosition(double position) {
+        armMotor.setTargetPosition((int)(position));
+        ((DcMotorEx) armMotor).setVelocity(2100);
+        armMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+    }
+
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriveOnlyOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriveOnlyOpMode.java
@@ -31,6 +31,7 @@
 
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.util.ElapsedTime;
 
 
@@ -53,13 +54,19 @@ public class DriveOnlyOpMode extends LinearOpMode {
 
     // Declare OpMode members.
     private ElapsedTime runtime = new ElapsedTime();
+    DriveSubsystem driveSubsystem;
+    SlideSubsystem slideSubsystem;
+    ArmSubsystem arm;
+    WristSubsystem wrist;
 
     @Override
     public void runOpMode() {
         DriveSubsystem driveSubsystem = new DriveSubsystem(hardwareMap, telemetry);
         telemetry.addData("Status", "Initialized");
         telemetry.update();
-
+        slideSubsystem = new SlideSubsystem(hardwareMap, telemetry);
+        arm = new ArmSubsystem(hardwareMap, telemetry);
+        wrist = new WristSubsystem(hardwareMap, telemetry);
         // Wait for the game to start (driver presses START)
         waitForStart();
         runtime.reset();
@@ -76,8 +83,38 @@ public void runOpMode() {
                 reductionFactor = 4;
             }
 
+            // Move Slide to positions
+            if (gamepad2.dpad_up){
+                slideSubsystem.setPosition(SlideSubsystem.LIFT_SCORING_IN_HIGH_BASKET);
+            } else if (gamepad2.dpad_down) {
+                slideSubsystem.setPosition(SlideSubsystem.LIFT_COLLAPSED);
+            }
+
+            // Handles move arm to set positions with a fudge fa ctor
+            double fudgeFactorPercentage = gamepad2.right_trigger + (-gamepad2.left_trigger);
+            if (gamepad1.a){
+                arm.moveToArmToCollectPosition(fudgeFactorPercentage);
+            } else if (gamepad1.b){
+                arm.moveArmToClearBarrierPosition(fudgeFactorPercentage);
+            } else if (gamepad1.x){
+                arm.moveArmToScoreSampleInLowPosition(fudgeFactorPercentage);
+            } else if (gamepad1.dpad_left){
+                arm.moveArmToCollapsedIntoRobotPosition(fudgeFactorPercentage);
+            } else if (gamepad1.dpad_right) {
+                arm.moveArmToScoreSpecimenPosition(fudgeFactorPercentage);
+            } else if (gamepad1.dpad_up){
+                arm.moveArmToAttachHangingHookPosition(fudgeFactorPercentage);
+            } else if (gamepad1.dpad_down){
+                arm.moveArmToWinchRobotPosition(fudgeFactorPercentage);
+            }
+
+            if (gamepad2.left_bumper){
+                wrist.moveToPosition(.75);
+            } else if (gamepad2.left_trigger > 0){
+                wrist.moveToPosition(0);
+            }
+
             driveSubsystem.moveRobotCentric(forward, strafe, turn, reductionFactor);
-            driveSubsystem.setPower(forward, strafe, turn, reductionFactor);
 
             // Show the elapsed game time and wheel power.
             telemetry.addData("Status", "Run Time: " + runtime.toString());

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SlideSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SlideSubsystem.java
@@ -0,0 +1,49 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+
+public class SlideSubsystem {
+
+    private DcMotor slideMotor;
+    public static final String SLIDE_MOTOR="slide-motor";
+
+    public static final double LIFT_TICKS_PER_MM = (111132.0 / 289.0) / 120.0;
+
+    public static final double LIFT_COLLAPSED = 0 * LIFT_TICKS_PER_MM;
+    public static final double LIFT_SCORING_IN_LOW_BASKET = 0 * LIFT_TICKS_PER_MM;
+    public static final double LIFT_SCORING_IN_HIGH_BASKET = 480 * LIFT_TICKS_PER_MM;
+
+    double liftPosition = LIFT_COLLAPSED;
+
+    private HardwareMap hardwareMap;
+    private Telemetry telemetry;
+
+    public SlideSubsystem (HardwareMap hm, Telemetry telemetry){
+        this.hardwareMap = hm;
+        this.telemetry = telemetry;
+
+        slideMotor = hardwareMap.get(DcMotor.class, SLIDE_MOTOR);
+
+        slideMotor.setDirection(DcMotorSimple.Direction.REVERSE);
+        slideMotor.setTargetPosition(0);
+        slideMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        slideMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+    }
+
+    public void changePosition(int positionDelta){
+        this.liftPosition = this.liftPosition + positionDelta;
+        setPosition(this.liftPosition);
+    }
+
+    public void setPosition(double liftPosition) {
+        slideMotor.setTargetPosition((int) (liftPosition));
+
+        ((DcMotorEx) slideMotor).setVelocity(2100);
+        slideMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/WristSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/WristSubsystem.java
@@ -0,0 +1,26 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.Servo;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+
+public class WristSubsystem {
+    public static final String WRIST_SERVO = "wrist-servo";
+
+    private HardwareMap hardwareMap;
+    private Telemetry telemetry;
+
+    private Servo servo;
+
+    public WristSubsystem(HardwareMap hm, Telemetry telemetry){
+        this.hardwareMap = hm;
+        this.telemetry = telemetry;
+
+        servo = hardwareMap.get(Servo.class, WRIST_SERVO);
+    }
+
+    public void moveToPosition(double position){
+        servo.setPosition(position);
+    }
+}