Skip to content
Draft
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
62 changes: 48 additions & 14 deletions src/main/java/frc/robot/Constants.java
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@

import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.Preferences;

/**
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
Expand All @@ -17,8 +18,41 @@
*/
public final class Constants {

public static final double MAX_SPEED = Units.feetToMeters(4.5);
public static double MAX_SPEED = Units.feetToMeters(4.5);

private static double prefDouble(String name, double defaultValue) {
String key = name;
Preferences.initDouble(key, defaultValue);
return Preferences.getDouble(key, defaultValue);
}

public static void refresh() {
MAX_SPEED = prefDouble("MAX_SPEED", Units.feetToMeters(4.5));

IntakeConstants.kPercentOutputIntake = prefDouble("Intake/PercentOutput", 0.50);
IntakeConstants.kIntakeAccel = prefDouble("Intake/Accel", 4.0);

IntakeDropConstants.kPercentOutputIntakeDrop = prefDouble("IntakeDrop/PercentOutput", 0.30);

ShootingConstants.kPercentOutputSorting = prefDouble("Shooting/PercentOutputSorting", -0.20);
ShootingConstants.kPercentOutputPassthrough = prefDouble("Shooting/PercentOutputPassthrough", -0.45);
ShootingConstants.kPercentOutputShooter = prefDouble("Shooting/PercentOutputShooter", -0.65);
ShootingConstants.kShooterAccel = prefDouble("Shooting/ShooterAccel", 4.0);

ShootingConstants.kShooterReferenceDistanceMeters =
prefDouble("Shooting/ReferenceDistanceMeters", 2.0);
ShootingConstants.kShooterPercentPerMeter =
prefDouble("Shooting/PercentPerMeter", 0.25);

AutoConstants.kMaxSpeedMetersPerSecond =
prefDouble("Auto/MaxSpeedMetersPerSecond", 3);
AutoConstants.kMaxAccelerationMetersPerSecondSquared =
prefDouble("Auto/MaxAccelerationMetersPerSecondSquared", 3);
AutoConstants.kMaxAngularSpeedRadiansPerSecond =
prefDouble("Auto/MaxAngularSpeedRadiansPerSecond", Math.PI);
AutoConstants.kMaxAngularSpeedRadiansPerSecondSquared =
prefDouble("Auto/MaxAngularSpeedRadiansPerSecondSquared", Math.PI);
}

public static final class DrivebaseConstants {
public static final double WHEEL_LOCK_TIME = 10; // seconds
Expand All @@ -39,15 +73,15 @@ public static final class OperatorConstants {
public static final class IntakeConstants {
// We will have to change these later
public static final int kIntakeSparkMaxPort = 10;
public static final double kPercentOutputIntake = 0.50;
public static double kPercentOutputIntake = 0.50;

public static final double kIntakeAccel = 4.0;
public static double kIntakeAccel = 4.0;
}

public static final class IntakeDropConstants {
// We will have to change these later
public static final int kIntakeDropSparkMaxPort = 13;
public static final double kPercentOutputIntakeDrop = 0.30;
public static double kPercentOutputIntakeDrop = 0.30;

}
public static final class ShootingConstants {
Expand All @@ -56,23 +90,23 @@ public static final class ShootingConstants {
public static final int kPassthroughSparkMaxPort = 11;
public static final int kShooterSparkMaxPort = 9;

public static final double kShooterAccel = 4.0;
public static double kShooterAccel = 4.0;

// We will need to change these later
public static final double kPercentOutputSorting = -0.20;
public static final double kPercentOutputPassthrough = -0.45;
public static final double kPercentOutputShooter = -0.65;
public static double kPercentOutputSorting = -0.20;
public static double kPercentOutputPassthrough = -0.45;
public static double kPercentOutputShooter = -0.65;
// Distance (meters) where shooter uses exactly kPercentOutputShooter.
public static final double kShooterReferenceDistanceMeters = 2.0;
public static double kShooterReferenceDistanceMeters = 2.0;
// Additional shooter output applied per meter away from reference distance.
public static final double kShooterPercentPerMeter = 0.25;
public static double kShooterPercentPerMeter = 0.25;
}

public static final class AutoConstants {
public static final double kMaxSpeedMetersPerSecond = 3;
public static final double kMaxAccelerationMetersPerSecondSquared = 3;
public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;
public static double kMaxSpeedMetersPerSecond = 3;
public static double kMaxAccelerationMetersPerSecondSquared = 3;
public static double kMaxAngularSpeedRadiansPerSecond = Math.PI;
public static double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;

public static final double kPXController = 1;
public static final double kPYController = 1;
Expand Down
4 changes: 4 additions & 0 deletions src/main/java/frc/robot/Robot.java
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@
import frc.robot.subsystems.Vision.Position;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import com.studica.frc.AHRS;
import edu.wpi.first.wpilibj.Preferences;

/**
* The methods in this class are called automatically corresponding to each
Expand Down Expand Up @@ -43,6 +44,8 @@ public static Robot getInstance() {
*/
@Override
public void robotInit() {
Preferences.removeAll();
Constants.refresh();
// Instantiate our RobotContainer. This will perform all our button bindings,
// and put our
// autonomous chooser on the dashboard.
Expand All @@ -69,6 +72,7 @@ public void robotInit() {
*/
@Override
public void robotPeriodic() {
Constants.refresh();
// Runs the Scheduler. This is responsible for polling buttons, adding
// newly-scheduled
// commands, running already-scheduled commands, removing finished or
Expand Down