Merge pull request #179 from XaverianTeamRobotics/testing-Michael-Lachut

v1.1.0

.gitattributes

@@ -0,0 +1,4 @@
+*.psd filter=lfs diff=lfs merge=lfs -text
+*.hproof filter=lfs diff=lfs merge=lfs -text
+java_pid31808.hprof filter=lfs diff=lfs merge=lfs -text
+*.hprof filter=lfs diff=lfs merge=lfs -text

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/main/opmodes/production/FullTeleOp.java

@@ -7,7 +7,7 @@
 import org.firstinspires.ftc.teamcode.main.utils.scripting.Script;
 import org.firstinspires.ftc.teamcode.main.utils.scripting.ScriptRunner;
 
-@TeleOp(name="FullTeleOp", group="production")
+@TeleOp(name="FullTeleOp", group="aproduction")
 public class FullTeleOp extends LinearOpMode {
 
     @Override

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/main/opmodes/production/autonomous/SimpleAuto.java

@@ -0,0 +1,16 @@
+package org.firstinspires.ftc.teamcode.main.opmodes.production.autonomous;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.main.utils.autonomous.starting.StartingPositionManager;
+
+@Autonomous(name = "StartingPositionBothMiddle", preselectTeleOp = "FullTeleOp")
+public class SimpleAuto extends LinearOpMode {
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        new StartingPositionManager(this, true, false, 3, true);
+    }
+
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/main/opmodes/production/autonomous/StartingPositionBlueLeft.java

@@ -5,11 +5,11 @@
 
 import org.firstinspires.ftc.teamcode.main.utils.autonomous.starting.StartingPositionManager;
 
-@Autonomous(name = "Starting Position Blue Left")
+@Autonomous(name = "Starting Position Blue Left", preselectTeleOp = "FullTeleOp")
 public class StartingPositionBlueLeft extends LinearOpMode {
     @Override
     public void runOpMode() throws InterruptedException {
-        new StartingPositionManager(this, true, false, 3);
+        new StartingPositionManager(this, true, false, 3, false);
     }
 
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/main/opmodes/production/autonomous/StartingPositionBlueRight.java

@@ -5,11 +5,11 @@
 
 import org.firstinspires.ftc.teamcode.main.utils.autonomous.starting.StartingPositionManager;
 
-@Autonomous(name = "Starting Position Blue Right")
+@Autonomous(name = "Starting Position Blue Right", preselectTeleOp = "FullTeleOp")
 public class StartingPositionBlueRight extends LinearOpMode {
     @Override
     public void runOpMode() throws InterruptedException {
-        new StartingPositionManager(this, true, true, 3);
+        new StartingPositionManager(this, true, true, 3, false);
     }
 
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/main/opmodes/production/autonomous/StartingPositionRedLeft.java

@@ -5,11 +5,11 @@
 
 import org.firstinspires.ftc.teamcode.main.utils.autonomous.starting.StartingPositionManager;
 
-@Autonomous(name = "Starting Position Red Left")
+@Autonomous(name = "Starting Position Red Left", preselectTeleOp = "FullTeleOp")
 public class StartingPositionRedLeft extends LinearOpMode {
     @Override
     public void runOpMode() throws InterruptedException {
-        new StartingPositionManager(this, false, true, 3);
+        new StartingPositionManager(this, false, true, 3, false);
     }
 
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/main/opmodes/production/autonomous/StartingPositionRedRight.java

@@ -5,11 +5,11 @@
 
 import org.firstinspires.ftc.teamcode.main.utils.autonomous.starting.StartingPositionManager;
 
-@Autonomous(name = "Starting Position Red Right")
+@Autonomous(name = "Starting Position Red Right", preselectTeleOp = "FullTeleOp")
 public class StartingPositionRedRight extends LinearOpMode {
     @Override
     public void runOpMode() throws InterruptedException {
-        new StartingPositionManager(this, false, false, 3);
+        new StartingPositionManager(this, false, false, 3, false);
     }
 
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/main/opmodes/tests/FloorIt.java

@@ -0,0 +1,49 @@
+package org.firstinspires.ftc.teamcode.main.opmodes.tests;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.teamcode.main.utils.autonomous.EncoderTimeoutManager;
+import org.firstinspires.ftc.teamcode.main.utils.autonomous.location.pipeline.PositionSystem;
+import org.firstinspires.ftc.teamcode.main.utils.helpers.elevator.ElevatorDriver;
+import org.firstinspires.ftc.teamcode.main.utils.interactions.groups.StandardTankVehicleDrivetrain;
+import org.firstinspires.ftc.teamcode.main.utils.io.InputSpace;
+import org.firstinspires.ftc.teamcode.main.utils.io.OutputSpace;
+import org.firstinspires.ftc.teamcode.main.utils.resources.Resources;
+import org.firstinspires.ftc.teamcode.main.utils.scripting.Script;
+import org.firstinspires.ftc.teamcode.main.utils.scripting.ScriptRunner;
+import org.firstinspires.ftc.teamcode.main.utils.scripting.TeleOpStubScript;
+
+@Autonomous(name="Go Go Power Rangers") // replace the name and group with your OpMode's name and group
+public class FloorIt extends LinearOpMode {
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        PositionSystem positionSystem = Resources.Navigation.Sensors.getPositionSystem(hardwareMap);
+
+        InputSpace input = new InputSpace(hardwareMap);
+        OutputSpace output = new OutputSpace(hardwareMap);
+        StandardTankVehicleDrivetrain tank = (StandardTankVehicleDrivetrain) input.getTank().getInternalInteractionSurface();
+        ElevatorDriver elevatorDriver = new ElevatorDriver(input, output, this);
+        positionSystem.setDrivetrain(tank);
+
+        waitForStart();
+
+        positionSystem.encoderDrive(-35, 35, 75);
+
+        EncoderTimeoutManager encoderTimeout = new EncoderTimeoutManager(5000);
+
+        encoderTimeout.restart();
+
+        while (positionSystem.areMotorsBusy() && !encoderTimeout.hasTimedOut() && opModeIsActive()) {
+            telemetry.addData("Motors busy for", encoderTimeout.getOperationTime());
+            telemetry.update();
+        }
+
+        positionSystem.getDrivetrain().brake();
+        encoderTimeout.restart();
+    }
+
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/main/opmodes/tests/InterruptTest.java

@@ -0,0 +1,30 @@
+package org.firstinspires.ftc.teamcode.main.opmodes.tests;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.main.utils.scripting.AutonomousStubScript;
+import org.firstinspires.ftc.teamcode.main.utils.scripting.Script;
+import org.firstinspires.ftc.teamcode.main.utils.scripting.ScriptRunner;
+
+@Autonomous(name="Interrupt Test", group="tests") // replace the name and group with your OpMode's name and group
+public class InterruptTest extends LinearOpMode {
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        waitForStart();
+
+        testInterrupts();
+    }
+
+    private void testInterrupts() throws InterruptedException {
+        do {
+            telemetry.addData("Time", time);
+            telemetry.update();
+            if (isStopRequested()) {
+                throw new InterruptedException();
+            }
+        } while (true);
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/main/opmodes/tests/VelocityTest.java

@@ -1,42 +1,50 @@
 package org.firstinspires.ftc.teamcode.main.opmodes.tests;
 
+import com.qualcomm.hardware.bosch.NaiveAccelerationIntegrator;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.util.ElapsedTime;
 
 import org.firstinspires.ftc.teamcode.main.utils.autonomous.location.pipeline.PositionSystem;
-import org.firstinspires.ftc.teamcode.main.utils.autonomous.location.pipeline.VelocityTracker;
 import org.firstinspires.ftc.teamcode.main.utils.interactions.items.StandardIMU;
 import org.firstinspires.ftc.teamcode.main.utils.resources.Resources;
 
-import java.util.concurrent.TimeUnit;
-
 @Autonomous(name="Velocity Test", group="tests")
 public class VelocityTest extends LinearOpMode {
 
+    PositionSystem positionSystem;
+    StandardIMU imu;
+
     @Override
     public void runOpMode() throws InterruptedException {
-        PositionSystem positionSystem = Resources.Navigation.Sensors.getPositionSystem(hardwareMap);
-        StandardIMU imu = positionSystem.imu;
-        positionSystem.setVelocityTracker(new VelocityTracker(imu));
+        waitForStart();
+
+        NaiveAccelerationIntegrator accelerationIntegrator;
+
+        positionSystem = Resources.Navigation.Sensors.getPositionSystem(hardwareMap);
+        imu = positionSystem.imu;
+        // telemetry.addAction(this::doTelemetry);
         while (!isStopRequested()) {
-            telemetry.addData("VELOCITY (CM/S)", positionSystem.getVelocity());
-            telemetry.addData("DISPLACEMENT (CM)", positionSystem.getDisplacement());
+            doTelemetry();
+            telemetry.update();
+        }
+    }
 
-            StandardIMU.VelocityReturnData acc = imu.getAcceleration();
-            telemetry.addData("ACCELERATION (CM/S^2)", "");
-            telemetry.addData("  X", acc.getX());
-            telemetry.addData("  Y", acc.getY());
-            telemetry.addData("  Z", acc.getZ());
+    public void doTelemetry() {
+        telemetry.addData("VELOCITY", imu.getVelocity().getY());
+        telemetry.addData("DISPLACEMENT", positionSystem.getDisplacement());
 
+        StandardIMU.VelocityReturnData acc = imu.getAcceleration();
+        telemetry.addData("ACCELERATION", "");
+        telemetry.addData("  X", acc.getX());
+        telemetry.addData("  Y", acc.getY());
+        telemetry.addData("  Z", acc.getZ());
 
-            StandardIMU.VelocityReturnData ang = imu.getAngularVelocity();
-            telemetry.addData("ANGULAR VELOCITY (DEG/S)", "");
-            telemetry.addData("  X", ang.getX());
-            telemetry.addData("  Y", ang.getY());
-            telemetry.addData("  Z", ang.getZ());
+        StandardIMU.VelocityReturnData ang = imu.getAngularVelocity();
+        telemetry.addData("ANGULAR VELOCITY (DEG/S)", "");
+        telemetry.addData("  X", ang.getX());
+        telemetry.addData("  Y", ang.getY());
+        telemetry.addData("  Z", ang.getZ());
 
-            telemetry.update();
-        }
+        telemetry.update();
     }
 }