diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 113cf05..208c1e4 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/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 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 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); } } } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 055230b..dd8e7ff 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/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); } } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 1ddab51..f445c86 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/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(); }