Merge branch 'master' of https://github.com/Spartronics4915/2019-Deep…

…Space

src/main/java/com/spartronics4915/frc2019/AutoModeSelector.java

@@ -42,7 +42,8 @@ private AutoModeCreator(String dashboardName, Supplier<AutoModeBase> creator)
             new AutoModeCreator("Other: Straight Path Test", () -> new PathTestMode(false)),
             new AutoModeCreator("Other: Curved Path Test", () -> new PathTestMode(true)),
             new AutoModeCreator("Other: Velocity Test", () -> new VelocityTestMode()),
-            new AutoModeCreator("Other: Characterize Drivetrain (Both)", () -> new CharacterizeDriveMode(SideToCharacterize.BOTH)),
+            new AutoModeCreator("Other: Diagnose Dropouts", () -> new DiagnoseDropoutsMode()),
+            new AutoModeCreator("Other: Characterize Drivetrain (Both+MOI)", () -> new CharacterizeDriveMode(SideToCharacterize.BOTH)),
             new AutoModeCreator("Other: Characterize Drivetrain (Left)", () -> new CharacterizeDriveMode(SideToCharacterize.LEFT)),
             new AutoModeCreator("Other: Characterize Drivetrain (Right)", () -> new CharacterizeDriveMode(SideToCharacterize.RIGHT)),
             new AutoModeCreator("Other: Characterize Drivetrain (Remote)", () -> new CharacterizeDriveRemoteMode())

src/main/java/com/spartronics4915/frc2019/Constants.java

@@ -29,30 +29,25 @@ public class Constants
     public static final double kDriveWheelTrackWidthInches = 25.75;
     public static final double kDriveWheelDiameterInches = 6;
     public static final double kDriveWheelRadiusInches = kDriveWheelDiameterInches / 2.0;
-    public static final double kTrackScrubFactor = 0.624; // Tune me!
+    public static final double kTrackScrubFactor = 1.202;
 
     // Tuned dynamics
-    public static final double kRobotLinearInertia = 27.22; // kg (robot's mass, guessed to be 60 lbs == 27.22 kg) TODO tune
-    public static final double kRobotAngularInertia = 461; // kg m^2 (from an online calculator) TODO tune
+    public static final double kRobotLinearInertia = 17.75; // kg (robot's mass) TODO tune
+    public static final double kRobotAngularInertia = 1.13; // kg m^2 (use the moi auto mode) TODO tune
     public static final double kRobotAngularDrag = 12.0; // N*m / (rad/sec) TODO tune
 
     // Right
-    public static final double kDriveRightVIntercept = 0.7503; // V
-    public static final double kDriveRightKv = 0.9288; // V per rad/s
-    public static final double kDriveRightKa = 0.1583; // V per rad/s^2
+    public static final double kDriveRightVIntercept = 0.6519; // V
+    public static final double kDriveRightKv = 0.2417; // V per rad/s
+    public static final double kDriveRightKa = 0.0214; // V per rad/s^2
 
     // Left
-    public static final double kDriveLeftVIntercept = 0.5574; // V
-    public static final double kDriveLeftKv = 0.9480; // V per rad/s
-    public static final double kDriveLeftKa = 0.1583; // V per rad/s^2
+    public static final double kDriveLeftVIntercept = 0.7111; // V
+    public static final double kDriveLeftKv = 0.2447; // V per rad/s
+    public static final double kDriveLeftKa = 0.0300; // V per rad/s^2
 
-    public static final double kDriveLeftDeadband = 0.078;
-    public static final double kDriveRightDeadband = 0.068;
-
-    // Geometry
-    public static final double kCenterToFrontBumperDistance = 38.25 / 2.0;
-    public static final double kCenterToRearBumperDistance = 38.25 / 2.0;
-    public static final double kCenterToSideBumperDistance = 33.75 / 2.0;
+    public static final double kDriveLeftDeadband = 0.04;
+    public static final double kDriveRightDeadband = 0.04;
 
     // LIDAR CONSTANTS ----------------
     public static final IReferenceModel kSegmentReferenceModel = new SegmentReferenceModel(
@@ -70,20 +65,32 @@ public class Constants
     public static final double kDriveDownShiftAngularVelocity = Math.PI / 2.0; // rad/sec
     public static final double kDriveUpShiftVelocity = 11.0 * 12.0; // inches per second
 
-    public static final double kPathKX = 10.0; // units/s per unit of error
+    // Adaptive pure pursuit
+    public static final double kPathKX = 10.0; // units/s per unit of error... This is essentially a P gain on longitudinal error
     public static final double kPathLookaheadTime = 0.4; // seconds to look ahead along the path for steering
     public static final double kPathMinLookaheadDistance = 24.0; // inches
 
     // PID gains for drive velocity loop (LOW GEAR)
     // Units: setpoint, error, and output are in ticks per second.
     public static final int kPositionPIDSlot = 0; // for compat with 2018
     public static final int kVelocityPIDSlot = 1; // for compat with 2018
-    public static final double kDriveVelocityKp = 5.0;
+
+    public static final double kDriveVelocityKp = 3.0;
     public static final double kDriveVelocityKi = 0.0;
     public static final double kDriveVelocityKd = 50.0;
     // The below should always be zero, because feedforward is dynamically produced by DriveMotionPlanner
     public static final double kDriveVelocityKf = 0.0;
     public static final int kDriveVelocityIZone = 0;
+
+    public static final double kDrivePositionKp = 8;
+    public static final double kDrivePositionKi = 0.0;
+    public static final double kDrivePositionKd = 0.0;
+    // Don't do feedforward on position control loops
+    public static final double kDrivePositionKf = 0.0;
+    public static final int kDrivePositionIZone = 0;
+    public static final double kTurnDegreeTolerance = 2; // Degrees
+    public static final double kTurnVelTolerance = 0.2; // in/sec
+
     public static final double kDriveVoltageRampRate = 0.0;
 
     /* I/O */

src/main/java/com/spartronics4915/frc2019/Robot.java

@@ -5,6 +5,7 @@
 import com.spartronics4915.frc2019.paths.TrajectoryGenerator;
 import com.spartronics4915.frc2019.subsystems.*;
 import com.spartronics4915.frc2019.subsystems.CargoChute.WantedState;
+import com.spartronics4915.lib.geometry.Rotation2d;
 import com.spartronics4915.lib.util.*;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.TimedRobot;
@@ -35,6 +36,7 @@ public class Robot extends TimedRobot
     private CargoIntake mCargoIntake = null;
     private Climber mClimber = null;
     private LED mLED = null;
+    private RobotStateEstimator mRobotStateEstimator = null;
     private Superstructure mSuperstructure = null;
     private AutoModeExecutor mAutoModeExecutor;
 
@@ -100,10 +102,11 @@ public void robotInit()
                 mClimber = Climber.getInstance();
                 mLED = LED.getInstance();
                 mSuperstructure = Superstructure.getInstance();
+                mRobotStateEstimator = RobotStateEstimator.getInstance();
 
                 mSubsystemManager = new SubsystemManager(
                         Arrays.asList(
-                                RobotStateEstimator.getInstance(),
+                                mRobotStateEstimator,
                                 mDrive,
                                 mPanelHandler,
                                 mCargoChute,
@@ -153,7 +156,7 @@ public void disabledInit()
             }
 
             Drive.getInstance().zeroSensors();
-            RobotStateEstimator.getInstance().resetRobotStateMaps(Timer.getFPGATimestamp());
+            mRobotStateEstimator.resetRobotStateMaps(Timer.getFPGATimestamp());
 
             // Reset all auto mode state.
             mAutoModeExecutor = new AutoModeExecutor();
@@ -179,7 +182,7 @@ public void autonomousInit()
 
             mDisabledLooper.stop();
 
-            RobotStateEstimator.getInstance().resetRobotStateMaps(Timer.getFPGATimestamp());
+            mRobotStateEstimator.resetRobotStateMaps(Timer.getFPGATimestamp());
             Drive.getInstance().zeroSensors();
 
             mAutoModeExecutor.setAutoMode(AutoModeSelector.getSelectedAutoMode());
@@ -321,16 +324,15 @@ public void teleopPeriodic()
             if (mSuperstructure.isDriverControlled())
             {
                 DriveSignal command = ArcadeDriveHelper.arcadeDrive(mControlBoard.getThrottle(), mControlBoard.getTurn(),
-                        true /* TODO: Decide squared inputs or not */).scale(mSuperstructure.isDrivingReversed() ? -1 : 1)/* .scale(6) */;
+                        true /* TODO: Decide squared inputs or not */).scale(mSuperstructure.isDrivingReversed() ? -1 : 1)/*.scale(48)*/;
 
                 mDrive.setOpenLoop(command);
-                /* mDrive.setVelocity(command, new DriveSignal(command.scale(Constants.kDriveLeftKv).getLeft()
-                 *     + Math.copySign(Constants.kDriveLeftVIntercept, command.getLeft()),
-                 *     command.scale(Constants.kDriveLeftKv).getRight()
-                 *     + Math.copySign(Constants.kDriveLeftVIntercept, command.getRight())));
-                 */
-
-                if (mControlBoard.getEjectPanel()) // 1: 6
+                // mDrive.setVelocity(command, new DriveSignal(
+                //     command.scale(Constants.kDriveLeftKv * (Constants.kDriveWheelDiameterInches / 2)).getLeft() + Math.copySign(Constants.kDriveLeftVIntercept, command.getLeft()),
+                //     command.scale(Constants.kDriveRightKv * (Constants.kDriveWheelDiameterInches / 2)).getRight() + Math.copySign(Constants.kDriveRightVIntercept, command.getRight())
+                // )); XXX Conversions on Kv are wrong
+
+                if(mControlBoard.getEjectPanel())// 1: 6
                     mPanelHandler.setWantedState(PanelHandler.WantedState.EJECT);
 
                 if (mControlBoard.getIntake()) // 1: 2
@@ -380,7 +382,7 @@ else if (mControlBoard.getEjectCargo())
                 }
                 else if (mControlBoard.getDriveToSelectedTarget())
                 {
-                    mSuperstructure.setWantedState(Superstructure.WantedState.ALIGN_AND_INTAKE_CARGO);
+                    mSuperstructure.setWantedState(Superstructure.WantedState.ALIGN_AND_INTAKE_PANEL);
                 }
                 else if (mControlBoard.getClimb())
                 {

src/main/java/com/spartronics4915/frc2019/VisionUpdateManager.java

@@ -1,5 +1,7 @@
 package com.spartronics4915.frc2019;
 
+import java.util.Optional;
+
 import com.spartronics4915.lib.geometry.Pose2d;
 import com.spartronics4915.lib.geometry.Rotation2d;
 import com.spartronics4915.lib.util.Logger;
@@ -47,9 +49,9 @@ private void visionKeyChangedCallback(EntryNotification entryNotification)
     /**
      * @return the latest vision update _or_ null if there has not been an update yet
      */
-    public VisionUpdate getLatestVisionUpdate()
+    public Optional<VisionUpdate> getLatestVisionUpdate()
     {
-        return mLatestVisionUpdate;
+        return Optional.ofNullable(mLatestVisionUpdate);
     }
 
     public static class VisionUpdate

src/main/java/com/spartronics4915/frc2019/auto/actions/CollectAccelerationData.java

@@ -4,11 +4,13 @@
 import com.spartronics4915.frc2019.auto.modes.CharacterizeDriveMode.SideToCharacterize;
 import com.spartronics4915.frc2019.subsystems.Drive;
 import com.spartronics4915.lib.physics.DriveCharacterization;
+import com.spartronics4915.lib.physics.DriveCharacterization.AccelerationDataPoint;
 import com.spartronics4915.lib.util.DriveSignal;
 import com.spartronics4915.lib.util.Logger;
 import com.spartronics4915.lib.util.ReflectingCSVWriter;
 import com.spartronics4915.lib.util.Util;
 import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 import java.nio.file.Paths;
 import java.util.List;
@@ -17,7 +19,7 @@ public class CollectAccelerationData implements Action
 {
 
     private static final double kPower = 0.5;
-    private static final double kTotalTime = 2.0; //how long to run the test for
+    private static final double kTotalTime = 3.0; //how long to run the test for
     private static final Drive mDrive = Drive.getInstance();
 
     private final ReflectingCSVWriter<DriveCharacterization.AccelerationDataPoint> mCSVWriter;
@@ -32,15 +34,13 @@ public class CollectAccelerationData implements Action
 
     /**
      * This test collects data about the behavior of the drivetrain in a "dynamic
-     * test",
-     * where we set the demand to a constant value and sample acceleration. The idea
-     * is that velocity is that steady-state velocity is negligle in this case, so
-     * we are only measuring the effects of acceleration.
+     * test", where we set the demand to a constant value and sample acceleration.
+     * The idea is that velocity is that steady-state velocity is negligle in this
+     * case, so we are only measuring the effects of acceleration.
      * 
-     * There are a couple of issues with this test (we should sample voltage
-     * directly, and we should subtract the voltage caused by velocity from this
-     * applied voltage), and it is advised to use the FeedRemoteCharacterization
-     * action instead of this.
+     * There are a couple of issues with this test (we should subtract the voltage
+     * caused by velocity from this applied voltage), and one should examine if the
+     * FeedRemoteCharacterization action is appropriate instead of this.
      * 
      * @param data        reference to the list where data points should be stored
      * @param reverse     if true drive in reverse, if false drive normally
@@ -67,17 +67,21 @@ public void start()
                 (mReverse ? -1.0 : 1.0) * (mTurn ? -1.0 : 1.0) * kPower));
         mStartTime = Timer.getFPGATimestamp();
         mPrevTime = mStartTime;
+        SmartDashboard.putBoolean("isDone", false);
         Logger.debug("Collecting acceleration data");
     }
 
     @Override
     public void update()
     {
+        // convert to radians/sec
         double currentVelocity =
                 mSide.getVelocityTicksPer100ms(mDrive) / Constants.kDriveEncoderPPR * (2 * Math.PI) * 10;
         double currentTime = Timer.getFPGATimestamp();
 
-        //don't calculate acceleration until we've populated prevTime and prevVelocity
+        SmartDashboard.putNumber("CollectAccelerationData/currentTime", currentTime);
+
+        // don't calculate acceleration until we've populated prevTime and prevVelocity
         if (mPrevTime == mStartTime)
         {
             mPrevTime = currentTime;
@@ -87,7 +91,7 @@ public void update()
 
         double acceleration = (currentVelocity - mPrevVelocity) / (currentTime - mPrevTime);
 
-        //ignore accelerations that are too small
+        // ignore accelerations that are effectively 0
         if (acceleration < Util.kEpsilon)
         {
             mPrevTime = currentTime;
@@ -96,8 +100,8 @@ public void update()
         }
 
         mAccelerationData.add(new DriveCharacterization.AccelerationDataPoint(
-                currentVelocity, //converted to radians per second
-                kPower * 12.0, //convert to volts
+                currentVelocity, // rads/sec
+                mSide.getVoltage(mDrive), //convert to volts
                 acceleration));
 
         mCSVWriter.add(mAccelerationData.get(mAccelerationData.size() - 1));
@@ -109,13 +113,19 @@ public void update()
     @Override
     public boolean isFinished()
     {
-        return Timer.getFPGATimestamp() - mStartTime > kTotalTime;
+        return Timer.getFPGATimestamp() - mStartTime > kTotalTime || SmartDashboard.getBoolean("isDone", false);
     }
 
     @Override
     public void done()
     {
+        // Remove anything before we hit our max acceleration (i.e. don't log the ramp-up period)
+        AccelerationDataPoint maxDataPoint = mAccelerationData.stream()
+                .max((AccelerationDataPoint dp0, AccelerationDataPoint dp1) -> Double.compare(Math.abs(dp0.acceleration), Math.abs(dp1.acceleration)))
+                .orElseThrow();
+        mAccelerationData.subList(0, mAccelerationData.indexOf(maxDataPoint)).clear();
+
         mDrive.setOpenLoop(DriveSignal.BRAKE);
-        mCSVWriter.flush();
+        mCSVWriter.flush(); // CSV writer will have value around 0 removed, but the max accel trimming will not be applied
     }
 }

src/main/java/com/spartronics4915/frc2019/auto/actions/CollectVelocityData.java

@@ -7,15 +7,18 @@
 import com.spartronics4915.lib.util.DriveSignal;
 import com.spartronics4915.lib.util.Logger;
 import com.spartronics4915.lib.util.ReflectingCSVWriter;
+import com.spartronics4915.lib.util.Util;
+
 import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 import java.nio.file.Paths;
 import java.util.List;
 
 public class CollectVelocityData implements Action
 {
 
-    private static final double kMaxPower = 0.5;
+    private static final double kMaxPower = 0.256;
     private static final double kRampRate = 0.02;
     private static final Drive mDrive = Drive.getInstance();
 
@@ -29,16 +32,18 @@ public class CollectVelocityData implements Action
     private double mStartTime = 0.0;
 
     /**
-     * This is a quasi-static test to determine Kv (the slope of of the voltage-speed curve).
-     * This test collects data on robot (for convenience) so a regression can be run.
+     * This is a quasi-static test to determine Kv (the slope of of the
+     * voltage-speed curve).
+     * This test collects data on robot (for convenience) so a regression can be
+     * run.
      * 
-     * If you want to calculate the moment of inertia then you should run this test turning
-     * in place. Otherwise you should go straight.
+     * If you want to calculate the moment of inertia then you should run this test
+     * turning in place. Otherwise you should go straight.
      * 
-     * @param data     reference to the list where data points should be stored
-     * @param reverse  if true drive in reverse, if false drive normally
-     * @param turnInPlace     if true turn, if false drive straight
-     * @param side     the side to collect data from/run
+     * @param data        reference to the list where data points should be stored
+     * @param reverse     if true drive in reverse, if false drive normally
+     * @param turnInPlace if true turn, if false drive straight
+     * @param side        the side to collect data from/run
      */
 
     public CollectVelocityData(List<DriveCharacterization.VelocityDataPoint> data, boolean reverse, boolean turnInPlace, SideToCharacterize side)
@@ -47,14 +52,16 @@ public CollectVelocityData(List<DriveCharacterization.VelocityDataPoint> data, b
         mReverse = reverse;
         mTurn = turnInPlace;
         mSide = side;
-        mCSVWriter = new ReflectingCSVWriter<>(Paths.get(System.getProperty("user.home"), "VELOCITY_DATA.csv").toString(), DriveCharacterization.VelocityDataPoint.class);
+        mCSVWriter = new ReflectingCSVWriter<>(Paths.get(System.getProperty("user.home"), "VELOCITY_DATA.csv").toString(),
+                DriveCharacterization.VelocityDataPoint.class);
 
     }
 
     @Override
     public void start()
     {
         mStartTime = Timer.getFPGATimestamp();
+        SmartDashboard.putBoolean("isDone", false);
         Logger.debug("Collecting velocity data");
     }
 
@@ -67,13 +74,21 @@ public void update()
             isFinished = true;
             return;
         }
+
+        SmartDashboard.putNumber("CollectVelocityData/percentPower", percentPower);
+
         mDrive.setOpenLoop(new DriveSignal(
-            (mReverse ? -1.0 : 1.0) * percentPower,
-            (mReverse ? -1.0 : 1.0) * (mTurn ? -1.0 : 1.0) * percentPower)
-        );
+                (mReverse ? -1.0 : 1.0) * percentPower,
+                (mReverse ? -1.0 : 1.0) * (mTurn ? -1.0 : 1.0) * percentPower));
+
+        // convert to rads/sec
+        double velocity = mSide.getVelocityTicksPer100ms(mDrive) / Constants.kDriveEncoderPPR * (2 * Math.PI) * 10;
+        if (velocity < Util.kEpsilon)
+            return;
+
         mVelocityData.add(new DriveCharacterization.VelocityDataPoint(
-                mSide.getVelocityTicksPer100ms(mDrive) / Constants.kDriveEncoderPPR * (2 * Math.PI) * 10, //convert velocity to radians per second
-                percentPower * 12.0 //convert to volts
+                velocity, // rads/sec
+                mSide.getVoltage(mDrive) // convert to volts
         ));
         mCSVWriter.add(mVelocityData.get(mVelocityData.size() - 1));
 
@@ -82,7 +97,7 @@ public void update()
     @Override
     public boolean isFinished()
     {
-        return isFinished;
+        return isFinished || SmartDashboard.getBoolean("isDone", false);
     }
 
     @Override