Goog

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java

@@ -1,27 +1,5 @@
 package org.firstinspires.ftc.teamcode.hardware;
 
-import static org.firstinspires.ftc.teamcode.util.Configurables.ARMACCSCORE;
-import static org.firstinspires.ftc.teamcode.util.Configurables.ARMPICKUPSTACK;
-import static org.firstinspires.ftc.teamcode.util.Configurables.ARMREST;
-import static org.firstinspires.ftc.teamcode.util.Configurables.ARMSCORE;
-import static org.firstinspires.ftc.teamcode.util.Configurables.BIGOPEN;
-import static org.firstinspires.ftc.teamcode.util.Configurables.CLOSE;
-import static org.firstinspires.ftc.teamcode.util.Configurables.HANG;
-import static org.firstinspires.ftc.teamcode.util.Configurables.HANGPLANE;
-import static org.firstinspires.ftc.teamcode.util.Configurables.HANGRELEASE;
-import static org.firstinspires.ftc.teamcode.util.Configurables.OPEN;
-import static org.firstinspires.ftc.teamcode.util.Configurables.PICKUP;
-import static org.firstinspires.ftc.teamcode.util.Configurables.PLANELAUNCH;
-import static org.firstinspires.ftc.teamcode.util.Configurables.PLANELOCK;
-import static org.firstinspires.ftc.teamcode.util.Configurables.SLIDEAUTOSTACKS;
-import static org.firstinspires.ftc.teamcode.util.Configurables.SLIDELAYERONE;
-import static org.firstinspires.ftc.teamcode.util.Configurables.SLIDELAYERTWO;
-import static org.firstinspires.ftc.teamcode.util.Configurables.SLIDEPICKUPSTACKSTWO;
-import static org.firstinspires.ftc.teamcode.util.Configurables.SLIDEUP;
-import static org.firstinspires.ftc.teamcode.util.Configurables.SLIDE_POWER_DOWN;
-import static org.firstinspires.ftc.teamcode.util.Configurables.SLIDE_POWER_UP;
-import static org.firstinspires.ftc.teamcode.util.Configurables.WRISTPICKUP;
-import static org.firstinspires.ftc.teamcode.util.Configurables.WRISTSCORE;
 import static org.firstinspires.ftc.teamcode.util.Constants.CLAW;
 import static org.firstinspires.ftc.teamcode.util.Constants.HANGLEFT;
 import static org.firstinspires.ftc.teamcode.util.Constants.HANGRIGHT;
@@ -35,6 +13,7 @@ import static org.firstinspires.ftc.teamcode.util.Constants.WRIST;
 
 import com.acmerobotics.dashboard.config.Config;
 import com.arcrobotics.ftclib.controller.PController;
+import com.arcrobotics.ftclib.controller.PIDFController;
 import com.arcrobotics.ftclib.gamepad.GamepadEx;
 import com.arcrobotics.ftclib.gamepad.GamepadKeys;
 import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
@@ -48,15 +27,21 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.Tra
 import org.firstinspires.ftc.teamcode.vision.Camera;
 
 import lombok.Getter;
+
 @Config
 public class Robot {
+    public static boolean clawIsOpen;
+    public static double WRISTDELAY = .3;
+    double delay;
+    public pickupMacroStates pickupMacroState = pickupMacroStates.IDLE;
+    public armMacroStates armMacroState = armMacroStates.IDLE;
+    @Getter
+    public Arm arm;
     @Getter
     private MecanumDrive drive;
     @Getter
     private Led led;
     @Getter
-    private Arm arm;
-    @Getter
     private Wrist wrist;
     @Getter
     private Claw claw;
@@ -69,8 +54,6 @@ public class Robot {
     @Getter
     private Slides slides;
 
-
-
     public Robot init(HardwareMap hardwareMap) {
         this.drive = new MecanumDrive(hardwareMap);
         this.hang = new Hang().init(hardwareMap);
@@ -79,12 +62,100 @@ public class Robot {
         this.claw = new Claw().init(hardwareMap);
 //        this.camera = new Camera(hardwareMap);
         this.plane = new Plane().init(hardwareMap);
-        this.slides= new Slides().init(hardwareMap);
+        this.slides = new Slides().init(hardwareMap);
 //        this.led = new Led().init(hardwareMap);
         return this;
     }
 
+    public void pickupMacro(GamepadEx gamepadEx, double runtime) {
+        switch (pickupMacroState) {
+            case IDLE:
+                if (gamepadEx.wasJustReleased(GamepadKeys.Button.DPAD_DOWN)) {
+                    pickupMacroState = pickupMacroStates.DROP;
+                }
+                break;
+            case OPEN:
+                delay = runtime + .3;
+                this.getClaw().open();
+                pickupMacroState = pickupMacroStates.DROP;
+                break;
+            case DROP:
+                if (runtime > delay) {
+                    this.getArm().pickup(true);
+                    delay = runtime + .2;
+                    pickupMacroState = pickupMacroStates.CLOSE;
+                }
+                break;
+            case CLOSE:
+                if (runtime > delay) {
+                    this.getClaw().close();
+                    delay = runtime + .3;
+                    pickupMacroState = pickupMacroStates.NEUTRAL;
+                }
+                break;
+            case NEUTRAL:
+                if (runtime > delay) {
+                    this.getArm().armRest(true);
+                    pickupMacroState = pickupMacroStates.IDLE;
+                }
+                break;
+
+        }
+    }
+
+    public void armMacro(GamepadEx gamepadEx, double runtime) {
+        switch (armMacroState) {
+            case IDLE:
+                if (gamepadEx.wasJustPressed(GamepadKeys.Button.X)) {
+                    armMacroState = armMacroStates.ARMSWING;
+                }
+                break;
+            case ARMSWING:
+                arm.armSecondaryScore();
+                delay = runtime + WRISTDELAY;
+                armMacroState = armMacroStates.WRIST;
+                break;
+            case WRIST:
+                if (runtime > delay) {
+                    wrist.wristScore();
+                    armMacroState = armMacroStates.IDLE;
+                }
+                break;
+        }
+    }
+
+    public TrajectorySequenceBuilder getTrajectorySequenceBuilder() {
+        this.drive.update();
+        return this.drive.trajectorySequenceBuilder(this.drive.getPoseEstimate());
+    }
+
+    public void update() {
+        this.arm.update();
+        this.wrist.update();
+        this.drive.update();
+    }
+
+    public enum pickupMacroStates {
+        IDLE, OPEN, DROP, CLOSE, NEUTRAL
+
+    }
+
+    public enum armMacroStates {
+        IDLE, ARMSWING, WRIST
+    }
+
+    @Config
     public static class Slides {
+        //Values
+        public static double SLIDE_POWER_UP = .7;
+        public static double SLIDE_POWER_DOWN = .5;
+        public static int SLIDELAYERONE = 60;
+        public static int SLIDEAUTOSTACKS = 250;
+        public static int SLIDEUP = 1150;
+        public static int SLIDELAYERTWO = 350;
+        public static int SLIDESTACK = 80;
+        public static int SLIDEPICKUPSTACKSTWO = 30;
+        //Motors
         public DcMotor slidesRight = null;
         public DcMotor slidesLeft = null;
 
@@ -116,17 +187,29 @@ public class Robot {
             this.slidesRight.setPower(power);
         }
 
-        public void slideUp(){this.slideTo(SLIDEUP, SLIDE_POWER_UP);}
+        public void slideUp() {
+            this.slideTo(SLIDEUP, SLIDE_POWER_UP);
+        }
 
-        public void slideDown(){this.slideTo(0, SLIDE_POWER_DOWN);}
+        public void slideDown() {
+            this.slideTo(0, SLIDE_POWER_DOWN);
+        }
 
-        public void slideFirstLayer(){this.slideTo(SLIDELAYERONE, SLIDE_POWER_UP);}
+        public void slideFirstLayer() {
+            this.slideTo(SLIDELAYERONE, SLIDE_POWER_UP);
+        }
 
-        public void slideScoreStack(){this.slideTo(SLIDELAYERTWO, SLIDE_POWER_UP);}
+        public void slideScoreStack() {
+            this.slideTo(SLIDELAYERTWO, SLIDE_POWER_UP);
+        }
 
-        public void slideAutoStacks(){this.slideTo(SLIDEAUTOSTACKS, SLIDE_POWER_UP);}
+        public void slideAutoStacks() {
+            this.slideTo(SLIDEAUTOSTACKS, SLIDE_POWER_UP);
+        }
 
-        public void slidePickupStackTwo(){this.slideTo(SLIDEPICKUPSTACKSTWO, SLIDE_POWER_UP);}
+        public void slidePickupStackTwo() {
+            this.slideTo(SLIDEPICKUPSTACKSTWO, SLIDE_POWER_UP);
+        }
 
         public void slideStop() {
             this.slidesLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
@@ -137,11 +220,18 @@ public class Robot {
             this.slidesRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
             this.slidesRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
             this.slidesRight.setTargetPosition(this.slidesRight.getCurrentPosition());
-            this.slidesRight.setPower(1);}
+            this.slidesRight.setPower(1);
+        }
 
     }
 
+    @Config
     public static class Hang {
+        //Values
+        public static int HANGRELEASE = 1550;
+        public static int HANG = 0;
+        public static int HANGPLANE = 1150;
+        //Motors
         public DcMotor hangLeft = null;
         public DcMotor hangRight = null;
 
@@ -161,43 +251,48 @@ public class Robot {
             return this;
         }
 
-        public void hangTo(int hangPos, double daPower) {
+        public void hangTo(int hangPos, double Power) {
             this.hangLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
             this.hangLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
             this.hangLeft.setTargetPosition(hangPos);
-            this.hangLeft.setPower(daPower);
+            this.hangLeft.setPower(Power);
 
             this.hangRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
             this.hangRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
             this.hangRight.setTargetPosition(hangPos);
-            this.hangRight.setPower(daPower);
+            this.hangRight.setPower(Power);
         }
 
-        public void hangRelease(){
+        public void hangRelease() {
-            this.hangTo(HANGRELEASE,1);
+            this.hangTo(HANGRELEASE, 1);
         }
 
-        public void hang(){
+        public void hang() {
-            this.hangTo(HANG,1);
+            this.hangTo(HANG, 1);
-        }
-        public void hangPlane(){
-            this.hangTo(HANGPLANE,1);
         }
 
-        public void hangIdle() {
+        public void hangPlane() {
-            this.hangLeft.setPower(0);
+            this.hangTo(HANGPLANE, 1);
-            this.hangRight.setPower(0);
+        }
-        }
-
-
-
     }
 
+    @Config
     public static class Arm {
+        //Values
+        public static double DIFFY_KP = 0.1;
+        public static double DIFFY_TOL = 0.01;
+        public static double ARM_MAX_DELTA = 0.02;
+        public static double ARMREST = 0.89;
+        public static double ARMSCORE = 0.4;
+        public static double ARMACCSCORE = .57;
+        public static double ARMPICKUPSTACK = 0.815;
+        public static double PICKUP = 0.92;
+        //PControler
+        public PController armPController;
+        private double armTarget;
+        //Servos
         private Servo leftArm;
         private Servo rightArm;
-        private PController armController;
-
 
         public Arm init(HardwareMap hardwareMap) {
             this.leftArm = hardwareMap.get(Servo.class, LEFTARM);
@@ -205,62 +300,143 @@ public class Robot {
             this.rightArm.setDirection(Servo.Direction.REVERSE);
             this.rightArm.setPosition(ARMREST);
             this.leftArm.setPosition(ARMREST);
+            this.armPController = new PController(DIFFY_KP);
+            this.armRest(true);
             return this;
         }
 
         public void pickup() {
-            this.leftArm.setPosition(PICKUP);
+            pickup(false);
-            this.rightArm.setPosition(PICKUP);
+        }
+
+        public void pickup(boolean now) {
+            moveArm(PICKUP, now);
         }
 
         public void armScore() {
-            this.leftArm.setPosition(ARMSCORE);
+            this.armSecondaryScore();
-            this.rightArm.setPosition(ARMSCORE);
         }
 
         public void armSecondaryScore() {
-            this.leftArm.setPosition(ARMACCSCORE);
+            moveArm(ARMACCSCORE, false);
-            this.rightArm.setPosition(ARMACCSCORE);
         }
 
-
         public void armPickupStack() {
-            this.leftArm.setPosition(ARMPICKUPSTACK);
+            moveArm(ARMPICKUPSTACK, false);
-            this.rightArm.setPosition(ARMPICKUPSTACK);
         }
 
         public void armRest() {
-            this.leftArm.setPosition(ARMREST);
+            armRest(false);
-            this.rightArm.setPosition(ARMREST);
+        }
+
+        public void armRest(boolean now) {
+            moveArm(ARMREST, now);
+        }
+
+        private void moveArm(double position, boolean now) {
+            this.armTarget = position;
+            this.armPController.setSetPoint(this.armTarget);
+            if (now) {
+                this.leftArm.setPosition(position);
+                this.rightArm.setPosition(position);
             }
         }
 
+        public boolean isAtTarget() {
+            return this.armPController.atSetPoint();
+        }
+
+        public void update() {
+            this.armPController.setSetPoint(this.armTarget);
+            this.armPController.setTolerance(DIFFY_TOL);
+            this.armPController.setP(DIFFY_KP);
+
+            if (!isAtTarget()) {
+                double armDelta = this.armPController.calculate(leftArm.getPosition());
+                if (Math.abs(this.armPController.getPositionError()) > 0.1) {
+                    armDelta = Math.copySign(ARM_MAX_DELTA, armDelta);
+                }
+                this.leftArm.setPosition(leftArm.getPosition() + armDelta);
+                this.rightArm.setPosition(rightArm.getPosition() + armDelta);
+            }
+
+//
+//            this.rightPController.setSetPoint(this.rightTarget);
+//            this.rightPController.setTolerance(DIFFY_TOL);
+//            this.rightPController.setP(DIFFY_KP);
+//            double rightDelta = this.rightPController.calculate(rightArm.getPosition());
+//            rightDelta = Math.max(-1 * DIFFY_MAX_DELTA, Math.min(DIFFY_MAX_DELTA, rightDelta));
+//            this.rightArm.setPosition(rightArm.getPosition() + rightDelta);
+
+            // A is where I am
+            // B is where I want to get to
+            // E is the difference between them
+            // X is the midpoint between A and B
+            // while I am at on the left side of X, delta should be constant
+            // once I am on the right side of X, delta should be whatever ther P controller says
+
+
+        }
+
+    }
+
+    @Config
     public static class Wrist {
+        //Pcontroller
+        public static double KP = 0.2;
+        public static double TOL = 0.005;
+        public static double MAX_DELTA = 0.04;
+        private PIDFController wristPController;
+        //Values
+        public static double WRISTPICKUP = 0.3;
+        public static double WRISTSCORE = .98;
+        //Servo
         private Servo wrist;
 
         public Wrist init(HardwareMap hardwareMap) {
             this.wrist = hardwareMap.get(Servo.class, WRIST);
             this.wrist.setPosition(WRISTPICKUP);
+            this.wristPController = new PController(KP);
+            this.wristPController.setSetPoint(WRISTPICKUP);
             return this;
         }
 
         public void wristPickup() {
-            this.wrist.setPosition(WRISTPICKUP);
+            this.wristPController.setSetPoint(WRISTPICKUP);
         }
 
         public void wristScore() {
-            this.wrist.setPosition(WRISTSCORE);
+            this.wristPController.setSetPoint(WRISTSCORE);
         }
 
+        public void update() {
+            this.wristPController.setTolerance(TOL);
+            this.wristPController.setP(KP);
+
+            if (!wristPController.atSetPoint()) {
+                double wristDelta = this.wristPController.calculate(wrist.getPosition());
+                if (Math.abs(this.wristPController.getPositionError()) > 0.1) {
+                    wristDelta = Math.copySign(MAX_DELTA, wristDelta);
+                }
+                this.wrist.setPosition(wrist.getPosition() + wristDelta);
+            }
+        }
     }
 
-    public static boolean clawIsOpen;
+    @Config
-
     public static class Claw {
+        //Values
+        public static double OPEN = 0.45;
+        public static double BIGOPEN = 0f;
+        public static double CLOSE = 0.85;
+        public static double CLAW_MIN = 0.46;
+        public static double CLAW_MAX = 0.5;
+        //Servo
         private Servo claw;
 
         public Claw init(HardwareMap hardwareMap) {
             this.claw = hardwareMap.get(Servo.class, CLAW);
+            this.claw.scaleRange(CLAW_MIN, CLAW_MAX);
             close();
             return this;
         }
@@ -286,6 +462,7 @@ public class Robot {
 
     }
 
+    @Config
     public static class Led {
         private RevBlinkinLedDriver led;
 
@@ -311,7 +488,12 @@ public class Robot {
         }
     }
 
+    @Config
     public static class Plane {
+        //Values
+        public static double PLANELOCK = 0.1;
+        public static double PLANELAUNCH = 0.9;
+        //Servo
         private Servo plane;
 
         public Plane init(HardwareMap hardwareMap) {
@@ -329,59 +511,5 @@ public class Robot {
         }
 
     }
-
-    public void pickupMacro(GamepadEx gamepadEx, double runtime) {
-        switch (pickupMacroState) {
-            case IDLE:
-                if (gamepadEx.wasJustPressed(GamepadKeys.Button.DPAD_DOWN)){
-                    pickupMacroState = pickupMacroStates.OPEN;
-                }
-                break;
-            case OPEN:
-                    delay = runtime + .3;
-                    this.getClaw().open();
-                    pickupMacroState = pickupMacroStates.DROP;
-                break;
-            case DROP:
-                    if (runtime > delay) {
-                        this.getArm().pickup();
-                        delay= runtime + .2;
-                        pickupMacroState = pickupMacroStates.CLOSE;
-                    }
-                break;
-            case CLOSE:
-                if (runtime > delay) {
-                    this.getClaw().close();
-                    delay= runtime + .3;
-                    pickupMacroState = pickupMacroStates.NEUTRAL;
-                }
-                break;
-            case NEUTRAL:
-                if (runtime > delay) {
-                    this.getArm().armRest();
-                    pickupMacroState = pickupMacroStates.IDLE;
-                }
-                break;
-
-        }
-    }
-
-    public pickupMacroStates pickupMacroState = pickupMacroStates.IDLE;
-
-    public enum pickupMacroStates{
-                IDLE,
-                OPEN,
-                DROP,
-                CLOSE,
-                NEUTRAL
-
-        }
-
-    double delay;
-
-    public TrajectorySequenceBuilder getTrajectorySequenceBuilder() {
-        this.drive.update();
-        return this.drive.trajectorySequenceBuilder(this.drive.getPoseEstimate());
-    }
 }
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java

@@ -11,10 +11,10 @@ import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveCons
 import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kA;
 import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kStatic;
 import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kV;
-import static org.firstinspires.ftc.teamcode.util.Configurables.SLOWMODE_SPEED;
+import static org.firstinspires.ftc.teamcode.util.Configurables.driveSpeed.SLOWMODE_SPEED;
-import static org.firstinspires.ftc.teamcode.util.Configurables.SLOWMODE_TURN;
+import static org.firstinspires.ftc.teamcode.util.Configurables.driveSpeed.SLOWMODE_TURN;
-import static org.firstinspires.ftc.teamcode.util.Configurables.SPEED;
+import static org.firstinspires.ftc.teamcode.util.Configurables.driveSpeed.SPEED;
-import static org.firstinspires.ftc.teamcode.util.Configurables.TURN;
+import static org.firstinspires.ftc.teamcode.util.Configurables.driveSpeed.TURN;
 
 import androidx.annotation.NonNull;
 
@@ -329,9 +329,9 @@ public class    MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDr
 
         this.setWeightedDrivePower(
                 new Pose2d(
-                        gamepad1.left_stick_y* -1 * speedScale,
+                        gamepad2.left_stick_y * -1 * speedScale,
-                        gamepad1.left_stick_x*-1 * speedScale,
+                        gamepad2.left_stick_x * -1 * speedScale,
-                        -gamepad1.right_stick_x * turnScale
+                        -gamepad2.right_stick_x * turnScale
                 ));
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBase.java

@@ -1,7 +1,6 @@
 package org.firstinspires.ftc.teamcode.opmodes;
 
 import com.acmerobotics.dashboard.FtcDashboard;
-import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.acmerobotics.roadrunner.geometry.Pose2d;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
@@ -9,7 +8,6 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import org.firstinspires.ftc.teamcode.hardware.Robot;
 import org.firstinspires.ftc.teamcode.opmodes.AutoRedFarTwoPlusTwo.autoState;
 
-@Config
 public abstract class AutoBase extends LinearOpMode {
     protected Pose2d initialPosition;
     Robot robot;
@@ -37,11 +35,11 @@ public abstract class AutoBase extends LinearOpMode {
             this.telemetry.addData("Park Position", parkLocation);
             this.telemetry.addData("Delay", delay);
             this.telemetry.update();
-
         }
+
         while (state != autoState.STOP) {
             followTrajectories();
-            robot.getDrive().update();
+            robot.update();
         }
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlue.java

@@ -1,7 +1,6 @@
 package org.firstinspires.ftc.teamcode.opmodes;
 
 import com.acmerobotics.dashboard.FtcDashboard;
-import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.acmerobotics.roadrunner.geometry.Pose2d;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
@@ -10,7 +9,6 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import org.firstinspires.ftc.teamcode.hardware.Robot;
 import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
 
-@Config
 @Autonomous(name = "autoBlue")
 public class AutoBlue extends LinearOpMode {
     protected Pose2d initialPosition;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueFarStem.java

@@ -1,7 +1,6 @@
 package org.firstinspires.ftc.teamcode.opmodes;
 
 import com.acmerobotics.dashboard.FtcDashboard;
-import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.acmerobotics.roadrunner.geometry.Pose2d;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
@@ -10,7 +9,6 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import org.firstinspires.ftc.teamcode.hardware.Robot;
 import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
 
-@Config
 @Autonomous(name = "autoBlueFar")
 public class AutoBlueFarStem extends LinearOpMode {
     protected Pose2d initialPosition;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueTwoPlusTwo.java

@@ -1,7 +1,6 @@
 package org.firstinspires.ftc.teamcode.opmodes;
 
 import com.acmerobotics.dashboard.FtcDashboard;
-import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.acmerobotics.roadrunner.geometry.Pose2d;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
@@ -12,7 +11,6 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants;
 import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
 import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
 
-@Config
 @Autonomous(name = "autoBlue2+2")
 public class AutoBlueTwoPlusTwo extends LinearOpMode {
     protected Pose2d initialPosition;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRed.java

@@ -1,7 +1,6 @@
 package org.firstinspires.ftc.teamcode.opmodes;
 
 import com.acmerobotics.dashboard.FtcDashboard;
-import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.acmerobotics.roadrunner.geometry.Pose2d;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
@@ -12,7 +11,6 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants;
 import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
 import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
 
-@Config
 @Autonomous(name = "autoRed")
 public class AutoRed extends LinearOpMode {
     protected Pose2d initialPosition;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFar.java

@@ -1,7 +1,6 @@
 package org.firstinspires.ftc.teamcode.opmodes;
 
 import com.acmerobotics.dashboard.FtcDashboard;
-import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.acmerobotics.roadrunner.geometry.Pose2d;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
@@ -12,7 +11,6 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants;
 import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
 import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
 
-@Config
 @Autonomous(name = "AutoRedFar")
 public class AutoRedFar extends LinearOpMode {
     protected Pose2d initialPosition;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFarTwoPlusTwo.java

@@ -1,6 +1,5 @@
 package org.firstinspires.ftc.teamcode.opmodes;
 
-import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.roadrunner.geometry.Pose2d;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 
@@ -8,7 +7,6 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants;
 import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
 import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
 
-@Config
 @Autonomous(name = "AutoRedFar2+2",preselectTeleOp = "Main TeleOp")
 public class AutoRedFarTwoPlusTwo extends AutoBase {
     //Pose2ds

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFaronepluszero.java

@@ -1,7 +1,6 @@
 package org.firstinspires.ftc.teamcode.opmodes;
 
 import com.acmerobotics.dashboard.FtcDashboard;
-import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.acmerobotics.roadrunner.geometry.Pose2d;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
@@ -12,7 +11,6 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants;
 import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
 import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
 
-@Config
 @Autonomous(name = "AutoRedFar1+0")
 public class AutoRedFaronepluszero extends LinearOpMode {
     protected Pose2d initialPosition;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedTwoPlusTwo.java

@@ -1,7 +1,6 @@
 package org.firstinspires.ftc.teamcode.opmodes;
 
 import com.acmerobotics.dashboard.FtcDashboard;
-import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.acmerobotics.roadrunner.geometry.Pose2d;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
@@ -11,8 +10,9 @@ import org.firstinspires.ftc.teamcode.hardware.Robot;
 import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants;
 import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
 import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
-@Config
+
 @Autonomous(name = "autoRed2+2")
+
 public class AutoRedTwoPlusTwo extends LinearOpMode {
     protected Pose2d initialPosition;
     private Robot robot;
@@ -37,7 +37,7 @@ public class AutoRedTwoPlusTwo extends LinearOpMode {
     final static Pose2d APPROACHING_BOARD = new Pose2d(70, -31, Math.toRadians(360));
     final static Pose2d SCORE_STACK = new Pose2d(73.5, -31, Math.toRadians(360));
     //Park
-    final static Pose2d BACK_OFF =  new Pose2d(60,-58,Math.toRadians(360));
+    final static Pose2d BACK_OFF = new Pose2d(60, -58, Math.toRadians(360));
     final static Pose2d PARK = new Pose2d(80, -60, Math.toRadians(360));
 
     protected void scorePreloadOne() {
@@ -64,7 +64,8 @@ public class AutoRedTwoPlusTwo extends LinearOpMode {
             case "LEFT":
                 builder.lineToLinearHeading(LEFT_BOARD,
                         MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
-                        MecanumDrive.getAccelerationConstraint(20));;
+                        MecanumDrive.getAccelerationConstraint(20));
+                ;
                 break;
             case "CENTER":
                 builder.lineToLinearHeading(CENTER_BOARD,
@@ -89,7 +90,7 @@ public class AutoRedTwoPlusTwo extends LinearOpMode {
         builder.addTemporalMarker(.3, robot.getArm()::armRest);
         builder.addTemporalMarker(.3, robot.getWrist()::wristPickup);
         builder.addTemporalMarker(.1, robot.getSlides()::slideDown);
-        builder.addTemporalMarker(1.5,robot.getClaw()::openStack);
+        builder.addTemporalMarker(1.5, robot.getClaw()::openStack);
         builder.addTemporalMarker(1.5, robot.getArm()::pickup);
         builder.lineToLinearHeading(TO_STACK);
         builder.lineToLinearHeading(TO_STACK_SLOW,
@@ -104,7 +105,7 @@ public class AutoRedTwoPlusTwo extends LinearOpMode {
         builder.addTemporalMarker(.3, robot.getArm()::armRest);
         builder.addTemporalMarker(.3, robot.getWrist()::wristPickup);
         builder.addTemporalMarker(.1, robot.getSlides()::slideDown);
-        builder.addTemporalMarker(1.5,robot.getClaw()::openStack);
+        builder.addTemporalMarker(1.5, robot.getClaw()::openStack);
         builder.addTemporalMarker(1.5, robot.getArm()::pickup);
         builder.lineToLinearHeading(TO_STACK);
         builder.lineToLinearHeading(TO_STACK_SLOW2,
@@ -129,7 +130,7 @@ public class AutoRedTwoPlusTwo extends LinearOpMode {
         builder.lineToLinearHeading(new Pose2d(75, -31, Math.toRadians(360)),
                 MecanumDrive.getVelocityConstraint(20, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
                 MecanumDrive.getAccelerationConstraint(20));
-        builder.addTemporalMarker(.2,this::clawSlowOpen);
+        builder.addTemporalMarker(.2, this::clawSlowOpen);
         this.robot.getDrive().followTrajectorySequence(builder.build());
     }
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/MainTeleOp.java

@@ -6,14 +6,13 @@ import com.qualcomm.robotcore.eventloop.opmode.OpMode;
 
 import org.firstinspires.ftc.teamcode.hardware.Robot;
 import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
-import org.firstinspires.ftc.teamcode.util.Configurables;
 
 @com.qualcomm.robotcore.eventloop.opmode.TeleOp(name = "Main TeleOp", group = "Development")
 public class MainTeleOp extends OpMode {
-    private Robot robot;
-    private MecanumDrive drive;
     GamepadEx controller1;
     GamepadEx controller2;
+    private Robot robot;
+    private MecanumDrive drive;
 
     @Override
     public void init() {
@@ -22,15 +21,15 @@ public class MainTeleOp extends OpMode {
         this.controller1 = new GamepadEx(this.gamepad1);
     }
 
-
-
     public void loop() {
+//GamePad Controls
         boolean slideUp = controller2.isDown(GamepadKeys.Button.DPAD_UP);
         boolean slideDown = controller2.isDown(GamepadKeys.Button.DPAD_LEFT);
         boolean hang = gamepad2.left_bumper;
         boolean hangRelease = gamepad2.right_bumper;
         boolean hangPlane = gamepad2.y;
         boolean plane = gamepad2.dpad_right;
+//Read Controller
         controller1.readButtons();
         controller2.readButtons();
 //Drive
@@ -44,51 +43,55 @@ public class MainTeleOp extends OpMode {
                 || controller2.wasJustReleased(GamepadKeys.Button.DPAD_LEFT)) {
             this.robot.getSlides().slideStop();
         }
-        if (gamepad2.left_trigger > .5){
+        if (gamepad2.left_trigger > .1) {
-            Configurables.SLIDE_POWER_UP = .3;
+            Robot.Slides.SLIDE_POWER_UP = .3;
         } else {
-            Configurables.SLIDE_POWER_UP = .7;
+            Robot.Slides.SLIDE_POWER_UP = .7;
         }
 ////Macros
         this.robot.pickupMacro(controller2, getRuntime()); //DPADDOWN
+        this.robot.armMacro(controller2, getRuntime()); //X
 //
 //Arm and Wrist
-        if (controller2.wasJustPressed(GamepadKeys.Button.X)) {
+//        if (controller2.wasJustPressed(GamepadKeys.Button.DPAD_DOWN)) {
-            this.robot.getArm().armSecondaryScore();
+//            this.robot.getArm().pickup(true);
-            this.robot.getWrist().wristScore();
+//        } else
-        } else if (controller2.wasJustPressed(GamepadKeys.Button.A)) {
+//        if (controller2.wasJustReleased(GamepadKeys.Button.DPAD_DOWN)) {
+//            this.robot.getArm().armRest(true);
+//        } else
+        if (controller2.wasJustPressed(GamepadKeys.Button.A)) {
             this.robot.getArm().armRest();
             this.robot.getWrist().wristPickup();
         }
-////Claw
+//Claw
         if (controller2.wasJustPressed(GamepadKeys.Button.B)) {
             gamepad1.rumble(300);
-        } else if (controller2.isDown(GamepadKeys.Button.B)){
+        } else if (controller2.isDown(GamepadKeys.Button.B) || controller2.isDown(GamepadKeys.Button.DPAD_DOWN)) {
             this.robot.getClaw().open();
-        } else if (controller2.wasJustReleased(GamepadKeys.Button.B)){
+        } else if (controller2.wasJustReleased(GamepadKeys.Button.B)) {
             this.robot.getClaw().close();
         }
-////Hang
+//Hang
         if (hang) {
             this.robot.getHang().hang();
-        } else if (hangRelease){
+        } else if (hangRelease) {
             this.robot.getHang().hangRelease();
         } else if (hangPlane) {
             this.robot.getHang().hangPlane();
         }
-
-        int PositionLeft = this.robot.getSlides().slidesLeft.getCurrentPosition();
-        telemetry.addData("positionLeft",(PositionLeft));
-        int PositionRight = this.robot.getSlides().slidesRight.getCurrentPosition();
-        telemetry.addData("positionRight",(PositionRight));
-        telemetry.update();
-//
 //Plane
         if (plane) {
             this.robot.getPlane().planeLaunch();
-        }else {
+        } else {
             this.robot.getPlane().planeLock();
         }
-//
+//Telemetry
+        int PositionLeft = this.robot.getSlides().slidesLeft.getCurrentPosition();
+        telemetry.addData("positionLeft", (PositionLeft));
+        int PositionRight = this.robot.getSlides().slidesRight.getCurrentPosition();
+        telemetry.addData("positionRight", (PositionRight));
+        telemetry.update();
+//Update Robot
+        this.robot.update();
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Configurables.java

@@ -13,37 +13,12 @@ public class Configurables {
     public static Color FTC_WHITE_LOWER = new Color(0, 0, 40);
     public static Color FTC_WHITE_UPPER = new Color(180, 30, 255);
 
-    //Servo Positions
-    public static double ARMREST = 0.91;
-    public static double ARMSCORE = 0.4;
-    public static double ARMACCSCORE = .57;
-    public static double ARMPICKUPSTACK = 0.815;
-    public static double PICKUP = 0.94;
-    public static double WRISTPICKUP = 0.3;
-    public static double WRISTSCORE = .98;
-    public static double OPEN = 0.481;
-    public static double BIGOPEN = 0.65;
-    public static double CLOSE = .51;
-    public static double PLANELOCK = 0.1;
-    public static double PLANELAUNCH = 0.9;
-
     //Drive Speed
+    @Config
+    public static class driveSpeed {
         public static double SPEED = 1;
         public static double SLOWMODE_SPEED = 0.3;
         public static double TURN = .75;
         public static double SLOWMODE_TURN = 0.3;
-
+    }
-    //Motor Positions
-    public static double SLIDE_POWER_UP = .7;
-    public static double SLIDE_POWER_DOWN = .5;
-    public static int SLIDELAYERONE = 60;
-    public static int SLIDEAUTOSTACKS = 250;
-    public static int SLIDEUP = 1150;
-    public static int HANGRELEASE = 1550;
-    public static int HANG = 0;
-    public static int HANGPLANE = 1150;
-    public static int SLIDELAYERTWO = 350;
-    public static int SLIDESTACK = 80;
-    public static int SLIDEPICKUPSTACKSTWO = 30;
-
 }