Merge pull request #25 from ftc16072/intake_arm_code

Intake arm code
ftc16072 · Nov 12, 2024 · 83f5004 · 83f5004
2 parents 7ec3a79 + 8b75271
commit 83f5004
Show file tree

Hide file tree

Showing 5 changed files with 116 additions and 3 deletions.
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/IntakeArm.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/IntakeArm.java
@@ -0,0 +1,60 @@
+package org.firstinspires.ftc.teamcode.ftc16072.Mechanisms;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.Servo;
+
+import org.firstinspires.ftc.teamcode.ftc16072.Tests.QQTest;
+import org.firstinspires.ftc.teamcode.ftc16072.Tests.TestTwoServos;
+
+import java.util.Arrays;
+import java.util.List;
+
+@Config
+public class IntakeArm extends QQMechanism {
+    //change values later
+    public static double ARM_DROP_POSITION = 0.7;
+    public static double ARM_INTAKE_POSITION = 0.9;
+    public static double SEARCHING_POSITION = 0.85;
+    public static double TRANSFER_POSITION = 0.8;
+    Servo leftArmServo;
+    Servo rightArmServo;
+
+    @Override
+    public void init(HardwareMap hwMap) {
+        leftArmServo = hwMap.get(Servo.class, "left_arm_servo");
+        rightArmServo = hwMap.get(Servo.class, "right_arm_servo");
+        rightArmServo.setDirection(Servo.Direction.REVERSE);
+    }
+    public void goToPos(double position){
+        leftArmServo.setPosition(position);
+        rightArmServo.setPosition(position);
+
+    }
+    public void goToDropPos(){
+        leftArmServo.setPosition(ARM_DROP_POSITION);
+        rightArmServo.setPosition(ARM_DROP_POSITION);
+    }
+    public void goToIntake(){
+        leftArmServo.setPosition(ARM_INTAKE_POSITION);
+        rightArmServo.setPosition(ARM_INTAKE_POSITION);
+    }
+    public void searching(){
+        leftArmServo.setPosition(SEARCHING_POSITION);
+        rightArmServo.setPosition(SEARCHING_POSITION);
+    }
+    public void transfer(){
+        leftArmServo.setPosition(TRANSFER_POSITION);
+        rightArmServo.setPosition(TRANSFER_POSITION);
+    }
+
+
+
+    @Override
+    public List<QQTest> getTests() {
+        return Arrays.asList(
+                new TestTwoServos("arm_pos", ARM_DROP_POSITION, ARM_INTAKE_POSITION, leftArmServo, rightArmServo));
+
+    }
+}
+
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/IntakeSlides.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/IntakeSlides.java
@@ -5,6 +5,7 @@
 import com.qualcomm.robotcore.hardware.DcMotor;
 
 import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.TouchSensor;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
 import org.firstinspires.ftc.teamcode.ftc16072.Tests.QQTest;
@@ -29,6 +30,7 @@ public class IntakeSlides extends QQMechanism {
 
     private DcMotor rightIntakeSlideMotor;
     private DcMotor leftIntakeSlideMotor;
+    private TouchSensor limitSwitch;
 
 
     public int currentPos;
@@ -59,6 +61,8 @@ public void init(HardwareMap hwMap) {
         leftIntakeSlideMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
         leftIntakeSlideMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
         leftIntakeSlideMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+
+        limitSwitch = hwMap.get(TouchSensor.class, "Slides_switch");
     }
     public void setPosition(int desiredPos){
         this.desiredPos = desiredPos;
@@ -86,6 +90,9 @@ public void update(){
         }
     }
 
+    public boolean isSwitchPressed = limitSwitch.isPressed();
+
+
 
 
     @Override

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/NoTurnTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/NoTurnTeleop.java
@@ -39,7 +39,7 @@ public void loop(){
             robot.claw.wristStart();
         }else if (gamepad1.dpad_left) {
             robot.claw.wristEnd();
-        }else if (gamepad1.dpad_down){
+        }else if (gamepad1.dpad_up){
             robot.scoreArm.manualPositionChange(MANUAL_CHANGE);
         }else if (gamepad1.dpad_down){
             robot.scoreArm.manualPositionChange(-MANUAL_CHANGE);
@@ -81,5 +81,14 @@ public void loop(){
         if(gamepad2.dpad_down){
             robot.intakeSlides.manualPositionChange(-5);
         }
+        if(gamepad2.left_bumper){
+            robot.intakeArm.goToIntake();
+        }
+        if(gamepad2.right_bumper){
+            robot.intakeArm.goToDropPos();
+        }
+        if(gamepad2.dpad_right){
+            robot.intakeArm.searching();
+        }
     }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Robot.java
@@ -4,9 +4,8 @@
 
 import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.Claw;
 import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.ControlHub;
-
+import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.IntakeArm;
 import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.IntakeClaw;
-
 import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.IntakeSlides;
 import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.MecanumDrive;
 import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.OpticalTrackingOdometrySensor;
@@ -22,6 +21,7 @@ public class Robot {
     public MecanumDrive mecanumDrive;
     public OpticalTrackingOdometrySensor otos;
     public Claw claw;
+    public IntakeArm intakeArm;
     //public DoubleReverse4Bar doubleReverse4Bar;
    // public Slides slides;
     public IntakeClaw intakeClaw;
@@ -41,6 +41,7 @@ public Robot() {
         otos = new OpticalTrackingOdometrySensor();
         claw = new Claw();
         intakeSlides = new IntakeSlides();
+        intakeArm = new IntakeArm();
         //doubleReverse4Bar = new DoubleReverse4Bar();
         //slides = new Slides();
 
@@ -51,6 +52,7 @@ public Robot() {
                 mecanumDrive,
                 // otos,
                 claw,
+                intakeArm,
                // slides,
                 //doubleReverse4Bar,
                 intakeSlides,

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Tests/TestTwoServos.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Tests/TestTwoServos.java
@@ -0,0 +1,35 @@
+package org.firstinspires.ftc.teamcode.ftc16072.Tests;
+
+import com.qualcomm.robotcore.hardware.Servo;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+
+public class TestTwoServos extends QQTest{
+    double onPosition;
+    double offPosition;
+    Servo servo;
+    Servo servo2;
+
+    public TestTwoServos(String name, double onPosition, double offPosition, Servo servo, Servo servo2){
+        super(name);
+        this.onPosition = onPosition;
+        this.offPosition = offPosition;
+        this.servo = servo;
+        this.servo2 = servo2;
+    }
+    @Override
+    public void run(boolean on, Telemetry telemetry) {
+        if(on){
+            servo.setPosition(onPosition);
+        }else{
+            servo.setPosition(offPosition);
+        }
+        if(on){
+            servo2.setPosition(onPosition);
+        }else{
+            servo2.setPosition(offPosition);
+        }
+        telemetry.addData("Position", servo.getPosition());
+        telemetry.addData("Position", servo2.getPosition());
+    }
+}