From 3849265627ed7371c0fa83a8966e72c8db04f49c Mon Sep 17 00:00:00 2001
From: Carlos <carlos@riv.as>
Date: Wed, 13 Nov 2024 15:29:15 -0800
Subject: [PATCH] Added speed control as per issue #3

---
 .../cometbots/CometBotTeleopCompetition.java  | 42 ++++++++++++++++++-
 1 file changed, 41 insertions(+), 1 deletion(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotTeleopCompetition.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotTeleopCompetition.java
index b811def..aef5d17 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotTeleopCompetition.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotTeleopCompetition.java
@@ -38,10 +38,22 @@ public class CometBotTeleopCompetition {
     public Gamepad previousGP1;
     public Gamepad currentGP2;
     public Gamepad previousGP2;
+
+    /*
+    Pedro/FTC Components
+     */
+    private Follower follower;
     private Telemetry telemetry;
+
+    /*
+    States
+     */
     public FieldStates fieldStates;
 
-    private Follower follower;
+    /*
+    Configurations
+     */
+    public double currentPower = MAX_POWER;
 
     public CometBotTeleopCompetition(HardwareMap hardwareMap, Telemetry telemetry, Gamepad gp1, Gamepad gp2) {
         this.motors = new MotorsSubsystem(hardwareMap, telemetry, .55);
@@ -82,6 +94,8 @@ public class CometBotTeleopCompetition {
         this.toArmParkPosition();
         this.toArmParkThenSwitchBetweenSubmarineAndFloorPosition();
         this.clawControl();
+        this.decreaseMaxPower();
+        this.increaseMaxPower();
 
         follower.setTeleOpMovementVectors(-this.GP1.left_stick_y, -this.GP1.left_stick_x, -this.GP1.right_stick_x, CENTRICITY);
         follower.update();
@@ -95,7 +109,33 @@ public class CometBotTeleopCompetition {
         this.telemetry.addData("Lift Position", this.lift.getPosition());
     }
 
+    /*
+    Type: PS4 / Logitech
+    Controller: 1
+    Button: Left Bumper
+    Assumption: Working motor mechanism
+    Action: Decreases maximum speed by -.05
+     */
+    public void decreaseMaxPower() {
+        if (this.currentGP1.left_bumper && !this.previousGP1.left_bumper) {
+            this.currentPower = this.currentPower - .05;
+            this.follower.setMaxPower(this.currentPower);
+        }
+    }
 
+    /*
+    Type: PS4 / Logitech
+    Controller: 1
+    Button: Left Bumper
+    Assumption: Working motor mechanism
+    Action: Increases maximum speed by +.05
+     */
+    public void increaseMaxPower() {
+        if (this.currentGP1.left_bumper && !this.previousGP1.left_bumper) {
+            this.currentPower = this.currentPower + .05;
+            this.follower.setMaxPower(this.currentPower);
+        }
+    }
 
     /*
     Type: PS4 / Logitech