Changes for sensor training

cc4h-Robotics-FTC · May 10, 2023 · 3d99e0f · 3d99e0f
1 parent 0638dad
commit 3d99e0f
Show file tree

Hide file tree

Showing 3 changed files with 57 additions and 17 deletions.
diff --git a/TeamCode/src/main/java/org/cc4hrobotics/ftc/fakes/FakeTelemetry.java b/TeamCode/src/main/java/org/cc4hrobotics/ftc/fakes/FakeTelemetry.java
@@ -8,6 +8,7 @@
 
 public class FakeTelemetry implements Telemetry {
     private boolean outputTelemetry = true;
+    private int loopCount = 0;
 
     public void setOutputTelemetry(boolean flag) {
         outputTelemetry = flag;
@@ -79,11 +80,15 @@ public void speak(String text, String languageCode, String countryCode) {
 
     }
 
+    public void setLoopCount(int lc) {
+        loopCount = lc;
+    }
+
     @Override
     public boolean update() {
         if (outputTelemetry && !list.isEmpty()) {
-            System.out.println("Telemetry Update");
-            System.out.println("----------------");
+            System.out.println("Telemetry Update (lc=" + loopCount + ")");
+            System.out.println("--------------------------");
             for (String s: list ) {
                 System.out.println(s);
             }

diff --git a/TeamCode/src/main/java/org/cc4hrobotics/ftc/fakes/sensors/FakeDigitalChannel.java b/TeamCode/src/main/java/org/cc4hrobotics/ftc/fakes/sensors/FakeDigitalChannel.java
@@ -5,15 +5,16 @@
 
 public class FakeDigitalChannel implements DigitalChannel {
     boolean state = true;
+    Mode mode;
 
     @Override
     public Mode getMode() {
-        return null;
+        return mode;
     }
 
     @Override
     public void setMode(Mode mode) {
-
+        this.mode = mode;
     }
 
     // IMPLEMENT ME

diff --git a/TeamCode/src/test/java/org/cc4hrobotics/ftc/LearnJavaTest.java b/TeamCode/src/test/java/org/cc4hrobotics/ftc/LearnJavaTest.java
@@ -4,9 +4,14 @@
 
 import android.content.Context;
 
+import com.qualcomm.robotcore.hardware.DigitalChannel;
+
 import org.cc4hrobotics.ftc.fakes.FakeHardwareMap;
+import org.cc4hrobotics.ftc.fakes.FakeTelemetry;
 import org.cc4hrobotics.ftc.fakes.drive.FakeExtendedDcMotor;
+import org.cc4hrobotics.ftc.fakes.sensors.FakeDigitalChannel;
 import org.cc4hrobotics.ftc.fakes.sensors.FakeDistanceSensor;
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
 import org.junit.Before;
 import org.junit.Test;
 
@@ -17,32 +22,53 @@ public class LearnJavaTest {
     private FakeExtendedDcMotor leftDriveMotor;
     private FakeExtendedDcMotor rightDriveMotor;
     private FakeDistanceSensor distanceSensor;
+    private FakeDistanceSensor distanceSensor2;
+    private FakeDigitalChannel digitalChannel;
+    private FakeTelemetry fakeTelemetry = new FakeTelemetry();
 
     @Test
     public void RunIt() {
         double distance = 10;
         LearnJavaOpMode ljt = new LearnJavaOpMode();
+
+        ljt.telemetry = fakeTelemetry;
         ljt.hardwareMap = hardwareMap;
         ljt.init();
 
+        digitalChannel.setState(true);
         sleep(5);
 
+        distanceSensor2.setDistance(100000);
         distanceSensor.setDistance(distance);
         for (int i = 0; i< 100; i++ ) {
-            ljt.loop();
 
-                //Make adjustments to the gamepad and watch robot adjust
-                ljt.gamepad1.left_stick_x -= 0.03;
-                ljt.gamepad1.left_stick_y += 0.02;
-                ljt.gamepad1.a = !ljt.gamepad1.a;
-
-                if (leftDriveMotor.getPower() > 0 && rightDriveMotor.getPower() > 0)  {
-                    distance = distance - 1;
-                } else if (leftDriveMotor.getPower() < 0 && rightDriveMotor.getPower() < 0)  {
-                    distance = distance + 1;
-                }
-                distanceSensor.setDistance(distance);
-                sleep(3);
+            fakeTelemetry.setLoopCount(i);
+            //Make adjustments to the gamepad and watch robot adjust
+            ljt.gamepad1.left_stick_x -= 0.03;
+            ljt.gamepad1.left_stick_y += 0.02;
+            ljt.gamepad1.a = !ljt.gamepad1.a;
+
+            if (leftDriveMotor.getPower() > 0 && rightDriveMotor.getPower() > 0)  {
+                distance = distance - 1;
+            } else if (leftDriveMotor.getPower() < 0 && rightDriveMotor.getPower() < 0)  {
+                distance = distance + 1;
+            }
+            distanceSensor.setDistance(Math.max(distance, 0.0));
+
+            distanceSensor2.setDistance(distanceSensor2.getDistance(DistanceUnit.CM)/2);
+
+            if (i == 25 && digitalChannel.getMode() == DigitalChannel.Mode.INPUT) {
+                digitalChannel.setState(false);
+                //override to zero for testing...
+                distanceSensor2.setDistance(3.14);
+            } else {
+                digitalChannel.setState(true);
+            }
+
+
+
+            ljt.loop();
+            sleep(3);
         }
     }
 
@@ -60,6 +86,14 @@ public void setup() {
 
         distanceSensor = new FakeDistanceSensor();
         hardwareMap.addDevice("distance_sensor", distanceSensor);
+
+        distanceSensor2 = new FakeDistanceSensor();
+        hardwareMap.addDevice("distance_sensor2", distanceSensor2);
+
+        digitalChannel = new FakeDigitalChannel();
+        hardwareMap.addDevice("sensor_digital", digitalChannel);
+
+
     }
 
     public void sleep(long seconds) {