diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java index ee9dbe4..86bb528 100644 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -25,7 +25,7 @@ public class ElevatorConstants { public static final double kPositionControllerI = 0; public static final double kPositionControllerD = 0; - public static final double kAllowedError = 0.25; + public static final double kAllowedError = 0.5; /* public static final double kVelocityControllerP = 0; @@ -37,8 +37,8 @@ public class ElevatorConstants { public static final double kFeedForwardG = 0.53; // calculated value public static final double kFeedForwardV = 4.78; // calculated value - public static final double kMaxVelocity = 0; - public static final double kMaxAcceleration = 0; + public static final double kMaxVelocity = 120.0; // 120 inches per second (COOKING) calculated max is 184 in/s + public static final double kMaxAcceleration = 500.0; // 400 inches per second^2 (also COOKING) calculated max is 600 in/s^2 public static final double kCoralIntakePosition = 0; public static final double kL1Position = 0; diff --git a/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java b/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java index f6dcd52..b269e2b 100644 --- a/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java +++ b/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java @@ -19,37 +19,35 @@ public class ManipulatorPivotConstants { public static final int kMotorAmpsMax = 40; - public static final double kPivotConversion = 12/60 * 20/60 * 28/12; + public static final double kPivotConversion = 12/60 * 20/60 * 12/28; public static final double kPivotMaxVelocity = 0; public static final double kPositionalP = 0; public static final double kPositionalI = 0; public static final double kPositionalD = 0; - public static final double kPositionalTolerance = Units.degreesToRadians(1); - - public static final double kVelocityP = 0; - public static final double kVelocityI = 0; - public static final double kVelocityD = 0; + public static final double kPositionalTolerance = Units.degreesToRadians(3.0); public static final double kFeedForwardS = 0; public static final double kFeedForwardG = 0.41; // calculated value public static final double kFeedForwardV = 0.68; //calculated value - // TODO Is this reasonable? - public static final double kVelocityTolerance = Units.degreesToRadians(3) / 60; + public static final double kFFGravityOffset = Units.degreesToRadians(-135.0); - public static final double kCoralIntakePosition = 0; - public static final double kL1Position = 0; - public static final double kL2Position = 0; - public static final double kL3Position = 0; + public static final double kMaxAcceleration = Units.degreesToRadians(1000.0); // degrees per second^2 calculated max = 2100 + public static final double kMaxVelocity = Units.degreesToRadians(100.0); // degrees per second calculated max = 168 + + public static final double kCoralIntakePosition = Units.degreesToRadians(175.0); + public static final double kL1Position = Units.degreesToRadians(0.0); + public static final double kL2Position = Units.degreesToRadians(25.0); + public static final double kL3Position = Units.degreesToRadians(60.0); public static final double kL4Position = 0; public static final double kL2AlgaePosition = 0; public static final double kL3AlgaePosition = 0; /**The closest position to the elevator brace without hitting it */ - public static final double kArmSafeStowPosition = 0; + public static final double kArmSafeStowPosition = Units.degreesToRadians(60.0); /**The forward rotation limit of the arm */ - public static final double kRotationLimit = 0; + public static final double kRotationLimit = Units.degreesToRadians(175.0); public static final double kMagnetOffset = 0.0; public static final double kAbsoluteSensorDiscontinuityPoint = 0.0; @@ -84,6 +82,15 @@ public class ManipulatorPivotConstants { .smartCurrentLimit(kMotorAmpsMax) .idleMode(kIdleMode); motorConfig.encoder.positionConversionFactor(kPivotConversion); + motorConfig.closedLoop + .p(kPositionalP) + .i(kPositionalI) + .d(kPositionalD) + .velocityFF(0.0); // keep at zero for position pid + motorConfig.closedLoop.maxMotion + .maxAcceleration(kMaxAcceleration) + .maxVelocity(kMaxVelocity) + .allowedClosedLoopError(kPositionalTolerance); } diff --git a/src/main/java/frc/robot/subsystems/ManipulatorPivot.java b/src/main/java/frc/robot/subsystems/ManipulatorPivot.java index 9757772..dafa938 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorPivot.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorPivot.java @@ -48,14 +48,6 @@ public class ManipulatorPivot extends SubsystemBase { positionController.enableContinuousInput(Units.degreesToRadians(-180), Units.degreesToRadians(179)); positionController.setTolerance(ManipulatorPivotConstants.kPositionalTolerance); - velocityController = new PIDController( - ManipulatorPivotConstants.kVelocityP, - ManipulatorPivotConstants.kVelocityI, - ManipulatorPivotConstants.kVelocityD - ); - - velocityController.setTolerance(ManipulatorPivotConstants.kVelocityTolerance); - canCoder = new CANcoder(ManipulatorPivotConstants.kCANcoderID); canCoder.getConfigurator().apply(ManipulatorPivotConstants.canCoderConfig);