diff --git a/src/main/java/first/robot/constants/AutoConstants.java b/src/main/java/first/robot/constants/AutoConstants.java new file mode 100644 index 0000000..7faecb1 --- /dev/null +++ b/src/main/java/first/robot/constants/AutoConstants.java @@ -0,0 +1,60 @@ +package first.robot.constants; + +import java.io.IOException; + +import org.json.simple.parser.ParseException; +import org.wpilib.math.trajectory.TrapezoidProfile; + +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.config.RobotConfig; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import com.pathplanner.lib.path.PathConstraints; + +public class AutoConstants { + public static final double kMaxSpeedMetersPerSecond = 4; + public static final double kMaxAccelerationMetersPerSecondSquared = 4; + public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI; + public static final double kMaxAngularAccelerationRadiansPerSecondSquared = Math.PI; + + public static final double kMaxSpeedMetersPerSecondAutoAlign = 2.5; + + public static final double kPXYController = 3.5; + public static final double kPThetaController = 5; + + public static final double kAlignPXYController = 2; + public static final double kAlignPThetaController = 5; + + // Constraint for the motion profiled robot angle controller + public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints( + kMaxAngularSpeedRadiansPerSecond, kMaxAngularAccelerationRadiansPerSecondSquared); + + public static final TrapezoidProfile.Constraints kAlignThetaControllerConstraints = new TrapezoidProfile.Constraints( + kMaxAngularSpeedRadiansPerSecond, kMaxAngularAccelerationRadiansPerSecondSquared); + + // TODO This is a constant being managed like a static rewriteable variable + public static RobotConfig kRobotConfig; + public static boolean kAutoConfigOk; + + public static final PPHolonomicDriveController kPPDriveController = new PPHolonomicDriveController( + new PIDConstants(kPXYController, 0, 0), + new PIDConstants(kPThetaController, 0, 0) + ); + + public static final PathConstraints kOnTheFlyConstraints = new PathConstraints( + kMaxSpeedMetersPerSecond, + kMaxAccelerationMetersPerSecondSquared, + kMaxAngularSpeedRadiansPerSecond, + kMaxAngularAccelerationRadiansPerSecondSquared + ); + + static { + try { + kRobotConfig = RobotConfig.fromGUISettings(); + kAutoConfigOk = true; + } catch (IOException | ParseException e) { + System.err.println("FAILED TO READ ROBOTCONFIG, WAS THE CONFIG SET UP IN PATHPLANNER?"); + e.printStackTrace(); + kAutoConfigOk = false; + } + } +} diff --git a/src/main/java/first/robot/constants/ClimberConstants.java b/src/main/java/first/robot/constants/ClimberConstants.java new file mode 100644 index 0000000..1f39665 --- /dev/null +++ b/src/main/java/first/robot/constants/ClimberConstants.java @@ -0,0 +1,68 @@ +package first.robot.constants; + +import java.util.function.Function; + +import com.revrobotics.spark.FeedbackSensor; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkMaxConfig; + +public class ClimberConstants { + // TODO Real values + public enum ClimberPositions { + kStow(0), + kClimbOffGround(0), + kUp(0); + + private double positionMeters; + + private ClimberPositions(double positionMeters) { + this.positionMeters = positionMeters; + } + + public double getPositionMeters() { + return positionMeters; + } + } + + public static final int kMotorCANID = 0; + + public static final double kConversionFactor = 0; + + public static final double kP = 0; + public static final double kI = 0; + public static final double kD = 0; + public static final double kS = 0; + public static final double kV = 0; + public static final double kA = 0; + + public static final boolean kMotorInverted = false; + + public static final int kCurrentLimit = 40; + + public static final IdleMode kIdleMode = IdleMode.kBrake; + + // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM + + public static final SparkMaxConfig kMotorConfig = new SparkMaxConfig(); + + public static final Function kPositionConversionFunction = (d) -> { + return d * kConversionFactor; + }; + + public static final Function kVelocityConversionFunction = (d) -> { + return kPositionConversionFunction.apply(d) / 60; + }; + + static { + kMotorConfig + .inverted(kMotorInverted) + .smartCurrentLimit(kCurrentLimit) + .idleMode(kIdleMode); + kMotorConfig.closedLoop + .feedbackSensor(FeedbackSensor.kPrimaryEncoder) + .pid(kP, kI, kD) + .outputRange(-1, 1) + .feedForward + .sva(kS, kV, kA); + } +}