Merge pull request #37 from ftc16072/Merge-in-Auto

Settled conflicts between Increased Score Power and Master
ftc16072 · Dec 31, 2024 · a1751c6 · a1751c6
2 parents 3663598 + 6f3fdc4
commit a1751c6
Show file tree

Hide file tree

Showing 3 changed files with 18 additions and 0 deletions.
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ScoreArm.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ScoreArm.java
@@ -24,6 +24,7 @@ public class ScoreArm extends QQMechanism{
     TouchSensor limitSwitch;
     TouchSensor rightChamberContact;
     TouchSensor leftChamberContact;
+    public static final double SCORE_POWER = -0.8;
     public static double kP = 0.00159;
     public static double kI = 0.0;
     public static double kD = 0;
@@ -35,6 +36,7 @@ public class ScoreArm extends QQMechanism{
     protected int currentPos;
     public int desiredPos;
     public double motorPower;
+    boolean isScoring;
 
     public static int INTAKE_POSITION = 0;
     public static int SCORING_POSITION = 200;
@@ -82,6 +84,7 @@ public void update(Telemetry telemetry){
             leftMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
             rightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
             rightMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+            isScoring = false;
             if (desiredPos < 0){
                 desiredPos = 0;
             }
@@ -91,13 +94,19 @@ public void update(Telemetry telemetry){
         this.motorPower = motorPower;
         leftMotor.setPower(motorPower);
         rightMotor.setPower(motorPower);
+        if (isScoring){
+            motorPower = SCORE_POWER;
+        }
         chamberContacted = rightChamberContact.isPressed() || leftChamberContact.isPressed();
         //pidfController.updateConstants(kP,kI,kD,kF,max,min);
         telemetry.addData("curerent pos",currentPos);
         telemetry.addData("desired pos",desiredPos);
         telemetry.addData("motor power",motorPower);
 
 
+    }
+    public void setNotScoring(){
+        isScoring = false;
     }
     public boolean isChamberContacted(){
         return chamberContacted;

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ScoringClaw.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ScoringClaw.java
@@ -86,6 +86,7 @@ public void close() {
         clawServo.setPosition(CLAW_CLOSE_POSITION);
         closedTimer.reset();
     }
+    public boolean isScoreSwitchPressed(){ return !scoreSwitch.isPressed();}
 
     @Override
     public void update(Telemetry telemetry) {

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
@@ -57,6 +57,7 @@ public void loop() {
             if (robot.intakeSlides.isSwitchPressed()) {
                 robot.scoreArm.goToIntake();
                 robot.scoringClaw.open();
+                robot.scoreArm.setNotScoring();
             } else {
                 robot.intakeSlides.startPosition();
             }
@@ -74,6 +75,9 @@ public void loop() {
         } else if (gamepad1.dpad_down) {
             robot.scoreArm.manualPositionChange(-SCORE_ARM_MANUAL_CHANGE);
         }
+        if (robot.scoringClaw.isScoreSwitchPressed()){
+            robot.scoreArm.setNotScoring();
+        }
         if (gamepad1.left_stick_button) {
             robot.controlHub.resetGyro();
         }
@@ -111,13 +115,16 @@ public void loop() {
         if (gamepad2.b) {
             robot.scoreArm.goToPlace();
             robot.intakeArm.goToIntake();
+            robot.scoreArm.setNotScoring();
         } else if (gamepad2.x) {
             robot.scoreArm.goToScoring();
             robot.intakeArm.goToIntake();
+            robot.scoreArm.setNotScoring();
         } else if (manipulatorXWasPressed) {
             robot.scoringClaw.open();
         } else if (gamepad2.y) {
             robot.scoreArm.goToMove();
+            robot.scoreArm.setNotScoring();
         }
         if (gamepad2.dpad_up) {
             robot.intakeSlides.extend();
@@ -126,6 +133,7 @@ public void loop() {
         }
         if (gamepad2.right_trigger > TRIGGER_THRESHOLD) {
             robot.scoringClaw.open();
+            robot.scoreArm.setNotScoring();
         } else if (gamepad2.right_bumper) {
             robot.scoringClaw.close();
         }