diff --git a/src/main/java/com/spartronics4915/frc2019/AutoModeSelector.java b/src/main/java/com/spartronics4915/frc2019/AutoModeSelector.java index 02b5c94..4def1d9 100755 --- a/src/main/java/com/spartronics4915/frc2019/AutoModeSelector.java +++ b/src/main/java/com/spartronics4915/frc2019/AutoModeSelector.java @@ -42,7 +42,8 @@ private AutoModeCreator(String dashboardName, Supplier 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()) diff --git a/src/main/java/com/spartronics4915/frc2019/Constants.java b/src/main/java/com/spartronics4915/frc2019/Constants.java index ab014e9..19a3631 100755 --- a/src/main/java/com/spartronics4915/frc2019/Constants.java +++ b/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,7 +65,8 @@ 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 @@ -78,12 +74,23 @@ public class Constants // 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 */ diff --git a/src/main/java/com/spartronics4915/frc2019/Robot.java b/src/main/java/com/spartronics4915/frc2019/Robot.java index 427a903..137266a 100755 --- a/src/main/java/com/spartronics4915/frc2019/Robot.java +++ b/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()) { diff --git a/src/main/java/com/spartronics4915/frc2019/VisionUpdateManager.java b/src/main/java/com/spartronics4915/frc2019/VisionUpdateManager.java index 6de3bd3..9b967e2 100644 --- a/src/main/java/com/spartronics4915/frc2019/VisionUpdateManager.java +++ b/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 getLatestVisionUpdate() { - return mLatestVisionUpdate; + return Optional.ofNullable(mLatestVisionUpdate); } public static class VisionUpdate diff --git a/src/main/java/com/spartronics4915/frc2019/auto/actions/CollectAccelerationData.java b/src/main/java/com/spartronics4915/frc2019/auto/actions/CollectAccelerationData.java index 2b4bf7d..ea9feee 100755 --- a/src/main/java/com/spartronics4915/frc2019/auto/actions/CollectAccelerationData.java +++ b/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 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 } } diff --git a/src/main/java/com/spartronics4915/frc2019/auto/actions/CollectVelocityData.java b/src/main/java/com/spartronics4915/frc2019/auto/actions/CollectVelocityData.java index ae8d309..474acb3 100755 --- a/src/main/java/com/spartronics4915/frc2019/auto/actions/CollectVelocityData.java +++ b/src/main/java/com/spartronics4915/frc2019/auto/actions/CollectVelocityData.java @@ -7,7 +7,10 @@ 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; @@ -15,7 +18,7 @@ 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 data, boolean reverse, boolean turnInPlace, SideToCharacterize side) @@ -47,7 +52,8 @@ public CollectVelocityData(List 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); } @@ -55,6 +61,7 @@ public CollectVelocityData(List data, b 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 diff --git a/src/main/java/com/spartronics4915/frc2019/auto/actions/DiagnoseDropouts.java b/src/main/java/com/spartronics4915/frc2019/auto/actions/DiagnoseDropouts.java new file mode 100644 index 0000000..25d2746 --- /dev/null +++ b/src/main/java/com/spartronics4915/frc2019/auto/actions/DiagnoseDropouts.java @@ -0,0 +1,70 @@ +package com.spartronics4915.frc2019.auto.actions; + +import java.nio.file.Paths; + +import com.spartronics4915.frc2019.Constants; +import com.spartronics4915.frc2019.subsystems.Drive; +import com.spartronics4915.lib.util.DriveSignal; +import com.spartronics4915.lib.util.ReflectingCSVWriter; +import com.spartronics4915.lib.util.Units; + +import edu.wpi.first.wpilibj.Timer; + +public class DiagnoseDropouts implements Action +{ + + private final DriveSignal kPercentDemand = new DriveSignal(0.5, 0.5); + private final double kRunTime = 10.0; // Seconds + + private final Drive mDrive = Drive.getInstance(); + private final Timer mTimer = new Timer(); + private final ReflectingCSVWriter mCSVWriter; + + public DiagnoseDropouts() + { + mCSVWriter = new ReflectingCSVWriter<>(Paths.get(System.getProperty("user.home"), "DROPOUT_DATAs.csv").toString(), VelocityTimeDataPoint.class); + mTimer.start(); + mTimer.reset(); + } + + @Override + public boolean isFinished() + { + return mTimer.hasPeriodPassed(kRunTime); + } + + @Override + public void update() + { + mDrive.setOpenLoop(kPercentDemand); + mCSVWriter.add(new VelocityTimeDataPoint(mDrive.getLeftVelocityTicksPer100ms())); + } + + @Override + public void done() + { + mCSVWriter.flush(); + } + + @Override + public void start() + { + + } + + public class VelocityTimeDataPoint + { + + public final double velocity; // rps + public final double time; // seconds since RoboRIO boot + + /** + * @param velocity in ticks/100ms + */ + public VelocityTimeDataPoint(double velocity) + { + this.velocity = Units.rads_per_sec_to_rpm(velocity / Constants.kDriveEncoderPPR * (2 * Math.PI) * 10); + this.time = Timer.getFPGATimestamp(); + } + } +} diff --git a/src/main/java/com/spartronics4915/frc2019/auto/actions/FeedRemoteCharacterization.java b/src/main/java/com/spartronics4915/frc2019/auto/actions/FeedRemoteCharacterization.java index 43d1f14..3fc55b7 100644 --- a/src/main/java/com/spartronics4915/frc2019/auto/actions/FeedRemoteCharacterization.java +++ b/src/main/java/com/spartronics4915/frc2019/auto/actions/FeedRemoteCharacterization.java @@ -2,6 +2,7 @@ import java.util.Arrays; +import com.spartronics4915.frc2019.Constants; import com.spartronics4915.frc2019.subsystems.Drive; import com.spartronics4915.lib.util.DriveSignal; import com.spartronics4915.lib.util.Logger; @@ -40,15 +41,19 @@ public void update() { double now = Timer.getFPGATimestamp(); - // The script says to use ft and ft/s, but we use inches because that works better for us + // The script says to use ft/s, but we use radians because that works better for us - // in and in/s - double leftPosition = mDrive.getLeftEncoderDistance(); - double leftVelocity = mDrive.getLeftLinearVelocity(); + // ft and rads/s + double leftPosition = mDrive.getLeftEncoderDistance() / 12; + double leftVelocity = mDrive.getLeftVelocityTicksPer100ms() / Constants.kDriveEncoderPPR * (2 * Math.PI) * 10; - // in and in/s - double rightPosition = mDrive.getRightEncoderDistance(); - double rightVelocity = mDrive.getRightLinearVelocity(); + // ft and rad/s + double rightPosition = mDrive.getRightEncoderDistance() / 12; + double rightVelocity = mDrive.getRightVelocityTicksPer100ms() / Constants.kDriveEncoderPPR * (2 * Math.PI) * 10; + + // Yes, I know we're mixing ft and rad/s, but it's because the position value + // isn't used except to display a sanity check on the remote script. Radians + // are hard to read, so we use ft to make our sanity check more... sane. // volts double battery = RobotController.getBatteryVoltage(); diff --git a/src/main/java/com/spartronics4915/frc2019/auto/actions/ZeroOdometryFromVision.java b/src/main/java/com/spartronics4915/frc2019/auto/actions/ZeroOdometryFromVision.java index 234790d..d2d51d0 100644 --- a/src/main/java/com/spartronics4915/frc2019/auto/actions/ZeroOdometryFromVision.java +++ b/src/main/java/com/spartronics4915/frc2019/auto/actions/ZeroOdometryFromVision.java @@ -1,5 +1,7 @@ package com.spartronics4915.frc2019.auto.actions; +import java.util.Optional; + import com.spartronics4915.frc2019.VisionUpdateManager; import com.spartronics4915.frc2019.subsystems.RobotStateEstimator; import com.spartronics4915.lib.util.RobotStateMap; @@ -31,8 +33,7 @@ public boolean isFinished() @Override public void update() { - VisionUpdateManager.VisionUpdate visionUpdate = VisionUpdateManager.reverseVisionManager.getLatestVisionUpdate(); - if (visionUpdate != null) + VisionUpdateManager.reverseVisionManager.getLatestVisionUpdate().ifPresent(visionUpdate -> { mVisionCaptureTime = visionUpdate.frameCapturedTime; if (mVisionCaptureTime > mStartTime) @@ -42,7 +43,7 @@ public void update() mZeroed = true; } - } + }); } @Override diff --git a/src/main/java/com/spartronics4915/frc2019/auto/modes/CharacterizeDriveMode.java b/src/main/java/com/spartronics4915/frc2019/auto/modes/CharacterizeDriveMode.java index 4f6cb58..f6d266b 100755 --- a/src/main/java/com/spartronics4915/frc2019/auto/modes/CharacterizeDriveMode.java +++ b/src/main/java/com/spartronics4915/frc2019/auto/modes/CharacterizeDriveMode.java @@ -38,6 +38,22 @@ public double getVelocityTicksPer100ms(Drive drive) return 0.0; } } + + public double getVoltage(Drive drive) + { + switch (this) + { + case BOTH: + return (drive.getLeftOutputVoltage() + drive.getRightOutputVoltage()) / 2; + case LEFT: + return drive.getLeftOutputVoltage(); + case RIGHT: + return drive.getRightOutputVoltage(); + default: + Logger.error("Unrecognized side to characterize " + this.toString()); + return -1; + } + } } private final static boolean kReverse = false; @@ -55,12 +71,11 @@ protected void routine() throws AutoModeEndedException List linearVelData = new ArrayList<>(); List linearAccData = new ArrayList<>(); - List angularVelData = new ArrayList<>(); List angularAccData = new ArrayList<>(); boolean turnInPlace; - // first collect linear data + // First collect linear data Logger.debug("Collecting linear data"); turnInPlace = false; runAction(new CollectVelocityData(linearVelData, kReverse, turnInPlace, mSide)); @@ -68,18 +83,20 @@ protected void routine() throws AutoModeEndedException runAction(new CollectAccelerationData(linearAccData, kReverse, turnInPlace, mSide)); runAction(new WaitAction(10)); - // next collect angular data - Logger.debug("Collecting angular data"); - turnInPlace = true; - runAction(new CollectAccelerationData(angularAccData, kReverse, turnInPlace, mSide)); + // Next collect angular data... It doesn't make sense to do this unless we're characterizing both sides. + if (mSide == SideToCharacterize.BOTH) + { + Logger.debug("Collecting angular data"); + turnInPlace = true; + runAction(new CollectAccelerationData(angularAccData, kReverse, turnInPlace, mSide)); + double moi = DriveCharacterization.calculateAngularInertia(linearAccData, angularAccData, + Units.inches_to_meters(Constants.kDriveWheelRadiusInches), Constants.kRobotLinearInertia); + Logger.notice("J: " + moi); + } CharacterizationConstants constants = DriveCharacterization.characterizeDrive(linearVelData, linearAccData); Logger.notice("Ks: " + constants.ks); Logger.notice("Kv: " + constants.kv); Logger.notice("Ka: " + constants.ka); - - double moi = DriveCharacterization.calculateAngularInertia(linearAccData, angularAccData, - Units.inches_to_meters(Constants.kDriveWheelRadiusInches), Constants.kRobotLinearInertia); - Logger.notice("J: " + moi); } } diff --git a/src/main/java/com/spartronics4915/frc2019/auto/modes/DiagnoseDropoutsMode.java b/src/main/java/com/spartronics4915/frc2019/auto/modes/DiagnoseDropoutsMode.java new file mode 100644 index 0000000..23119f1 --- /dev/null +++ b/src/main/java/com/spartronics4915/frc2019/auto/modes/DiagnoseDropoutsMode.java @@ -0,0 +1,16 @@ +package com.spartronics4915.frc2019.auto.modes; + +import com.spartronics4915.frc2019.auto.AutoModeBase; +import com.spartronics4915.frc2019.auto.AutoModeEndedException; +import com.spartronics4915.frc2019.auto.actions.DiagnoseDropouts; + +public class DiagnoseDropoutsMode extends AutoModeBase +{ + + @Override + protected void routine() throws AutoModeEndedException + { + runAction(new DiagnoseDropouts()); + } + +} \ No newline at end of file diff --git a/src/main/java/com/spartronics4915/frc2019/auto/modes/PathTestMode.java b/src/main/java/com/spartronics4915/frc2019/auto/modes/PathTestMode.java index 36fc6d4..59ec98c 100755 --- a/src/main/java/com/spartronics4915/frc2019/auto/modes/PathTestMode.java +++ b/src/main/java/com/spartronics4915/frc2019/auto/modes/PathTestMode.java @@ -24,8 +24,8 @@ protected void routine() throws AutoModeEndedException Drive.getInstance().startLogging(); Trajectory> t = mCurved ? - TrajectoryGenerator.getInstance().getTrajectorySet().curvedTest.get(true) : - TrajectoryGenerator.getInstance().getTrajectorySet().straightTest.get(true); + TrajectoryGenerator.getInstance().getTrajectorySet().curvedTest.get(false) : + TrajectoryGenerator.getInstance().getTrajectorySet().straightTest.get(false); runAction(new DriveTrajectory(t, true)); Drive.getInstance().stopLogging(); diff --git a/src/main/java/com/spartronics4915/frc2019/paths/TrajectoryGenerator.java b/src/main/java/com/spartronics4915/frc2019/paths/TrajectoryGenerator.java index f782983..1646e2b 100755 --- a/src/main/java/com/spartronics4915/frc2019/paths/TrajectoryGenerator.java +++ b/src/main/java/com/spartronics4915/frc2019/paths/TrajectoryGenerator.java @@ -126,7 +126,7 @@ private Trajectory> getStraightTest() { List waypoints = new ArrayList<>(); waypoints.add(new Pose2d(0d, 0d, Rotation2d.identity())); - waypoints.add(new Pose2d(60d, 0d, Rotation2d.identity())); + waypoints.add(new Pose2d(120d, 0d, Rotation2d.identity())); return generateTrajectory(false, waypoints, Arrays.asList(new CentripetalAccelerationConstraint(kMaxCentripetalAccel)), kMaxVelocity, kMaxAccel, kMaxVoltage); @@ -137,7 +137,7 @@ private Trajectory> getCurvedTest() List waypoints = new ArrayList<>(); waypoints.add(new Pose2d(0d, 0d, Rotation2d.identity())); waypoints.add(new Pose2d(78d, 78d, Rotation2d.fromDegrees(90))); - return generateTrajectory(true, waypoints, + return generateTrajectory(false, waypoints, Arrays.asList(new CentripetalAccelerationConstraint(kMaxCentripetalAccel)), kMaxVelocity, kMaxAccel, kMaxVoltage); } diff --git a/src/main/java/com/spartronics4915/frc2019/planners/DriveMotionPlanner.java b/src/main/java/com/spartronics4915/frc2019/planners/DriveMotionPlanner.java index c40f8a9..936a123 100755 --- a/src/main/java/com/spartronics4915/frc2019/planners/DriveMotionPlanner.java +++ b/src/main/java/com/spartronics4915/frc2019/planners/DriveMotionPlanner.java @@ -38,7 +38,7 @@ public enum FollowerType NONLINEAR_FEEDBACK } - FollowerType mFollowerType = FollowerType.FEEDFORWARD_ONLY; + FollowerType mFollowerType = FollowerType.NONLINEAR_FEEDBACK; public void setFollowerType(FollowerType type) { @@ -306,8 +306,8 @@ protected Output updatePurePursuit(DifferentialDrive.DriveDynamics dynamics, Pos protected Output updateNonlinearFeedback(DifferentialDrive.DriveDynamics dynamics, Pose2d current_state) { // Implements eqn. 5.12 from https://www.dis.uniroma1.it/~labrob/pub/papers/Ramsete01.pdf - final double kBeta = 2.0; // >0. - final double kZeta = 0.7; // Damping coefficient, [0, 1]. + final double kBeta = 4; // >0. + final double kZeta = 0.4; // Damping coefficient, [0, 1]. // Compute gain parameter. final double k = 2.0 * kZeta * Math.sqrt(kBeta * dynamics.chassis_velocity.linear * dynamics.chassis_velocity.linear diff --git a/src/main/java/com/spartronics4915/frc2019/subsystems/CargoIntake.java b/src/main/java/com/spartronics4915/frc2019/subsystems/CargoIntake.java index 545210b..8c0398d 100644 --- a/src/main/java/com/spartronics4915/frc2019/subsystems/CargoIntake.java +++ b/src/main/java/com/spartronics4915/frc2019/subsystems/CargoIntake.java @@ -59,7 +59,7 @@ private enum SystemState private CargoIntake() { - boolean success = true;//IR sensor anolog port 7 to detect cargo going into chute + boolean success = true; // IR sensor anolog port 7 to detect cargo going into chute try { if (!CANProbe.getInstance().validatePCMId(Constants.kCargoHatchArmPCMId)) throw new RuntimeException("CargoIntake PCM isn't on the CAN bus!"); diff --git a/src/main/java/com/spartronics4915/frc2019/subsystems/Drive.java b/src/main/java/com/spartronics4915/frc2019/subsystems/Drive.java index c953473..eb5b595 100755 --- a/src/main/java/com/spartronics4915/frc2019/subsystems/Drive.java +++ b/src/main/java/com/spartronics4915/frc2019/subsystems/Drive.java @@ -20,6 +20,8 @@ import com.spartronics4915.lib.trajectory.timing.TimedState; import com.spartronics4915.lib.util.DriveSignal; import com.spartronics4915.lib.util.ReflectingCSVWriter; +import com.spartronics4915.lib.util.Units; + import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.drive.DifferentialDrive; @@ -47,8 +49,7 @@ public class Drive extends Subsystem private Rotation2d mGyroOffset = Rotation2d.identity(); private boolean mOverrideTrajectory = false; private double[] mYawPitchRoll = new double[3]; - - + private double mTargetHeading = 0; // Degrees, for closed-loop turning private final ILoop mLoop = new ILoop() { @@ -195,7 +196,7 @@ private static double radiansPerSecondToTicksPer100ms(double rad_s) private static double ticksPer100msToInchesPerSecond(double t) { - return (t / Constants.kDriveEncoderPPR) * 10 * (Constants.kDriveWheelDiameterInches * Math.PI); + return (t / Constants.kDriveEncoderPPR) * 10 * (Constants.kDriveWheelDiameterInches * Math.PI); } @Override @@ -210,11 +211,15 @@ public synchronized void setWantTurn(Rotation2d heading) { logDebug("Switching to robot relative turn: " + heading); mDriveControlState = DriveControlState.TURN; + updateTalonsForPosition(); } + mTargetHeading = heading.getDegrees(); + WheelState w = mMotionPlanner.getModel().solveInverseKinematics(new ChassisState(0.0, heading.getRadians())); - mPeriodicIO.left_demand = w.left; - mPeriodicIO.right_demand = w.right; + mPeriodicIO.left_demand = (getLeftEncoderRotations() + inchesToRotations(Units.meters_to_inches(w.left / 10))) * Constants.kDriveEncoderPPR; + mPeriodicIO.right_demand = (getRightEncoderRotations() + inchesToRotations(Units.meters_to_inches(w.right / 10))) * Constants.kDriveEncoderPPR; } + /** * Configure talons for open loop control */ @@ -260,15 +265,16 @@ private synchronized void setPathVelocity(DriveSignal signal, DriveSignal feedfo * Configure talons for velocity control without paths * * @param inchesPerSecVelocity Desired velocity in inches per second - * @param feedforwardVoltage Voltage (0-12) to apply arbitrarily to velocity PID output + * @param feedforwardVoltage Voltage (0-12) to apply arbitrarily to velocity + * PID output */ public synchronized void setVelocity(DriveSignal inchesPerSecVelocity, DriveSignal feedforwardVoltage) { if (mDriveControlState != DriveControlState.VELOCITY) { logDebug("Switching to closed loop velocity. Target: " + - inchesPerSecVelocity.toString() + - ", Arbitrary feedforward: " + feedforwardVoltage.toString()); + inchesPerSecVelocity.toString() + + ", Arbitrary feedforward: " + feedforwardVoltage.toString()); updateTalonsForVelocity(); mDriveControlState = DriveControlState.VELOCITY; } @@ -286,6 +292,13 @@ private void updateTalonsForVelocity() mRightMaster.selectProfileSlot(Constants.kVelocityPIDSlot, 0); } + private void updateTalonsForPosition() + { + setBrakeMode(true); + mLeftMaster.selectProfileSlot(Constants.kPositionPIDSlot, 0); + mRightMaster.selectProfileSlot(Constants.kPositionPIDSlot, 0); + } + public synchronized void setTrajectory(TrajectoryIterator> trajectory) { if (mMotionPlanner != null) @@ -307,6 +320,14 @@ public boolean isDoneWithTrajectory() return mMotionPlanner.isDone() || mOverrideTrajectory; } + public boolean isDoneTurning() + { + return mDriveControlState == DriveControlState.TURN && + Math.abs(mTargetHeading - mPeriodicIO.gyro_heading.getDegrees()) < Constants.kTurnDegreeTolerance && + Math.abs(getLeftLinearVelocity()) < Constants.kTurnVelTolerance && + Math.abs(getRightLinearVelocity()) < Constants.kTurnVelTolerance; + } + public boolean isBrakeMode() { return mIsBrakeMode; @@ -470,7 +491,7 @@ private void updatePathFollower() final double now = Timer.getFPGATimestamp(); DriveMotionPlanner.Output output = - mMotionPlanner.update(now, RobotStateEstimator.getInstance().getEncoderRobotStateMap().getFieldToVehicle(now)); + mMotionPlanner.update(now, RobotStateEstimator.getInstance().getEncoderRobotStateMap().getFieldToVehicle(now)); // DriveSignal signal = new DriveSignal(demand.left_feedforward_voltage / 12.0, demand.right_feedforward_voltage / 12.0); @@ -483,7 +504,7 @@ private void updatePathFollower() new DriveSignal(radiansPerSecondToTicksPer100ms(output.left_velocity), radiansPerSecondToTicksPer100ms(output.right_velocity)), new DriveSignal(output.left_feedforward_voltage / 12.0, - output.right_feedforward_voltage / 12.0)); + output.right_feedforward_voltage / 12.0)); // if accel is rads per sec^2, why divide by 1000 (and not 100?) mPeriodicIO.left_accel = radiansPerSecondToTicksPer100ms(output.left_accel) / 1000.0; @@ -525,6 +546,12 @@ public synchronized void reloadGains(TalonSRX talon) talon.config_kD(Constants.kVelocityPIDSlot, Constants.kDriveVelocityKd, Constants.kLongCANTimeoutMs); talon.config_kF(Constants.kVelocityPIDSlot, Constants.kDriveVelocityKf, Constants.kLongCANTimeoutMs); talon.config_IntegralZone(Constants.kVelocityPIDSlot, Constants.kDriveVelocityIZone, Constants.kLongCANTimeoutMs); + + talon.config_kP(Constants.kPositionPIDSlot, Constants.kDrivePositionKp, Constants.kLongCANTimeoutMs); + talon.config_kI(Constants.kPositionPIDSlot, Constants.kDrivePositionKi, Constants.kLongCANTimeoutMs); + talon.config_kD(Constants.kPositionPIDSlot, Constants.kDrivePositionKd, Constants.kLongCANTimeoutMs); + talon.config_kF(Constants.kPositionPIDSlot, Constants.kDrivePositionKf, Constants.kLongCANTimeoutMs); + talon.config_IntegralZone(Constants.kPositionPIDSlot, Constants.kDrivePositionIZone, Constants.kLongCANTimeoutMs); } @Override @@ -572,21 +599,19 @@ public synchronized void readPeriodicInputs() mCSVWriter.add(mPeriodicIO); } - // System.out.println("control state: " + mDriveControlState + ", left: " + mPeriodicIO.left_demand + ", right: " + mPeriodicIO.right_demand); } @Override public synchronized void writePeriodicOutputs() { - /* NB: this is where the rubber hits the road. ie: here are direct - controls over the talon to power the drive train. The rest of - this file orchestrates the periodic updateing of our inputs: - - mDriveControlState - mPeriodicIO.left_demand, right_demand (pct or vel or pos) - mPeriodicIO.left_accel, right_accel (used with *Kd* in velocity mode) - + /* + * NB: this is where the rubber hits the road. ie: here are direct + * controls over the talon to power the drive train. The rest of + * this file orchestrates the periodic updateing of our inputs: + * mDriveControlState + * mPeriodicIO.left_demand, right_demand (pct or vel or pos) + * mPeriodicIO.left_accel, right_accel (used with *Kd* in velocity mode) */ if (mDriveControlState == DriveControlState.OPEN_LOOP) { @@ -594,7 +619,7 @@ mPeriodicIO.left_accel, right_accel (used with *Kd* in velocity mode) mRightMaster.set(ControlMode.PercentOutput, mPeriodicIO.right_demand, DemandType.ArbitraryFeedForward, 0.0); } - else if (mDriveControlState == DriveControlState.TURN) + else if (mDriveControlState == DriveControlState.TURN) { mLeftMaster.set(ControlMode.Position, mPeriodicIO.left_demand); mRightMaster.set(ControlMode.Position, mPeriodicIO.right_demand); @@ -614,13 +639,13 @@ else if (mDriveControlState == DriveControlState.TURN) mLeftMaster.set(ControlMode.Velocity, mPeriodicIO.left_demand, DemandType.ArbitraryFeedForward, - mPeriodicIO.left_feedforward + - Constants.kDriveVelocityKd*mPeriodicIO.left_accel/1023.0); + mPeriodicIO.left_feedforward + + Constants.kDriveVelocityKd * mPeriodicIO.left_accel / 1023.0); mRightMaster.set(ControlMode.Velocity, mPeriodicIO.right_demand, DemandType.ArbitraryFeedForward, - mPeriodicIO.right_feedforward + - Constants.kDriveVelocityKd*mPeriodicIO.right_accel/1023.0); + mPeriodicIO.right_feedforward + + Constants.kDriveVelocityKd * mPeriodicIO.right_accel / 1023.0); // // Following can be used to debug the feedback term without the // Talon-side PID. @@ -635,16 +660,16 @@ else if (mDriveControlState == DriveControlState.TURN) @Override public boolean checkSystem(String variant) { - if(!isInitialized()) + if (!isInitialized()) { logWarning("can't check uninitialized system"); return false; } boolean success = true; - DriveSignal zero = new DriveSignal(0,0); + DriveSignal zero = new DriveSignal(0, 0); DriveSignal feedfwd = zero; logNotice("checkSystem " + variant + " ---------------"); - if(variant.equals("velocityControl")) + if (variant.equals("velocityControl")) { Timer timer = new Timer(); logNotice(" enter velocity control mode"); @@ -747,43 +772,43 @@ public boolean checkSystem(String variant) else { boolean leftSide = TalonSRXChecker.CheckTalons(this, - new ArrayList() - { - + new ArrayList() { - add(new TalonSRXChecker.TalonSRXConfig("left_master", mLeftMaster)); - add(new TalonSRXChecker.TalonSRXConfig("left_slave", mLeftSlave)); - } - }, new TalonSRXChecker.CheckerConfig() - { + { + add(new TalonSRXChecker.TalonSRXConfig("left_master", mLeftMaster)); + add(new TalonSRXChecker.TalonSRXConfig("left_slave", mLeftSlave)); + } + }, new TalonSRXChecker.CheckerConfig() { - mCurrentFloor = 2; - mRPMFloor = 1500; - mCurrentEpsilon = 2.0; - mRPMEpsilon = 250; - mRPMSupplier = () -> mLeftMaster.getSelectedSensorVelocity(0); - } - }); - boolean rightSide = TalonSRXChecker.CheckTalons(this, - new ArrayList() - { + { + mCurrentFloor = 2; + mRPMFloor = 1500; + mCurrentEpsilon = 2.0; + mRPMEpsilon = 250; + mRPMSupplier = () -> mLeftMaster.getSelectedSensorVelocity(0); + } + }); + boolean rightSide = TalonSRXChecker.CheckTalons(this, + new ArrayList() { - add(new TalonSRXChecker.TalonSRXConfig("right_master", mRightMaster)); - add(new TalonSRXChecker.TalonSRXConfig("right_slave", mRightSlave)); - } - }, new TalonSRXChecker.CheckerConfig() - { + { + add(new TalonSRXChecker.TalonSRXConfig("right_master", mRightMaster)); + add(new TalonSRXChecker.TalonSRXConfig("right_slave", mRightSlave)); + } + }, new TalonSRXChecker.CheckerConfig() { - mCurrentFloor = 2; - mRPMFloor = 1500; - mCurrentEpsilon = 2.0; - mRPMEpsilon = 250; - mRPMSupplier = () -> mRightMaster.getSelectedSensorVelocity(0); - } - }); + + { + mCurrentFloor = 2; + mRPMFloor = 1500; + mCurrentEpsilon = 2.0; + mRPMEpsilon = 250; + mRPMSupplier = () -> mRightMaster.getSelectedSensorVelocity(0); + } + }); success = leftSide && rightSide; } return success; @@ -840,4 +865,4 @@ public static class PeriodicIO public double right_feedforward; public TimedState path_setpoint = new TimedState(Pose2dWithCurvature.identity()); } -} \ No newline at end of file +} diff --git a/src/main/java/com/spartronics4915/frc2019/subsystems/RobotStateEstimator.java b/src/main/java/com/spartronics4915/frc2019/subsystems/RobotStateEstimator.java index 23aa297..97d89c8 100755 --- a/src/main/java/com/spartronics4915/frc2019/subsystems/RobotStateEstimator.java +++ b/src/main/java/com/spartronics4915/frc2019/subsystems/RobotStateEstimator.java @@ -55,6 +55,8 @@ public static RobotStateEstimator getInstance() mLidarRobotState, vehicleToLidar, () -> Timer.getFPGATimestamp()); + + logInitialized(true); } public RobotStateMap getEncoderRobotStateMap() diff --git a/src/main/java/com/spartronics4915/frc2019/subsystems/Superstructure.java b/src/main/java/com/spartronics4915/frc2019/subsystems/Superstructure.java index c18fca3..2463376 100755 --- a/src/main/java/com/spartronics4915/frc2019/subsystems/Superstructure.java +++ b/src/main/java/com/spartronics4915/frc2019/subsystems/Superstructure.java @@ -3,10 +3,12 @@ import com.spartronics4915.lib.util.ILooper; import com.spartronics4915.lib.util.Logger; import com.spartronics4915.lib.util.RobotStateMap; +import com.spartronics4915.lib.util.Units; import java.util.ArrayList; import java.util.Arrays; +import com.spartronics4915.frc2019.VisionUpdateManager; import com.spartronics4915.frc2019.paths.TrajectoryGenerator; import com.spartronics4915.frc2019.planners.DriveMotionPlanner; import com.spartronics4915.lib.geometry.Pose2d; @@ -66,7 +68,7 @@ public enum WantedState ALIGN_AND_INTAKE_PANEL, ALIGN_AND_SHOOT_CARGO_ROCKET, ALIGN_AND_SHOOT_CARGO_BAY, - ALIGN_AND_SHOOT_PANEL, + ALIGN_AND_EJECT_PANEL, // Climb CLIMB, // Panel ejecting (no auto align) @@ -120,6 +122,8 @@ public enum SystemState private static final DriveSignal kPlatformDriveSpeed = new DriveSignal(1, 1); + private static final Pose2d kBackOutOffset = new Pose2d(36, 0, Rotation2d.identity()); + // These constants are _just_ for dynamic paths. See TrajectoryGenerator for constants for premade paths // XXX: Currently unused private static final double kMaxPathVelocity = 240.0; // inches/s @@ -149,9 +153,9 @@ public void onStart(double timestamp) synchronized (Superstructure.this) { mWantedState = WantedState.DRIVER_CONTROL; + mSystemState = SystemState.DRIVER_CONTROLLING; mStateChangedTimer.reset(); mStateChangedTimer.start(); - mSystemState = SystemState.DRIVER_CONTROLLING; mStateChanged = true; } } @@ -204,18 +208,10 @@ public void onLoop(double timestamp) mCargoIntake.setWantedState(CargoIntake.WantedState.INTAKE); mCargoChute.setWantedState(CargoChute.WantedState.BRING_BALL_TO_TOP); if (mStateChanged) - { - ArrayList waypoints = new ArrayList<>(); - waypoints.add(mRobotStateMap.getFieldToVehicle(Timer.getFPGATimestamp())); - waypoints.add(Pose2d.identity()); - - double startTime = Timer.getFPGATimestamp(); - TrajectoryIterator> t = - new TrajectoryIterator<>((new TimedView<>((mTrajectoryGenerator.generateTrajectory(false, waypoints))))); - // TODO: Maybe plug in our current velocity as the start veloicty of the path? - Logger.info("Path generated; took " + (Timer.getFPGATimestamp() - startTime) + " seconds."); - mDrive.setTrajectory(t); - } + makeAndDrivePath(Pose2d.identity(), false); + // TODO: Target selection/vision integration + // VisionUpdateManager.forwardVisionManager.getLatestVisionUpdate() + // .ifPresent(v -> makeAndDrivePath(v.getFieldPosition(mRobotStateMap)), false) if (mWantedState == WantedState.ALIGN_AND_INTAKE_CARGO && (mDrive.isDoneWithTrajectory() || mCargoChute.atTarget())) { @@ -226,27 +222,29 @@ public void onLoop(double timestamp) } break; case ALIGNING_CLOSEST_REVERSE_TARGET: - // TODO: Put in paths mCargoIntake.setWantedState(CargoIntake.WantedState.HOLD); + if (mStateChanged) + makeAndDrivePath(Pose2d.identity(), true); + // TODO: Target selection/vision integration + // VisionUpdateManager.reverseVisionManager.getLatestVisionUpdate() + // .ifPresent(v -> makeAndDrivePath(v.getFieldPosition(mRobotStateMap)), true); + if (mDrive.isDoneWithTrajectory() && newState == mSystemState) { - if (mWantedState == WantedState.ALIGN_AND_SHOOT_PANEL) + if (mWantedState == WantedState.ALIGN_AND_EJECT_PANEL) newState = SystemState.MOVING_CHUTE_TO_EJECT_PANEL; else if (mWantedState == WantedState.ALIGN_AND_INTAKE_PANEL) newState = SystemState.INTAKING_PANEL; - else if (mWantedState == WantedState.ALIGN_AND_SHOOT_CARGO_BAY || mWantedState == WantedState.ALIGN_AND_SHOOT_CARGO_ROCKET) + else if (mWantedState == WantedState.ALIGN_AND_SHOOT_CARGO_BAY + || mWantedState == WantedState.ALIGN_AND_SHOOT_CARGO_ROCKET) newState = SystemState.EJECTING_CARGO; } break; case INTAKING_PANEL: mCargoChute.setWantedState(CargoChute.WantedState.LOWER); - if (mWantedState == WantedState.ALIGN_AND_INTAKE_PANEL - && mStateChangedTimer.hasPeriodPassed(kPanelHandlingDuration)) - { - mWantedState = WantedState.DRIVER_CONTROL; - newState = SystemState.DRIVER_CONTROLLING; - } + if (newState == mSystemState && mStateChangedTimer.hasPeriodPassed(kPanelHandlingDuration)) + newState = SystemState.BACKING_OUT_FROM_LOADING; break; case MOVING_CHUTE_TO_EJECT_PANEL: mCargoChute.setWantedState(CargoChute.WantedState.LOWER); @@ -259,9 +257,12 @@ else if (mWantedState == WantedState.ALIGN_AND_SHOOT_CARGO_BAY || mWantedState = if (mCargoChute.atTarget()) mPanelHandler.setWantedState(PanelHandler.WantedState.EJECT); - if (newState == mSystemState && mStateChangedTimer.hasPeriodPassed(kPanelHandlingDuration) + if (mWantedState == WantedState.ALIGN_AND_EJECT_PANEL && mStateChangedTimer.hasPeriodPassed(kPanelHandlingDuration) && mCargoChute.atTarget() && mPanelHandler.atTarget()) - newState = SystemState.BACKING_OUT_FROM_LOADING; + { + mWantedState = WantedState.DRIVER_CONTROL; + newState = SystemState.DRIVER_CONTROLLING; + } break; case EJECTING_CARGO: if (mWantedState == WantedState.ALIGN_AND_SHOOT_CARGO_BAY) @@ -278,13 +279,17 @@ else if (mWantedState == WantedState.ALIGN_AND_SHOOT_CARGO_ROCKET) } break; case BACKING_OUT_FROM_LOADING: - // TODO: Put in paths - if (mWantedState == WantedState.ALIGN_AND_SHOOT_PANEL && mDrive.isDoneWithTrajectory() && newState == mSystemState) + if (mStateChanged) + makeAndDrivePath(mRobotStateMap.getFieldToVehicle(Timer.getFPGATimestamp()).transformBy(kBackOutOffset), false); + + if (newState == mSystemState && mDrive.isDoneWithTrajectory()) newState = SystemState.TURNING_AROUND; break; case TURNING_AROUND: - // TODO: Implement drive quick turn - if (mWantedState == WantedState.ALIGN_AND_SHOOT_PANEL) + if (mStateChanged) + mDrive.setWantTurn(mDrive.getHeading().rotateBy(Rotation2d.fromDegrees(180))); + + if (mWantedState == WantedState.ALIGN_AND_INTAKE_PANEL && mDrive.isDoneTurning()) { mWantedState = WantedState.DRIVER_CONTROL; newState = SystemState.DRIVER_CONTROLLING; @@ -315,6 +320,21 @@ public void onStop(double timestamp) } }; + private void makeAndDrivePath(Pose2d goalPose, boolean reversed) + { + ArrayList waypoints = new ArrayList<>(); + waypoints.add(mRobotStateMap.getFieldToVehicle(Timer.getFPGATimestamp())); + waypoints.add(goalPose); + + double startTime = Timer.getFPGATimestamp(); + TrajectoryIterator> t = + new TrajectoryIterator<>((new TimedView<>((mTrajectoryGenerator.generateTrajectory(reversed, waypoints))))); + // TODO: Maybe plug in our current velocity as the start veloicty of the path? + Logger.info("Path generated; took " + (Timer.getFPGATimestamp() - startTime) + " seconds."); + + mDrive.setTrajectory(t); + } + private SystemState defaultStateTransfer() { SystemState newState = mSystemState; @@ -332,7 +352,9 @@ private SystemState defaultStateTransfer() case ALIGN_AND_INTAKE_PANEL: // Assumes PANEL intake is on the reverse side if (mSystemState == SystemState.ALIGNING_CLOSEST_REVERSE_TARGET || - mSystemState == SystemState.INTAKING_PANEL) + mSystemState == SystemState.INTAKING_PANEL || + mSystemState == SystemState.BACKING_OUT_FROM_LOADING || + mSystemState == SystemState.TURNING_AROUND) break; newState = SystemState.ALIGNING_CLOSEST_REVERSE_TARGET; break; @@ -346,11 +368,9 @@ private SystemState defaultStateTransfer() break; newState = SystemState.ALIGNING_CLOSEST_REVERSE_TARGET; break; - case ALIGN_AND_SHOOT_PANEL: + case ALIGN_AND_EJECT_PANEL: if (mSystemState == SystemState.ALIGNING_CLOSEST_REVERSE_TARGET || - mSystemState == SystemState.EJECTING_PANEL || - mSystemState == SystemState.BACKING_OUT_FROM_LOADING || - mSystemState == SystemState.TURNING_AROUND) + mSystemState == SystemState.EJECTING_PANEL) break; newState = SystemState.ALIGNING_CLOSEST_REVERSE_TARGET; break; @@ -406,6 +426,8 @@ public synchronized boolean isDriverControlled() public void stop() { // Subsystem manager stops these, we don't + mWantedState = WantedState.DRIVER_CONTROL; + mSystemState = SystemState.DRIVER_CONTROLLING; } @Override @@ -433,4 +455,4 @@ public void outputTelemetry() dashboardPutState(mSystemState.toString()); dashboardPutWantedState(mWantedState.toString()); } -} \ No newline at end of file +} diff --git a/src/main/java/com/spartronics4915/lib/physics/DriveCharacterization.java b/src/main/java/com/spartronics4915/lib/physics/DriveCharacterization.java index 6d55e26..1fc8b95 100755 --- a/src/main/java/com/spartronics4915/lib/physics/DriveCharacterization.java +++ b/src/main/java/com/spartronics4915/lib/physics/DriveCharacterization.java @@ -4,7 +4,6 @@ import com.spartronics4915.lib.util.PolynomialRegression; import com.spartronics4915.lib.util.Util; -import java.util.ArrayList; import java.util.List; public class DriveCharacterization @@ -71,9 +70,9 @@ public static CharacterizationConstants characterizeDrive(List linearAccelerationData, List angularAccelerationData, double wheelRadius, double robotMass) { + // We currently throw out samples if things don't match up... FIXME? + int smallestLength = Math.min(angularAccelerationData.size(), linearAccelerationData.size()); Logger.debug("Linear accel data has " + linearAccelerationData.size() + " samples, angular has " + angularAccelerationData.size()); - double[] x = new double[angularAccelerationData.size()]; - double[] y = new double[linearAccelerationData.size()]; + + double[] x = new double[smallestLength]; + double[] y = new double[smallestLength]; for (int i = 0; i < x.length; i++) x[i] = angularAccelerationData.get(i).acceleration;