Something something arm stuff

TeamCode/src/main/java/ServoPractice.java

@@ -0,0 +1,27 @@
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.Servo;
+
+public class ServoPractice extends OpMode {
+    //servo
+    Servo test_servo;
+
+
+    public void init(){
+        //hw map
+        test_servo = hardwareMap.get(Servo.class, "test_servo");
+    }
+    public void loop(){
+        //make it move
+        test_servo.setPosition(0);
+        try {
+            Thread.sleep(500);
+        } catch (InterruptedException e) {
+            throw new RuntimeException(e);
+        }
+        test_servo.setPosition(1);
+    }
+    public void stop(){
+
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SlideArm.java

@@ -0,0 +1,165 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.Servo;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import org.firstinspires.ftc.robotcontroller.external.samples.UtilityOctoQuadConfigMenu;
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+
+
+@TeleOp(name = "ArmControl")
+public class SlideArm extends OpMode {
+
+
+
+    DcMotor Slide;
+    Servo ClawServo;
+    Servo ArmServo;
+    Servo WristServo;
+    boolean xPressed;
+    boolean yPressed;
+    boolean bPressed;
+    double ticks = 753.2;
+
+
+    public void init(){
+        Slide = hardwareMap.get(DcMotor.class, "SlideMotor");
+        ArmServo = hardwareMap.get(Servo.class,"WristServo");
+        WristServo = hardwareMap.get(Servo.class, "ArmServo");
+        ClawServo = hardwareMap.get(Servo.class, "ClawServo");
+        xPressed = false;
+        yPressed = false;
+        bPressed = false;
+    }
+
+    double power = 0;
+    double position = 1;
+
+    public void loop() {
+        Slide.setPower(-gamepad2.left_stick_y);
+        Slide.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        if (gamepad2.left_bumper) {
+            ClawServo.setPosition(0);
+        } else if (gamepad2.right_bumper) {
+            ClawServo.setPosition(1);
+        }
+
+        /*if(gamepad2.dpad_down){
+            WristServo.setPosition(0.78);
+            ArmServo.setPosition(0.55);
+        }
+        else if (gamepad2.dpad_up){
+            WristServo.setPosition(0.4);
+            ArmServo.setPosition(0.2);
+
+        }*/
+        if (gamepad2.a) {
+            Slide.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        }
+
+        if (gamepad2.dpad_down) {
+            SlideArm.ServoSteps(WristServo, 0.78, 4, 8);
+            try {
+                Thread.sleep(50);
+            } catch (InterruptedException e) {
+                throw new RuntimeException(e);
+            }
+            SlideArm.ServoSteps(ArmServo, 0.55, 10, 5);
+        } else if (gamepad2.dpad_up) {
+
+            WristServo.setPosition(0.4);
+            ArmServo.setPosition(0.2);
+
+        }
+
+        telemetry.addData("Slide Ticks", Slide.getCurrentPosition());
+        telemetry.update();
+        //limit = 6600;
+
+
+        if (gamepad2.x) {
+            xPressed = true;
+        }
+
+        while (xPressed && Slide.getCurrentPosition() < 6300) {
+            Slide.setPower(0.6);
+            Slide.setTargetPosition(6300);
+            Slide.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+            telemetry.addData("In While Loop:", Slide.getCurrentPosition());
+            telemetry.update();
+        }
+        if (xPressed && Slide.getCurrentPosition() >= 6300)
+        {
+            xPressed = false;
+        }
+
+        Slide.setPower(0);
+
+        if (gamepad2.y) {
+            yPressed = !true;
+        }
+
+        while (yPressed && Slide.getCurrentPosition() < 3150) {
+            Slide.setPower(0.6);
+            Slide.setTargetPosition(3150);
+            Slide.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+            telemetry.addData("In While Loop:", Slide.getCurrentPosition());
+            telemetry.update();
+        }
+        Slide.setPower(0);
+        if (yPressed && Slide.getCurrentPosition() >= 3150)
+        {
+            yPressed = false;
+        }
+        if (gamepad2.b) {
+            bPressed = !bPressed;
+        }
+
+        while (bPressed && Slide.getCurrentPosition() > 0) {
+            Slide.setPower(0.6);
+            Slide.setTargetPosition(0);
+            Slide.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+            telemetry.addData("In While Loop:", Slide.getCurrentPosition());
+            telemetry.update();
+        }
+        Slide.setPower(0);
+        if (xPressed && Slide.getCurrentPosition() >= 0)
+        {
+            bPressed = false;
+        } }
+
+    /**
+     * This function takes 4 parameters and makes your servo move in multiple steps for precision
+     * @param servo the servo you want to move
+     * @param targetPosition the position you want to go to
+     * @param Steps how many steps you want to move before reaching targetPosition
+     * @param millis how much to sleep between steps
+     */
+    public static void ServoSteps (Servo servo, double targetPosition, int Steps, long millis){
+        double startingPosition = servo.getPosition();
+        double howFarToMove = targetPosition-startingPosition;
+        double Increment = howFarToMove/Steps;
+        double currentPosition = startingPosition;
+        for(int i = 0; i < Steps; i ++) {
+            servo.setPosition(currentPosition + Increment);
+            currentPosition += Increment;
+            try {
+                Thread.sleep(millis);
+            } catch (InterruptedException e) {
+                throw new RuntimeException(e);
+            }
+        }
+
+    }
+
+
+
+    public void stop(){
+
+    }
+
+
+}