diff --git a/TeamCode/src/main/java/ServoPractice.java b/TeamCode/src/main/java/ServoPractice.java new file mode 100644 index 0000000..c0ac775 --- /dev/null +++ b/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(){ + + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/SlideArm.java b/TeamCode/src/main/java/SlideArm.java deleted file mode 100644 index e69de29..0000000 diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SlideArm.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SlideArm.java new file mode 100644 index 0000000..fb55661 --- /dev/null +++ b/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(){ + + } + + +}