tuned mecannum drive

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/MecanumDrive.java

@@ -31,8 +31,8 @@ public class MecanumDrive implements Mechanism {
     public static final double MM_PER_IN = 25.4;
 
     // depending on drive motor
-    public static final double TICKS_PER_REV = 537.7; // for gobilda 5203 312 rpm motors
-    public static final double MAX_RPM = 312;
+    public static final double TICKS_PER_REV = 384.5; // for gobilda 5203 312 rpm motors
+    public static final double MAX_RPM = 435;
     public static final double WHEEL_DIAM_IN = 96 / MM_PER_IN;
     public static double TRACK_WIDTH_IN = 16.65;  // from TrackWidthTuner
     public static final double MAX_MOTOR_VELOCITY = MAX_RPM * Math.PI * WHEEL_DIAM_IN / SECS_PER_MIN;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/MiddleSpikeToBackboard.java

@@ -0,0 +1,78 @@
+package org.firstinspires.ftc.teamcode.ftc16072.OpModes;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.acmerobotics.roadrunner.util.NanoClock;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+
+import org.firstinspires.ftc.teamcode.ftc16072.Robot;
+
+@Autonomous
+public class MiddleSpikeToBackboard extends OpMode {
+    Robot robot = new Robot();
+    Trajectory trajectory;
+    private enum State {BEGIN, AWAY, PAUSE, RETURN, DONE}
+    State state = State.BEGIN;
+    NanoClock clock;
+    double startPause;
+    @Override
+    public void init() {
+        robot.makeDriveOnly();
+        robot.init(hardwareMap);
+        clock = NanoClock.system();
+    }
+
+    @Override
+    public void loop() {
+        robot.nav.updatePoseEstimate();
+        Pose2d currentPose = robot.nav.getPoseEstimate();
+
+        //Trajectory to middle tape
+        Trajectory leftSpikeTrajectory = robot.nav.trajectoryBuilder(currentPose, false)
+                .back(25)
+                .strafeRight(25)
+                .build();
+
+        Trajectory trajectory2 = robot.nav.trajectoryBuilder(trajectory1.end(), false)
+                .strafeRight(25)
+                .build();
+
+        telemetry.addData("STATE", state);
+        telemetry.addData("POSE", "x = %.2f y = %.2f h = %.1f", currentPose.getX(), currentPose.getY(), Math.toDegrees(currentPose.getHeading()));
+        switch (state) {
+            case BEGIN:
+                state = State.AWAY;
+
+                robot.nav.setPoseEstimate(currentPose);
+                robot.nav.follower.followTrajectory(trajectory1);
+                break;
+            case AWAY:
+                if (robot.nav.isDoneFollowing(currentPose)) {
+                    state = State.PAUSE;
+                    startPause = clock.seconds();
+                }
+                break;
+            case PAUSE:
+                if ((clock.seconds() - startPause) > 2.0) {
+
+                    state = State.RETURN;
+
+                }
+                break;
+            case RETURN:
+                robot.nav.setPoseEstimate(trajectory1.end());
+                robot.nav.follower.followTrajectory(trajectory2);
+
+                break;
+            case DONE:
+                if (robot.nav.isDoneFollowing(currentPose)) {
+                    state = State.DONE;
+                }
+                break;
+        }
+
+
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/RRTests/RR_OurSpline.java

@@ -10,7 +10,7 @@
 
 import org.firstinspires.ftc.teamcode.ftc16072.Robot;
 
-@Disabled
+
 @Autonomous(group = "RR")
 public class RR_OurSpline extends OpMode {
 
@@ -39,6 +39,8 @@ public void loop() {
 
         telemetry.addData("STATE", state);
         telemetry.addData("POSE", "x = %.2f y = %.2f h = %.1f", currentPose.getX(), currentPose.getY(), Math.toDegrees(currentPose.getHeading()));
+        telemetry.addData("Seconds Elapsed", clock.seconds());
+        telemetry.addData("Pause", (clock.seconds() - startPause));
         switch (state) {
             case BEGIN:
                 state = State.AWAY;
@@ -61,12 +63,12 @@ public void loop() {
                 }
                 break;
             case PAUSE:
-                if ((clock.seconds() - startPause) > 2.0) {
+                if ((clock.seconds() - startPause) > 10.0) {
                     state = State.RETURN;
-                    trajectory = robot.nav.trajectoryBuilder(currentPose, true)
-                            .splineTo(new Vector2d(0, 0), Math.toRadians(180))
+                    Trajectory trajectory3 = robot.nav.trajectoryBuilder(currentPose, true)
+                            .back(24)
                             .build();
-                    robot.nav.follower.followTrajectory(trajectory);
+                    robot.nav.follower.followTrajectory(trajectory3);
                 }
                 break;
             case RETURN:

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/RRTests/RR_StrafeTest.java

@@ -11,7 +11,6 @@
 
 import org.firstinspires.ftc.teamcode.ftc16072.Robot;
 
-@Disabled
 @Autonomous(group = "RR")
 public class RR_StrafeTest extends OpMode {
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/RRTests/RR_StraightTest.java

@@ -11,7 +11,6 @@
 
 import org.firstinspires.ftc.teamcode.ftc16072.Robot;
 
-@Disabled
 @Autonomous(group = "RR")
 public class RR_StraightTest extends OpMode {
     Robot robot = new Robot();

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/RRTests/RR_TrackWidthTuner.java

@@ -13,7 +13,6 @@
 import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.MecanumDrive;
 import org.firstinspires.ftc.teamcode.ftc16072.Robot;
 
-@Disabled
 @Autonomous(group = "RR")
 public class RR_TrackWidthTuner extends OpMode {
     public static double TURN_ANGLE_DEGREES = 180;