Goog

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

@@ -31,7 +31,7 @@ import lombok.Getter;
 @Config
 public class Robot {
     public static boolean clawIsOpen;
-    public static double WRISTDELAY = .3;
+    public static double WRISTDELAY = .08;
     double delay;
     public pickupMacroStates pickupMacroState = pickupMacroStates.IDLE;
     public armMacroStates armMacroState = armMacroStates.IDLE;
@@ -148,10 +148,10 @@ public class Robot {
     public static class Slides {
         //Values
         public static double SLIDE_POWER_UP = .7;
-        public static double SLIDE_POWER_DOWN = .5;
+        public static double SLIDE_POWER_DOWN = .2;
         public static int SLIDELAYERONE = 60;
         public static int SLIDEAUTOSTACKS = 250;
-        public static int SLIDEUP = 1150;
+        public static int SLIDEUP = 630;
         public static int SLIDELAYERTWO = 350;
         public static int SLIDESTACK = 80;
         public static int SLIDEPICKUPSTACKSTWO = 30;
@@ -426,11 +426,11 @@ public class Robot {
     @Config
     public static class Claw {
         //Values
-        public static double OPEN = 0.45;
+        public static double OPEN = 0.65;
         public static double BIGOPEN = 0f;
-        public static double CLOSE = 0.85;
+        public static double CLOSE = 0.73;
-        public static double CLAW_MIN = 0.46;
+        public static double CLAW_MIN = 0;
-        public static double CLAW_MAX = 0.5;
+        public static double CLAW_MAX = 1;
         //Servo
         private Servo claw;
 

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

@@ -329,9 +329,9 @@ public class    MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDr
 
         this.setWeightedDrivePower(
                 new Pose2d(
-                        gamepad2.left_stick_y * -1 * speedScale,
+                        gamepad1.left_stick_y * -1 * speedScale,
-                        gamepad2.left_stick_x * -1 * speedScale,
+                        gamepad1.left_stick_x * -1 * speedScale,
-                        -gamepad2.right_stick_x * turnScale
+                        -gamepad1.right_stick_x * turnScale
                 ));
     }
 }

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

@@ -0,0 +1,44 @@
+package org.firstinspires.ftc.teamcode.opmodes;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.hardware.Robot;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
+
+@Autonomous(name = "Test Stuff")
+public class AutoTest extends LinearOpMode {
+    protected Pose2d initialPosition;
+    private Robot robot;
+
+    //Pose2ds
+    //Preloads
+    final static Pose2d FIRSTMOVE = new Pose2d(10, 10, Math.toRadians(0));
+
+    protected void sequenceOne() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        builder.splineToConstantHeading(new Vector2d(10, 10), Math.toRadians(0));
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+    }
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+
+        this.telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+        this.robot = new Robot().init(hardwareMap);
+        this.initialPosition = new Pose2d(0, 0, Math.toRadians(0));
+        this.robot.getDrive().setPoseEstimate(initialPosition);
+
+        // Do super fancy chinese shit
+        while (!this.isStarted()) {
+        }
+
+        sequenceOne();
+
+    }
+
+}

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

@@ -25,6 +25,8 @@ public class MainTeleOp extends OpMode {
 //GamePad Controls
         boolean slideUp = controller2.isDown(GamepadKeys.Button.DPAD_UP);
         boolean slideDown = controller2.isDown(GamepadKeys.Button.DPAD_LEFT);
+        boolean slideStop = controller2.wasJustReleased(GamepadKeys.Button.DPAD_UP) || controller2.wasJustReleased(GamepadKeys.Button.DPAD_LEFT);
+        boolean slideSlow = gamepad2.left_trigger > .1;
         boolean hang = gamepad2.left_bumper;
         boolean hangRelease = gamepad2.right_bumper;
         boolean hangPlane = gamepad2.y;
@@ -39,19 +41,21 @@ public class MainTeleOp extends OpMode {
             this.robot.getSlides().slideUp();
         } else if (slideDown) {
             this.robot.getSlides().slideDown();
-        } else if (controller2.wasJustReleased(GamepadKeys.Button.DPAD_UP)
+        } else if (slideStop) {
-                || controller2.wasJustReleased(GamepadKeys.Button.DPAD_LEFT)) {
             this.robot.getSlides().slideStop();
         }
-        if (gamepad2.left_trigger > .1) {
+        //Slide Power
-            Robot.Slides.SLIDE_POWER_UP = .3;
+        if (slideSlow) {
+            Robot.Slides.SLIDE_POWER_UP = .25;
+            Robot.Slides.SLIDE_POWER_DOWN = .2;
         } else {
             Robot.Slides.SLIDE_POWER_UP = .7;
+            Robot.Slides.SLIDE_POWER_DOWN = .5;
         }
-////Macros
+//Macros
         this.robot.pickupMacro(controller2, getRuntime()); //DPADDOWN
         this.robot.armMacro(controller2, getRuntime()); //X
-//
+
 //Arm and Wrist
 //        if (controller2.wasJustPressed(GamepadKeys.Button.DPAD_DOWN)) {
 //            this.robot.getArm().pickup(true);

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

@@ -1,6 +1,7 @@
 package org.firstinspires.ftc.teamcode.util;
 
 import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
 
 @Config
 public class Configurables {
@@ -21,4 +22,22 @@ public class Configurables {
         public static double TURN = .75;
         public static double SLOWMODE_TURN = 0.3;
     }
+
+    //Auto Temp
+    @Config
+    public static class AuToDeV {
+        //Things
+        public static double X1 = 0, Y1 = 0, R1 = 0;
+        public static double X2 = 0, Y2 = 0, R2 = 0;
+        public static double X3 = 0, Y3 = 0, R3 = 0;
+        public static double X4 = 0, Y4 = 0, R4 = 0;
+        public static double X5 = 0, Y5 = 0, R5 = 0;
+
+        //Pose2d
+        public static Pose2d TEMP1 = new Pose2d(X1, Y1, Math.toRadians(R1));
+        public static Pose2d TEMP2 = new Pose2d(X2, Y2, Math.toRadians(R2));
+        public static Pose2d TEMP3 = new Pose2d(X3, Y3, Math.toRadians(R3));
+        public static Pose2d TEMP4 = new Pose2d(X4, Y4, Math.toRadians(R4));
+        public static Pose2d TEMP5 = new Pose2d(X5, Y5, Math.toRadians(R5));
+    }
 }