cry about it

src/main/java/frc4388/robot/RobotMap.java

@@ -186,9 +186,9 @@ public class RobotMap {
   PWM         rightClaw  = new PWM(1);
   CANSparkMax spinnyspin = new CANSparkMax(18, MotorType.kBrushless);
 
-  // ============================================
-  // =============== AUTO TESTING ===============
-  // ============================================
+  // * ============================================
+  // * =============== AUTO TESTING ===============
+  // * ============================================
 
   static class MotorTest {
     boolean result;
@@ -205,8 +205,8 @@ public class RobotMap {
   private static final long TEST_TIME = 500;
 
   public ArrayList<MotorTest> motor_tests = new ArrayList<>();
-  private int    test_count = 0;
-  private long   test_time  = 0;
+  private int    test_count     = 0;
+  private long   test_time      = 0;
   private double motor_position = 0;
 
   public void restart_motor_tests() {
@@ -222,23 +222,9 @@ public class RobotMap {
   public boolean run_periodic_tests() {
     if (test_count >= motor_tests.size()) return true;
 
+    // fine
     MotorTest current_test = motor_tests.get(test_count);
 
-    if (test_time == 0) {
-      test_time = System.currentTimeMillis();
-
-      switch (current_test.motor_type) {
-        case FALCON:
-          motor_position = current_test.as_falcon.getSelectedSensorPosition();
-          current_test.as_falcon.set(.1);
-          break;
-
-        case NEO:
-          // TODO: destroy robot
-          break;
-      }
-    }
-
     if (System.currentTimeMillis() - test_time >= TEST_TIME) {
       switch (current_test.motor_type) {
         case FALCON:
@@ -247,24 +233,40 @@ public class RobotMap {
           if (Math.abs(new_pos - motor_position) > .1) {
             current_test.result = true;
           } else {
-            System.out.printf("[DISCONNECTED] %s\n", current_test.name);
+            System.err.printf("[DISCONNECTED] %s\n", current_test.name);
           }
 
+          current_test.as_falcon.stopMotor();
           break;
         }
 
-        case NEO: break;
+        case NEO:
+        {
+          double new_pos = current_test.as_neo.get();
+          break;
+        }
       }
 
-      test_time = 0;
-      test_count++;
+      test_time    = System.currentTimeMillis();
+      current_test = motor_tests.get(test_count++);
+
+      switch (current_test.motor_type) {
+        case FALCON:
+          motor_position = current_test.as_falcon.getSelectedSensorPosition();
+          current_test.as_falcon.set(.1);
+          break;
+
+        case NEO:
+          current_test.as_neo.set(.1);
+          break;
+      }
     }
 
     return false;
   }
 
   private void setup_motor_tests() throws IllegalArgumentException, IllegalAccessException {
-    Class map_clazz = this.getClass();
+    Class<? extends RobotMap> map_clazz = this.getClass();
     
     for (Field field : map_clazz.getFields()) {
       if (field.getType().equals(WPI_TalonFX.class)) {
@@ -273,6 +275,17 @@ public class RobotMap {
         test.name       = field.getName();
         test.motor_type = MotorTest.MotorType.FALCON;
         test.as_falcon  = (WPI_TalonFX) field.get(this);
+
+        motor_tests.add(test);
+      }
+      else if (field.getType().equals(CANSparkMax.class)) {
+        MotorTest test  = new MotorTest();
+        test.result     = false;
+        test.name       = field.getName();
+        test.motor_type = MotorTest.MotorType.NEO;
+        test.as_neo     = (CANSparkMax) field.get(this);
+
+        motor_tests.add(test);
       }
     }
   }

src/main/java/frc4388/robot/subsystems/SwerveDrive.java

@@ -84,6 +84,7 @@ public class SwerveDrive extends SubsystemBase {
     setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
   }
 
+ 
   /**
    * Set each module of the swerve drive to the corresponding desired state.
    * @param desiredStates Array of module states to set.
@@ -93,6 +94,8 @@ public class SwerveDrive extends SubsystemBase {
     for (int i = 0; i < desiredStates.length; i++) {
       SwerveModule module = modules[i];
       SwerveModuleState state = desiredStates[i];
+      SmartDashboard.putNumber("test motor" + i, module.getBusVoltage());
+     
       module.setDesiredState(state);
     }
   }

src/main/java/frc4388/robot/subsystems/SwerveModule.java

@@ -81,7 +81,9 @@ public class SwerveModule extends SubsystemBase {
         // * Note: This assumes that the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees.
         return Rotation2d.fromDegrees(encoder.getAbsolutePosition());
     }
-    
+    public double getBusVoltage() {
+        return this.driveMotor.getBusVoltage();
+    }
     public double getAngularVel() {
         return this.angleMotor.getSelectedSensorVelocity();
     }