From d283ca2a8e393a15f50cf78a89ccf0304b5a384a Mon Sep 17 00:00:00 2001
From: Luke West <l.s.west88@gmail.com>
Date: Thu, 7 Jan 2016 18:38:16 -0500
Subject: [PATCH 1/2] Made Robot static, added comments to Robot, cleaned up
 some telemetry, added precautions to SensorState so public functions can't be
 called with invalid names.

---
 FtcRobotController/FtcRobotController.iml     |  30 ++--
 .../ftcrobotcontroller/opmodes/DropPath.java  |   6 +-
 .../opmodes/MeetTwoPath.java                  |   4 +-
 .../opmodes/NewDrivetrainTest.java            |   4 +-
 .../opmodes/SensorStateOpMode.java            |   2 +-
 .../src/main/java/lib/DriveTrain.java         |   3 -
 .../src/main/java/lib/Robot.java              | 136 +++++++++---------
 .../src/main/java/lib/SensorState.java        |  39 +++--
 8 files changed, 122 insertions(+), 102 deletions(-)

diff --git a/FtcRobotController/FtcRobotController.iml b/FtcRobotController/FtcRobotController.iml
index de9e7076842..378e4b32a81 100644
--- a/FtcRobotController/FtcRobotController.iml
+++ b/FtcRobotController/FtcRobotController.iml
@@ -12,10 +12,12 @@
         <option name="SELECTED_TEST_ARTIFACT" value="_android_test_" />
         <option name="ASSEMBLE_TASK_NAME" value="assembleDebug" />
         <option name="COMPILE_JAVA_TASK_NAME" value="compileDebugSources" />
-        <option name="SOURCE_GEN_TASK_NAME" value="generateDebugSources" />
         <option name="ASSEMBLE_TEST_TASK_NAME" value="assembleDebugAndroidTest" />
         <option name="COMPILE_JAVA_TEST_TASK_NAME" value="compileDebugAndroidTestSources" />
-        <option name="TEST_SOURCE_GEN_TASK_NAME" value="generateDebugAndroidTestSources" />
+        <afterSyncTasks>
+          <task>generateDebugAndroidTestSources</task>
+          <task>generateDebugSources</task>
+        </afterSyncTasks>
         <option name="ALLOW_USER_CONFIGURATION" value="false" />
         <option name="MANIFEST_FILE_RELATIVE_PATH" value="/src/main/AndroidManifest.xml" />
         <option name="RES_FOLDER_RELATIVE_PATH" value="/src/main/res" />
@@ -63,12 +65,9 @@
       <sourceFolder url="file://$MODULE_DIR$/src/androidTest/jni" isTestSource="true" />
       <sourceFolder url="file://$MODULE_DIR$/src/androidTest/rs" isTestSource="true" />
       <excludeFolder url="file://$MODULE_DIR$/build/intermediates/assets" />
-      <excludeFolder url="file://$MODULE_DIR$/build/intermediates/bundles" />
       <excludeFolder url="file://$MODULE_DIR$/build/intermediates/classes" />
-      <excludeFolder url="file://$MODULE_DIR$/build/intermediates/coverage-instrumented-classes" />
       <excludeFolder url="file://$MODULE_DIR$/build/intermediates/dependency-cache" />
       <excludeFolder url="file://$MODULE_DIR$/build/intermediates/dex" />
-      <excludeFolder url="file://$MODULE_DIR$/build/intermediates/dex-cache" />
       <excludeFolder url="file://$MODULE_DIR$/build/intermediates/exploded-aar/Analytics-release/jars" />
       <excludeFolder url="file://$MODULE_DIR$/build/intermediates/exploded-aar/FtcCommon-release/jars" />
       <excludeFolder url="file://$MODULE_DIR$/build/intermediates/exploded-aar/Hardware-release/jars" />
@@ -76,28 +75,27 @@
       <excludeFolder url="file://$MODULE_DIR$/build/intermediates/exploded-aar/RobotCore-release/jars" />
       <excludeFolder url="file://$MODULE_DIR$/build/intermediates/exploded-aar/WirelessP2p-release/jars" />
       <excludeFolder url="file://$MODULE_DIR$/build/intermediates/incremental" />
-      <excludeFolder url="file://$MODULE_DIR$/build/intermediates/jacoco" />
-      <excludeFolder url="file://$MODULE_DIR$/build/intermediates/javaResources" />
-      <excludeFolder url="file://$MODULE_DIR$/build/intermediates/libs" />
-      <excludeFolder url="file://$MODULE_DIR$/build/intermediates/lint" />
       <excludeFolder url="file://$MODULE_DIR$/build/intermediates/manifests" />
-      <excludeFolder url="file://$MODULE_DIR$/build/intermediates/ndk" />
       <excludeFolder url="file://$MODULE_DIR$/build/intermediates/pre-dexed" />
-      <excludeFolder url="file://$MODULE_DIR$/build/intermediates/proguard" />
       <excludeFolder url="file://$MODULE_DIR$/build/intermediates/res" />
+      <excludeFolder url="file://$MODULE_DIR$/build/intermediates/resources" />
       <excludeFolder url="file://$MODULE_DIR$/build/intermediates/rs" />
       <excludeFolder url="file://$MODULE_DIR$/build/intermediates/symbols" />
+      <excludeFolder url="file://$MODULE_DIR$/build/intermediates/tmp" />
       <excludeFolder url="file://$MODULE_DIR$/build/outputs" />
       <excludeFolder url="file://$MODULE_DIR$/build/tmp" />
     </content>
-    <orderEntry type="jdk" jdkName="Android 4.4.2 Google APIs" jdkType="Android SDK" />
+    <orderEntry type="jdk" jdkName="Android API 19 Platform" jdkType="Android SDK" />
     <orderEntry type="sourceFolder" forTests="false" />
-    <orderEntry type="library" exported="" name="android-support-v4" level="project" />
+    <orderEntry type="library" exported="" name="RobotCore-release-" level="project" />
     <orderEntry type="library" exported="" name="ModernRobotics-release-" level="project" />
-    <orderEntry type="library" exported="" name="Analytics-release-" level="project" />
-    <orderEntry type="library" exported="" name="FtcCommon-release-" level="project" />
     <orderEntry type="library" exported="" name="WirelessP2p-release-" level="project" />
+    <orderEntry type="library" exported="" name="Analytics-release-" level="project" />
+    <orderEntry type="library" exported="" name="android-support-v4" level="project" />
     <orderEntry type="library" exported="" name="Hardware-release-" level="project" />
-    <orderEntry type="library" exported="" name="RobotCore-release-" level="project" />
+    <orderEntry type="library" exported="" name="FtcCommon-release-" level="project" />
+    <orderEntry type="library" exported="" name="usb-android-19" level="project" />
+    <orderEntry type="library" exported="" name="effects-android-19" level="project" />
+    <orderEntry type="library" exported="" name="maps-android-19" level="project" />
   </component>
 </module>
\ No newline at end of file
diff --git a/FtcRobotController/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/DropPath.java b/FtcRobotController/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/DropPath.java
index a9c66b8f016..74f416fe250 100644
--- a/FtcRobotController/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/DropPath.java
+++ b/FtcRobotController/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/DropPath.java
@@ -17,12 +17,12 @@
 public class DropPath extends LinearOpMode {
     @Override
     public void runOpMode() throws InterruptedException {
-        Robot dave = new Robot(hardwareMap, telemetry, this); // makes Robot "dave"
+        Robot.init(hardwareMap, telemetry, this); // makes Robot "dave"
 
         TwoWheelDrive dave_train = new TwoWheelDrive(hardwareMap.dcMotor.get("leftdrive"), true,
                 hardwareMap.dcMotor.get("rightdrive"), false, 10);
 
-        dave.registerDriveTrain(dave_train);
+        Robot.registerDrivetrain(dave_train);
 
         Robot.state = new SensorState(hardwareMap, 1, 0);
         Robot.state.registerSensor("mr", SensorState.SensorType.COLOR, false, 12);
@@ -41,7 +41,7 @@ public void runOpMode() throws InterruptedException {
 
 //        dave_train.move(0.2, "hero", this);
 
-        dave.colorSweep(SensorState.ColorType.BLUE, 4, 10, "mrs", "mr", -0.3);
+        Robot.colorSweep(SensorState.ColorType.BLUE, 4, 10, -0.3, "mrs", "mr", "hero");
 
         while (opModeIsActive()){
 //            telemetry.addData("running", Robot.state.getSensorReading("hero"));
diff --git a/FtcRobotController/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/MeetTwoPath.java b/FtcRobotController/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/MeetTwoPath.java
index 7ea896d59f1..c6654739ff5 100644
--- a/FtcRobotController/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/MeetTwoPath.java
+++ b/FtcRobotController/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/MeetTwoPath.java
@@ -17,7 +17,7 @@
 public class MeetTwoPath extends LinearOpMode {
     @Override
     public void runOpMode() throws InterruptedException {
-        Robot dave = new Robot(hardwareMap, telemetry, this); // makes Robot "dave"
+        Robot.init(hardwareMap, telemetry, this); // makes Robot "dave"
 
         Robot.state = new SensorState(hardwareMap, 1, 0);
         Robot.state.registerSensor("hero", SensorState.SensorType.GYRO, true, 12);
@@ -33,7 +33,7 @@ public void runOpMode() throws InterruptedException {
                 hardwareMap.dcMotor.get("rightRear"), false,
                 4);
 
-        dave.registerDriveTrain(dave_train);
+        Robot.registerDrivetrain(dave_train);
 
         waitForStart();
 
diff --git a/FtcRobotController/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/NewDrivetrainTest.java b/FtcRobotController/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/NewDrivetrainTest.java
index ce1cab6b15b..a0812cca87a 100644
--- a/FtcRobotController/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/NewDrivetrainTest.java
+++ b/FtcRobotController/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/NewDrivetrainTest.java
@@ -13,13 +13,13 @@
 public class NewDrivetrainTest extends LinearOpMode {
     @Override
     public void runOpMode() throws InterruptedException{
-        Robot dave = new Robot(hardwareMap, telemetry, this); // makes Robot "dave"
+        Robot.init(hardwareMap, telemetry, this); // makes Robot "dave"
 
         DriveTrain dave_train = new TwoWheelDrive(
                 hardwareMap.dcMotor.get("leftdrive"), true,
                 hardwareMap.dcMotor.get("rightdrive"), false, 10);
 
-        dave.registerDriveTrain(dave_train);
+        Robot.registerDrivetrain(dave_train);
 
         Robot.state = new SensorState(hardwareMap, 1, 0);
         Robot.state.registerSensor("mr", SensorState.SensorType.COLOR, false, 12);
diff --git a/FtcRobotController/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/SensorStateOpMode.java b/FtcRobotController/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/SensorStateOpMode.java
index 96e38f00d69..ff619656588 100644
--- a/FtcRobotController/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/SensorStateOpMode.java
+++ b/FtcRobotController/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/SensorStateOpMode.java
@@ -14,7 +14,7 @@ public class SensorStateOpMode extends LinearOpMode {
     private Thread state_thread;
 
     public void runOpMode() throws InterruptedException{
-        Robot dave = new Robot(hardwareMap, telemetry, this);
+        Robot.init(hardwareMap, telemetry, this);
 
         Robot.state = new SensorState(hardwareMap, 1, 0);
         Robot.state.registerSensor("mr", SensorState.SensorType.COLOR, true, 60);
diff --git a/FtcRobotController/src/main/java/lib/DriveTrain.java b/FtcRobotController/src/main/java/lib/DriveTrain.java
index b599bef5a6d..d1f4cad5d9b 100644
--- a/FtcRobotController/src/main/java/lib/DriveTrain.java
+++ b/FtcRobotController/src/main/java/lib/DriveTrain.java
@@ -1,9 +1,6 @@
 package lib;
 
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.util.ElapsedTime;
-import com.qualcomm.robotcore.util.Range;
 
 /**
  * Created by luke on 10/7/15.
diff --git a/FtcRobotController/src/main/java/lib/Robot.java b/FtcRobotController/src/main/java/lib/Robot.java
index fc607280466..442be7bc4dc 100644
--- a/FtcRobotController/src/main/java/lib/Robot.java
+++ b/FtcRobotController/src/main/java/lib/Robot.java
@@ -11,171 +11,173 @@
  * Created by luke on 10/7/15.
  */
 
-
 public class Robot {
     public static SensorState state;
 
     // Hardware map pulls device Objects from the robot.
     // Drivetrain handles functions specific to our drive type (four-wheeld, two-wheel, treads, etc).
-    private HardwareMap hmap;
-    public DriveTrain drivetrain;
+    private static HardwareMap hmap;
+    public static DriveTrain drivetrain;
+
+    // Telemetry lets any class anywhere print messages through Robot
     public static Telemetry tel;
+
     // Store the objects corresponding to the devices of the robot (motors, sensors, servos) in hashmaps.
-    private HashMap<String, Object> motors;
-    private HashMap<String, Servo> servos;
-    public UltraServoHelper ultraservohelper;
+    private static HashMap<String, Object> motors;
+    private static HashMap<String, Servo> servos;
+    public static UltraServoHelper ultraservohelper;
     public static LinearOpMode waiter;
 
-    public Robot (HardwareMap hmap, Telemetry tel, LinearOpMode opm) {
-        this.hmap = hmap;
-        this.tel = tel;
-        this.servos = new HashMap<String, Servo>();
-        this.ultraservohelper = new UltraServoHelper();
-        waiter = opm;
-
-        // 100 millisecond delay between updates.
-//        state = new SensorState(hmap, 100);
-//        Thread state_thread = new Thread(state);
-//        state_thread.start();
-    }
+    // Prevents instantiation
+    private Robot(){}
 
-    // If someone tries to get a device not registered in a hashmap.
-    public class HardwareNotRegisteredException extends Exception {
-        public HardwareNotRegisteredException() { super(); }
-        public HardwareNotRegisteredException(String cause) { super(cause); }
-    }
-    public void registerServo(String servoName) {
-        servos.put(servoName, hmap.servo.get(servoName));
+    public static void init (HardwareMap hmap, Telemetry tel, LinearOpMode opm) {
+        Robot.hmap = hmap;
+        Robot.tel = tel;
+        Robot.servos = new HashMap<String, Servo>();
+        Robot.ultraservohelper = new UltraServoHelper();
+        Robot.waiter = opm;
     }
 
-    public void setPosition(String name, int position) {
-        servos.get(name).setPosition(position/180);
+    // REGISTRATION FUNCTIONS
+    public static void registerDrivetrain(DriveTrain d){
+        Robot.drivetrain = d;
     }
 
-    // REGISTRATION FUNCTIONS
-    // It makes more sense to have the opmode construct a drivetrain and pass it to Robot than to repeat
-    // constructors in Robot and the drivetrain classes.
-    public void registerDriveTrain(DriveTrain d){
-        this.drivetrain = d;
+    public static void registerServo(String servoName) {
+        servos.put(servoName, hmap.servo.get(servoName));
     }
 
-    public void registerUltrasonicServo(String sensorName, String servoName) {
+    public static void registerUltrasonicServo(String sensorName, String servoName) {
         if(servos.containsKey(servoName)) {
             ultraservohelper.registerServo(sensorName, servos.get(servoName));
         }
         else{
-            this.registerServo(servoName);
+            registerServo(servoName);
             ultraservohelper.registerServo(sensorName, servos.get(servoName));
         }
     }
 
-    public void tillSense(String sensorName, int servoPosition, double power, int distance, int filterlength) {
+    // MOVEMENT FUNCTIONS
+    public static void setPosition(String servoName, int position) {
+        servos.get(servoName).setPosition(position / 180);
+    }
+
+    public void tillSense(String sensorName, int servoPosition, double power, int distance, int filterlength, String gyro_name) {
         PID ultraPID = new PID(0.05, 0.005, 0, false,0.1);
+
         ultraPID.setTarget(distance);
         ultraPID.setMinOutput(-1);
         ultraPID.setMaxOutput(1);
+
         ultraservohelper.setPosition(sensorName, servoPosition);
+
         try {
             Thread.sleep(400);
         } catch (InterruptedException e) {
             e.printStackTrace();
         }
-        drivetrain.move(power,"hero",waiter);
+
+        drivetrain.move(power, gyro_name, waiter);
+
         while(!ultraPID.isAtTarget() && waiter.opModeIsActive()){
             power = ultraPID.update(state.getAvgSensorData(sensorName));
             drivetrain.move(power);
 
             Log.i("AvgUSDistance", "" + state.getAvgSensorData(sensorName));
+
             try{
                 Thread.sleep(10);
             } catch (InterruptedException e) {
                 e.printStackTrace();
             }
         }
+
         drivetrain.move(0);
     }
 
     public void parallel(String sensorNameA, String sensorNameB, double power, double thresh, int filterlength) {
         double diff;
-        int count =0;
-        do{
-            tel.addData("frontAvg", Robot.state.getAvgSensorData("frontUltra"));
-            tel.addData("rearAvg", Robot.state.getAvgSensorData("rearUltra"));
-            tel.addData("frontReading", Robot.state.getSensorReading("frontUltra"));
-            tel.addData("rearReading", Robot.state.getSensorReading("rearUltra"));
+        int count = 0;
 
+        do {
             diff = (state.getAvgSensorData(sensorNameA) - state.getAvgSensorData(sensorNameB));
-            if(Math.signum(diff) == 1 && count==0){
+
+            if(Math.signum(diff) == 1 && count==0) {
                 drivetrain.setLeftMotors(-power);
                 drivetrain.setRightMotors(power);
-            }
-            else if(Math.signum(diff) == -1 && count==0) {
+            } else if(Math.signum(diff) == -1 && count==0) {
                 drivetrain.setLeftMotors(power);
                 drivetrain.setRightMotors(-power);
             }
+
+            // If count isn't 0, then we're waiting to see if the robot slips at all, which means
+            // We need to keep the motors at 0.
             else{
                 drivetrain.setLeftMotors(0);
                 drivetrain.setRightMotors(0);
             }
 
+            // Wait to see if the robot slips
             if(Math.abs(diff) < thresh){
                 count ++;
-            }
-            else{
+            } else{
                 count = 0;
             }
+
             try {
                 Thread.sleep(5);
             } catch (InterruptedException e) {
                 e.printStackTrace();
             }
-        }while (count<15 && waiter.opModeIsActive());
+        } while (count<15 && waiter.opModeIsActive());
 
         drivetrain.move(0);
 
     }
 
     // tillSense for colors. If the first color we detect is the color argument (our teams color)
-    // Then we will hit that button.
-    // Otherwise, we go to the next light.
-    public void colorSweep(SensorState.ColorType color, double low_threshold, double high_threshold, String lightname, String colorname, double power) {
+    // Then we will hit that button. Otherwise, we go to the next light.
+    public static void colorSweep( SensorState.ColorType color,
+                            double low_threshold,
+                            double high_threshold,
+                            double power,
+                            String light_name,
+                            String color_name,
+                            String gyro_name) {
 
-        SensorState.ColorType stored_color = SensorState.ColorType.NONE;               // First detected color
-        SensorState.ColorType dominant = state.getColorData(colorname);   // Current dominant color detected
-        double average = 0.0;                     // Average of light values
-        double reading = 0.0;
+        SensorState.ColorType dominant = state.getColorData(color_name);   // Current dominant color detected
+        double average;   // Average of light values
+        double reading;   // reading of light values
 
-//        drivetrain.move(power,"hero", waiter);
+        drivetrain.move(power,gyro_name, waiter);
 
-        average = state.getAvgSensorData(lightname);
+        average = state.getAvgSensorData(light_name);
 
         while (waiter.opModeIsActive()) {
-            reading = state.getAvgSensorData(lightname);
-            tel.addData("Reading", reading);
-            tel.addData("Average", average);
+            reading = state.getAvgSensorData(light_name);
 
             if (average + low_threshold <= reading && reading <= average + high_threshold){
-//                break;
+                break;
             }
+
             try{
                 Thread.sleep(1);
-            } catch (InterruptedException ex){}
+            } catch (InterruptedException ex){
+                ex.printStackTrace();
+            }
         }
 
         drivetrain.stopMove();
 
-
+        // Add code here to drop the climbers in the right bin
         if (dominant == color){
             tel.addData("Color", "CORRECT");
-//            drivetrain.move(0.0);
-//            ((TwoWheelDrive)drivetrain).moveDistance(0.18, 8);
         }
 
         // First color detected is wrong color, so hit other button, which must be the right button.
         else {
             tel.addData("Color", "WRONG");
-//            drivetrain.move(0.0);
-//            ((TwoWheelDrive)drivetrain).moveDistance(-0.18, 8);
         }
     }
 }
diff --git a/FtcRobotController/src/main/java/lib/SensorState.java b/FtcRobotController/src/main/java/lib/SensorState.java
index d0085fcbaa9..97d414df7ca 100644
--- a/FtcRobotController/src/main/java/lib/SensorState.java
+++ b/FtcRobotController/src/main/java/lib/SensorState.java
@@ -113,9 +113,14 @@ public static ColorType[] toColor(int[] i){
         }
     }
 
-    private HashMap<SensorType, HardwareMap.DeviceMapping> maps;    // HashMap of DeviceMappings from HardwareMap to grab sensor objects in registration.
-    private HashMap<String, SensorContainer> sensorContainers;                        // Stores SensorContainer objects (definition at bottom)
-    private HashMap<SensorType, SensorContainer[]> types_inv;                // Allows recovery of all sensors of a certain type.
+    // HashMap of DeviceMappings from HardwareMap to grab sensor objects in registration.
+    private HashMap<SensorType, HardwareMap.DeviceMapping> maps;
+
+    // Stores SensorContainer objects (definition at bottom)
+    private HashMap<String, SensorContainer> sensorContainers;
+
+    // Allows recovery of all sensors of a certain type.
+    private HashMap<SensorType, SensorContainer[]> types_inv;
     private HardwareMap hmap;
 
     private DigitalChannel usPin;
@@ -177,8 +182,11 @@ public SensorState(HardwareMap hmap, int milli_interval, int nano_interval) {
      * @param data_length   The number of sensor readings to store for the sensor
      */
     public synchronized void registerSensor(String name, SensorType type, boolean update, int data_length){
-        Object sensor_obj = maps.get(type).get(name);                                   // Get underlying sensor object for the sensor
-        SensorContainer sen = new SensorContainer(sensor_obj, type, name, update, data_length);      // Make a SensorContainer to wrap around the object
+        // Get underlying sensor object for the sensor
+        Object sensor_obj = maps.get(type).get(name);
+
+        // Make a SensorContainer to wrap around the object
+        SensorContainer sen = new SensorContainer(sensor_obj, type, name, update, data_length);
 
         if (type == SensorType.GYRO)
             ((GyroSensor) sensor_obj).calibrate();
@@ -188,7 +196,7 @@ public synchronized void registerSensor(String name, SensorType type, boolean up
         }
 
         sensorContainers.put(name, sen);
-        updateTypes_Inv(sen);
+        updateTypesInv(sen);
     }
 
     /**
@@ -200,6 +208,7 @@ public synchronized void registerSensor(String name, SensorType type, boolean up
     public void setUltrasonicPin(String pin_name){
         if (!this.usPinWasSet) {
             this.usPin = hmap.digitalChannel.get(pin_name);
+            assert(this.usPin != null);
             this.usPinWasSet = true;
         }
     }
@@ -209,7 +218,7 @@ public void setUltrasonicPin(String pin_name){
      *
      * @param sen   SensorContainer to add to the HashMap. This will be grouped into the array indexed by the sensor's type
      */
-    private void updateTypes_Inv(SensorContainer sen){
+    private void updateTypesInv(SensorContainer sen){
         SensorContainer[] old_sensors = types_inv.get(sen.type);        // Pull out the old array of sensors
         int old_length = old_sensors.length;
 
@@ -232,10 +241,13 @@ private void updateTypes_Inv(SensorContainer sen){
     ************************
      */
 
+    // All of these will throw errors if called with a name that hasn't been registered.
+
     /**
      * Returns true if the given gyro is currently calibrating, and therefore can't give good values.
      */
     public synchronized boolean gyroIsCalibrating(String gyro_name){
+        assert(sensorContainers.keySet().contains(gyro_name));
         return ((GyroSensor)sensorContainers.get(gyro_name).sensor).isCalibrating();
     }
 
@@ -243,10 +255,15 @@ public synchronized boolean gyroIsCalibrating(String gyro_name){
      * Returns true if all the values of the filter array have been filled, allowing averaging.
      */
     public synchronized boolean filterIsFilled(String name){
+        assert(sensorContainers.keySet().contains(name));
         return sensorContainers.get(name).filter.isFilled();
     }
 
+    /**
+     * Change the length of the specified filter
+     */
     public synchronized void changeFilterLength(String name, int fl){
+        assert(sensorContainers.keySet().contains(name));
         sensorContainers.get(name).filter.changeFilter_length(fl);
     }
 
@@ -255,6 +272,7 @@ public synchronized void changeFilterLength(String name, int fl){
      * Allows us to access by name only in the public functions.
      */
     public synchronized ColorType getColorData(String name){
+        assert(sensorContainers.keySet().contains(name));
         return getDominantColor(sensorContainers.get(name));
     }
 
@@ -262,6 +280,7 @@ public synchronized ColorType getColorData(String name){
      * Immediately return a value for the given sensor, without waiting for another run().
      */
     public synchronized double getSensorReading(String name){
+        assert(sensorContainers.keySet().contains(name));
         return getSensorReading(sensorContainers.get(name));
     }
 
@@ -270,6 +289,7 @@ public synchronized double getSensorReading(String name){
      * cpu cycles.
      */
     public synchronized void changeUpdateStatus(String name, boolean update){
+        assert(sensorContainers.keySet().contains(name));
         sensorContainers.get(name).update = update;
     }
 
@@ -280,6 +300,7 @@ public synchronized void changeUpdateStatus(String name, boolean update){
      * @return      The SensorData object corresponding to the given sensor.
      */
     public synchronized Filter getFilter(String name){
+        assert(sensorContainers.keySet().contains(name));
         return sensorContainers.get(name).filter.clone();
     }
 
@@ -287,6 +308,7 @@ public synchronized Filter getFilter(String name){
      * Using the most recent chronological sensor data, average the last several readings
      */
     public synchronized double getAvgSensorData(String name) {
+        assert(sensorContainers.keySet().contains(name));
         return sensorContainers.get(name).filter.getAvg();
     }
 
@@ -294,6 +316,7 @@ public synchronized double getAvgSensorData(String name) {
      * Get a String[] array of all sensor names belonging to sensors of a certain type.
      */
     public synchronized String[] getSensorsFromType(SensorType type){
+        assert(types_inv.keySet().contains(type));
         SensorContainer[] sens = types_inv.get(type);
         String[] ret = new String[sens.length];
 
@@ -368,7 +391,7 @@ private double getSensorReading(SensorContainer sen){
                             ex.printStackTrace();
                         }
                     } else {
-                        throw new RuntimeException();
+                        throw new RuntimeException("usPin not set in SensorState");
                     }
 
                 case LIGHT:

From d929a97af862fbfb684c7a6855fd7b94dc840b24 Mon Sep 17 00:00:00 2001
From: Vivek Bhupatiraju <triforce314@gmail.com>
Date: Fri, 8 Jan 2016 15:43:55 -0500
Subject: [PATCH 2/2] new Robot tested, worked

---
 FtcRobotController/FtcRobotController.iml     | 30 ++++++++++---------
 .../ftcrobotcontroller/opmodes/DropPath.java  | 10 ++-----
 .../opmodes/NewDrivetrainTest.java            | 16 ----------
 .../src/main/java/lib/Robot.java              |  4 +--
 4 files changed, 20 insertions(+), 40 deletions(-)

diff --git a/FtcRobotController/FtcRobotController.iml b/FtcRobotController/FtcRobotController.iml
index 378e4b32a81..de9e7076842 100644
--- a/FtcRobotController/FtcRobotController.iml
+++ b/FtcRobotController/FtcRobotController.iml
@@ -12,12 +12,10 @@
         <option name="SELECTED_TEST_ARTIFACT" value="_android_test_" />
         <option name="ASSEMBLE_TASK_NAME" value="assembleDebug" />
         <option name="COMPILE_JAVA_TASK_NAME" value="compileDebugSources" />
+        <option name="SOURCE_GEN_TASK_NAME" value="generateDebugSources" />
         <option name="ASSEMBLE_TEST_TASK_NAME" value="assembleDebugAndroidTest" />
         <option name="COMPILE_JAVA_TEST_TASK_NAME" value="compileDebugAndroidTestSources" />
-        <afterSyncTasks>
-          <task>generateDebugAndroidTestSources</task>
-          <task>generateDebugSources</task>
-        </afterSyncTasks>
+        <option name="TEST_SOURCE_GEN_TASK_NAME" value="generateDebugAndroidTestSources" />
         <option name="ALLOW_USER_CONFIGURATION" value="false" />
         <option name="MANIFEST_FILE_RELATIVE_PATH" value="/src/main/AndroidManifest.xml" />
         <option name="RES_FOLDER_RELATIVE_PATH" value="/src/main/res" />
@@ -65,9 +63,12 @@
       <sourceFolder url="file://$MODULE_DIR$/src/androidTest/jni" isTestSource="true" />
       <sourceFolder url="file://$MODULE_DIR$/src/androidTest/rs" isTestSource="true" />
       <excludeFolder url="file://$MODULE_DIR$/build/intermediates/assets" />
+      <excludeFolder url="file://$MODULE_DIR$/build/intermediates/bundles" />
       <excludeFolder url="file://$MODULE_DIR$/build/intermediates/classes" />
+      <excludeFolder url="file://$MODULE_DIR$/build/intermediates/coverage-instrumented-classes" />
       <excludeFolder url="file://$MODULE_DIR$/build/intermediates/dependency-cache" />
       <excludeFolder url="file://$MODULE_DIR$/build/intermediates/dex" />
+      <excludeFolder url="file://$MODULE_DIR$/build/intermediates/dex-cache" />
       <excludeFolder url="file://$MODULE_DIR$/build/intermediates/exploded-aar/Analytics-release/jars" />
       <excludeFolder url="file://$MODULE_DIR$/build/intermediates/exploded-aar/FtcCommon-release/jars" />
       <excludeFolder url="file://$MODULE_DIR$/build/intermediates/exploded-aar/Hardware-release/jars" />
@@ -75,27 +76,28 @@
       <excludeFolder url="file://$MODULE_DIR$/build/intermediates/exploded-aar/RobotCore-release/jars" />
       <excludeFolder url="file://$MODULE_DIR$/build/intermediates/exploded-aar/WirelessP2p-release/jars" />
       <excludeFolder url="file://$MODULE_DIR$/build/intermediates/incremental" />
+      <excludeFolder url="file://$MODULE_DIR$/build/intermediates/jacoco" />
+      <excludeFolder url="file://$MODULE_DIR$/build/intermediates/javaResources" />
+      <excludeFolder url="file://$MODULE_DIR$/build/intermediates/libs" />
+      <excludeFolder url="file://$MODULE_DIR$/build/intermediates/lint" />
       <excludeFolder url="file://$MODULE_DIR$/build/intermediates/manifests" />
+      <excludeFolder url="file://$MODULE_DIR$/build/intermediates/ndk" />
       <excludeFolder url="file://$MODULE_DIR$/build/intermediates/pre-dexed" />
+      <excludeFolder url="file://$MODULE_DIR$/build/intermediates/proguard" />
       <excludeFolder url="file://$MODULE_DIR$/build/intermediates/res" />
-      <excludeFolder url="file://$MODULE_DIR$/build/intermediates/resources" />
       <excludeFolder url="file://$MODULE_DIR$/build/intermediates/rs" />
       <excludeFolder url="file://$MODULE_DIR$/build/intermediates/symbols" />
-      <excludeFolder url="file://$MODULE_DIR$/build/intermediates/tmp" />
       <excludeFolder url="file://$MODULE_DIR$/build/outputs" />
       <excludeFolder url="file://$MODULE_DIR$/build/tmp" />
     </content>
-    <orderEntry type="jdk" jdkName="Android API 19 Platform" jdkType="Android SDK" />
+    <orderEntry type="jdk" jdkName="Android 4.4.2 Google APIs" jdkType="Android SDK" />
     <orderEntry type="sourceFolder" forTests="false" />
-    <orderEntry type="library" exported="" name="RobotCore-release-" level="project" />
+    <orderEntry type="library" exported="" name="android-support-v4" level="project" />
     <orderEntry type="library" exported="" name="ModernRobotics-release-" level="project" />
-    <orderEntry type="library" exported="" name="WirelessP2p-release-" level="project" />
     <orderEntry type="library" exported="" name="Analytics-release-" level="project" />
-    <orderEntry type="library" exported="" name="android-support-v4" level="project" />
-    <orderEntry type="library" exported="" name="Hardware-release-" level="project" />
     <orderEntry type="library" exported="" name="FtcCommon-release-" level="project" />
-    <orderEntry type="library" exported="" name="usb-android-19" level="project" />
-    <orderEntry type="library" exported="" name="effects-android-19" level="project" />
-    <orderEntry type="library" exported="" name="maps-android-19" level="project" />
+    <orderEntry type="library" exported="" name="WirelessP2p-release-" level="project" />
+    <orderEntry type="library" exported="" name="Hardware-release-" level="project" />
+    <orderEntry type="library" exported="" name="RobotCore-release-" level="project" />
   </component>
 </module>
\ No newline at end of file
diff --git a/FtcRobotController/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/DropPath.java b/FtcRobotController/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/DropPath.java
index 74f416fe250..ec2d9b50a95 100644
--- a/FtcRobotController/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/DropPath.java
+++ b/FtcRobotController/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/DropPath.java
@@ -1,12 +1,6 @@
 package com.qualcomm.ftcrobotcontroller.opmodes;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.eventloop.opmode.OpMode;
-import com.qualcomm.robotcore.hardware.AnalogInput;
-import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.LightSensor;
-import com.qualcomm.robotcore.util.Range;
 
-import lib.FourWheelDrive;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import lib.Robot;
 import lib.SensorState;
 import lib.TwoWheelDrive;
@@ -39,7 +33,7 @@ public void runOpMode() throws InterruptedException {
 
         while (Robot.state.gyroIsCalibrating("hero"));
 
-//        dave_train.move(0.2, "hero", this);
+        dave_train.turnWithGyro(90, "hero");
 
         Robot.colorSweep(SensorState.ColorType.BLUE, 4, 10, -0.3, "mrs", "mr", "hero");
 
diff --git a/FtcRobotController/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/NewDrivetrainTest.java b/FtcRobotController/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/NewDrivetrainTest.java
index a0812cca87a..fbd41223591 100644
--- a/FtcRobotController/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/NewDrivetrainTest.java
+++ b/FtcRobotController/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/NewDrivetrainTest.java
@@ -15,29 +15,13 @@ public class NewDrivetrainTest extends LinearOpMode {
     public void runOpMode() throws InterruptedException{
         Robot.init(hardwareMap, telemetry, this); // makes Robot "dave"
 
-        DriveTrain dave_train = new TwoWheelDrive(
-                hardwareMap.dcMotor.get("leftdrive"), true,
-                hardwareMap.dcMotor.get("rightdrive"), false, 10);
-
-        Robot.registerDrivetrain(dave_train);
-
         Robot.state = new SensorState(hardwareMap, 1, 0);
-        Robot.state.registerSensor("mr", SensorState.SensorType.COLOR, false, 12);
-        Robot.state.registerSensor("mrs", SensorState.SensorType.LIGHT, true, 12);
-        Robot.state.registerSensor("hero", SensorState.SensorType.GYRO, true, 12);
-        Robot.state.registerSensor("rearUltra", SensorState.SensorType.ULTRASONIC, true, 50);
-        Robot.state.registerSensor("frontUltra", SensorState.SensorType.ULTRASONIC, true, 50);
-        Robot.state.setUltrasonicPin("ultraToggle");
 
         Thread state_thread = new Thread(Robot.state);
         state_thread.start();
 
         waitForStart();
 
-        while (Robot.state.gyroIsCalibrating("hero"));
-
-        dave_train.turnWithGyro(45,"hero");
-
         while (opModeIsActive()){
             try {
                 Thread.sleep(10);
diff --git a/FtcRobotController/src/main/java/lib/Robot.java b/FtcRobotController/src/main/java/lib/Robot.java
index 442be7bc4dc..6146a6957a5 100644
--- a/FtcRobotController/src/main/java/lib/Robot.java
+++ b/FtcRobotController/src/main/java/lib/Robot.java
@@ -31,12 +31,12 @@ public class Robot {
     // Prevents instantiation
     private Robot(){}
 
-    public static void init (HardwareMap hmap, Telemetry tel, LinearOpMode opm) {
+    public static void init (HardwareMap hmap, Telemetry tel, LinearOpMode waiter) {
         Robot.hmap = hmap;
         Robot.tel = tel;
         Robot.servos = new HashMap<String, Servo>();
         Robot.ultraservohelper = new UltraServoHelper();
-        Robot.waiter = opm;
+        Robot.waiter = waiter;
     }
 
     // REGISTRATION FUNCTIONS