diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java index 386b493..a6a40cb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java @@ -51,32 +51,6 @@ import java.util.List; @Config public final class TankDrive { - // drive model parameters - public static double IN_PER_TICK = 0; - public static double TRACK_WIDTH_TICKS = 0; - - // feedforward parameters in tick units - public static double kS = 0; - public static double kV = 0; - public static double kA = 0; - - // path profile parameters - public static double MAX_WHEEL_VEL = 50; - public static double MIN_PROFILE_ACCEL = -30; - public static double MAX_PROFILE_ACCEL = 50; - - // turn profile parameters - public static double MAX_ANG_VEL = Math.PI; // shared with path - public static double MAX_ANG_ACCEL = Math.PI; - - // path controller gains - public static double RAMSETE_ZETA = 0.7; // in the range (0, 1) - public static double RAMSETE_BBAR = 2.0; // positive - - // turn controller gains - public static double TURN_GAIN = 0.0; - public static double TURN_VEL_GAIN = 0.0; - public static class Params { // drive model parameters public double inPerTick = 0; @@ -107,19 +81,19 @@ public final class TankDrive { public static Params PARAMS = new Params(); - public final TankKinematics kinematics = new TankKinematics(IN_PER_TICK * TRACK_WIDTH_TICKS); + public final TankKinematics kinematics = new TankKinematics(PARAMS.inPerTick * PARAMS.trackWidthTicks); - public final MotorFeedforward feedforward = new MotorFeedforward(kS, kV / IN_PER_TICK, kA / IN_PER_TICK); + public final MotorFeedforward feedforward = new MotorFeedforward(PARAMS.kS, PARAMS.kV / PARAMS.inPerTick, PARAMS.kA / PARAMS.inPerTick); public final TurnConstraints defaultTurnConstraints = new TurnConstraints( - MAX_ANG_VEL, -MAX_ANG_ACCEL, MAX_ANG_ACCEL); + PARAMS.maxAngVel, -PARAMS.maxAngVel, PARAMS.maxAngAccel); public final VelConstraint defaultVelConstraint = new MinVelConstraint(Arrays.asList( - kinematics.new WheelVelConstraint(MAX_WHEEL_VEL), - new AngularVelConstraint(MAX_ANG_VEL) + kinematics.new WheelVelConstraint(PARAMS.maxWheelVel), + new AngularVelConstraint(PARAMS.maxAngVel) )); public final AccelConstraint defaultAccelConstraint = - new ProfileAccelConstraint(MIN_PROFILE_ACCEL, MAX_PROFILE_ACCEL); + new ProfileAccelConstraint(PARAMS.minProfileAccel, PARAMS.maxProfileAccel); public final List leftMotors, rightMotors; @@ -130,8 +104,6 @@ public final class TankDrive { public final Localizer localizer; public Pose2d pose; - public final double inPerTick = IN_PER_TICK; - private final LinkedList poseHistory = new LinkedList<>(); public class DriveLocalizer implements Localizer { @@ -187,11 +159,11 @@ public final class TankDrive { new DualNum