diff --git a/src/main/java/org/sciborgs1155/robot/shooter/Shooter.java b/src/main/java/org/sciborgs1155/robot/shooter/Shooter.java index 18e9e20f..aa02de01 100644 --- a/src/main/java/org/sciborgs1155/robot/shooter/Shooter.java +++ b/src/main/java/org/sciborgs1155/robot/shooter/Shooter.java @@ -9,8 +9,9 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import java.util.function.DoubleSupplier; import org.sciborgs1155.robot.Robot; -import org.sciborgs1155.robot.shooter.ShooterConstants.Flywheel; -import org.sciborgs1155.robot.shooter.ShooterConstants.Pivot; +import org.sciborgs1155.robot.shooter.ShooterConstants.FlywheelConstants; +import org.sciborgs1155.robot.shooter.ShooterConstants.PivotConstants; +import org.sciborgs1155.robot.shooter.ShooterConstants.PivotConstants.ClimbConstants; import org.sciborgs1155.robot.shooter.feeder.FeederIO; import org.sciborgs1155.robot.shooter.feeder.RealFeeder; import org.sciborgs1155.robot.shooter.feeder.SimFeeder; @@ -48,29 +49,51 @@ public Command runFeederInverse(double voltage) { // Make sure this is correct !!! public Command runFlywheel(DoubleSupplier velocity) { - PIDController pid = new PIDController(Flywheel.kP, Flywheel.kI, Flywheel.kD); - SimpleMotorFeedforward ff = new SimpleMotorFeedforward(Flywheel.kS, Flywheel.kV, Flywheel.kA); + PIDController pid = + new PIDController(FlywheelConstants.kP, FlywheelConstants.kI, FlywheelConstants.kD); + SimpleMotorFeedforward ff = + new SimpleMotorFeedforward( + FlywheelConstants.kS, FlywheelConstants.kV, FlywheelConstants.kA); return run(() -> flywheel.setVoltage( pid.calculate(flywheel.getVelocity(), velocity.getAsDouble()) + ff.calculate(velocity.getAsDouble()))) .finallyDo(() -> pid.close()) - .withName("running Flywheel"); + .withName("running FlywheelConstants"); } public Command runPivot(double goalAngle) { ProfiledPIDController pid = new ProfiledPIDController( - Pivot.kP, - Pivot.kI, - Pivot.kD, - new TrapezoidProfile.Constraints(Pivot.MAX_VELOCITY, Pivot.MAX_ACCEL)); - ArmFeedforward ff = new ArmFeedforward(Pivot.kS, Pivot.kG, Pivot.kV); + PivotConstants.kP, + PivotConstants.kI, + PivotConstants.kD, + new TrapezoidProfile.Constraints( + PivotConstants.MAX_VELOCITY, PivotConstants.MAX_ACCEL)); + ArmFeedforward ff = new ArmFeedforward(PivotConstants.kS, PivotConstants.kG, PivotConstants.kV); return run(() -> pivot.setVoltage( pid.calculate(pivot.getPosition(), goalAngle) - + ff.calculate(goalAngle + Pivot.POSITION_OFFSET, pid.getSetpoint().velocity))) + + ff.calculate( + goalAngle + PivotConstants.POSITION_OFFSET, pid.getSetpoint().velocity))) .withName("running Pivot"); } + + public Command climb(double goalAngle) { + ProfiledPIDController pid = + new ProfiledPIDController( + ClimbConstants.kP, + ClimbConstants.kI, + ClimbConstants.kD, + new TrapezoidProfile.Constraints( + ClimbConstants.MAX_VELOCITY, ClimbConstants.MAX_ACCEL)); + ArmFeedforward ff = new ArmFeedforward(ClimbConstants.kS, ClimbConstants.kG, ClimbConstants.kV); + + return run(() -> + pivot.setVoltage( + pid.calculate(pivot.getPosition(), goalAngle) + + ff.calculate(goalAngle, pid.getSetpoint().velocity))) + .withName("climbing. . ."); + } } diff --git a/src/main/java/org/sciborgs1155/robot/shooter/ShooterConstants.java b/src/main/java/org/sciborgs1155/robot/shooter/ShooterConstants.java index 0b94718f..73ad928a 100644 --- a/src/main/java/org/sciborgs1155/robot/shooter/ShooterConstants.java +++ b/src/main/java/org/sciborgs1155/robot/shooter/ShooterConstants.java @@ -5,7 +5,7 @@ import edu.wpi.first.units.*; public class ShooterConstants { - public static class Flywheel { + public static class FlywheelConstants { public static final double GEARING = 1; public static final Measure CURRENT_LIMIT = Amps.of(1); @@ -27,7 +27,7 @@ public static class Flywheel { public static final double kA = 0; } - public static class Feeder { + public static class FeederConstants { public static final Measure CURRENT_LIMIT = Amps.of(1); public static final double GEARING = 1; @@ -36,7 +36,7 @@ public static class Feeder { public static final double kA = 1; } - public static class Pivot { + public static class PivotConstants { public static final double GEARING = 1; public static final double CONVERSION = 0; @@ -64,10 +64,16 @@ public static class Pivot { public static final double kA = 1; public static final double kG = 1; - public static final class Climb { + public static final class ClimbConstants { public static final double kP = 1; public static final double kI = 0; public static final double kD = 0; + + public static final double MAX_VELOCITY = 1; + public static final double MAX_ACCEL = 1; + public static final double kS = 1; + public static final double kG = 1; + public static final double kV = 1; } } } diff --git a/src/main/java/org/sciborgs1155/robot/shooter/feeder/RealFeeder.java b/src/main/java/org/sciborgs1155/robot/shooter/feeder/RealFeeder.java index a4775c95..8cd638f2 100644 --- a/src/main/java/org/sciborgs1155/robot/shooter/feeder/RealFeeder.java +++ b/src/main/java/org/sciborgs1155/robot/shooter/feeder/RealFeeder.java @@ -1,7 +1,7 @@ package org.sciborgs1155.robot.shooter.feeder; import static org.sciborgs1155.robot.Ports.Shooter.Feeder.*; -import static org.sciborgs1155.robot.shooter.ShooterConstants.Feeder.*; +import static org.sciborgs1155.robot.shooter.ShooterConstants.FeederConstants.*; import com.revrobotics.CANSparkFlex; import com.revrobotics.CANSparkLowLevel.MotorType; diff --git a/src/main/java/org/sciborgs1155/robot/shooter/feeder/SimFeeder.java b/src/main/java/org/sciborgs1155/robot/shooter/feeder/SimFeeder.java index e36eefc0..0e0e4efb 100644 --- a/src/main/java/org/sciborgs1155/robot/shooter/feeder/SimFeeder.java +++ b/src/main/java/org/sciborgs1155/robot/shooter/feeder/SimFeeder.java @@ -1,6 +1,6 @@ package org.sciborgs1155.robot.shooter.feeder; -import static org.sciborgs1155.robot.shooter.ShooterConstants.Feeder.*; +import static org.sciborgs1155.robot.shooter.ShooterConstants.FeederConstants.*; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; diff --git a/src/main/java/org/sciborgs1155/robot/shooter/flywheel/RealFlywheel.java b/src/main/java/org/sciborgs1155/robot/shooter/flywheel/RealFlywheel.java index 8c4534d8..9623e103 100644 --- a/src/main/java/org/sciborgs1155/robot/shooter/flywheel/RealFlywheel.java +++ b/src/main/java/org/sciborgs1155/robot/shooter/flywheel/RealFlywheel.java @@ -2,7 +2,7 @@ import static edu.wpi.first.units.Units.*; import static org.sciborgs1155.robot.Ports.Shooter.Flywheel.*; -import static org.sciborgs1155.robot.shooter.ShooterConstants.Flywheel.*; +import static org.sciborgs1155.robot.shooter.ShooterConstants.FlywheelConstants.*; import com.revrobotics.CANSparkFlex; import com.revrobotics.CANSparkLowLevel.MotorType; diff --git a/src/main/java/org/sciborgs1155/robot/shooter/flywheel/SimFlywheel.java b/src/main/java/org/sciborgs1155/robot/shooter/flywheel/SimFlywheel.java index 02382825..c1139a1a 100644 --- a/src/main/java/org/sciborgs1155/robot/shooter/flywheel/SimFlywheel.java +++ b/src/main/java/org/sciborgs1155/robot/shooter/flywheel/SimFlywheel.java @@ -1,7 +1,7 @@ package org.sciborgs1155.robot.shooter.flywheel; -import static org.sciborgs1155.robot.shooter.ShooterConstants.Flywheel.GEARING; -import static org.sciborgs1155.robot.shooter.ShooterConstants.Flywheel.MOI; +import static org.sciborgs1155.robot.shooter.ShooterConstants.FlywheelConstants.GEARING; +import static org.sciborgs1155.robot.shooter.ShooterConstants.FlywheelConstants.MOI; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; diff --git a/src/main/java/org/sciborgs1155/robot/shooter/pivot/RealPivot.java b/src/main/java/org/sciborgs1155/robot/shooter/pivot/RealPivot.java index ebd9d1c2..a16d5477 100644 --- a/src/main/java/org/sciborgs1155/robot/shooter/pivot/RealPivot.java +++ b/src/main/java/org/sciborgs1155/robot/shooter/pivot/RealPivot.java @@ -1,7 +1,7 @@ package org.sciborgs1155.robot.shooter.pivot; import static org.sciborgs1155.robot.Ports.Shooter.Pivot.*; -import static org.sciborgs1155.robot.shooter.ShooterConstants.Pivot.*; +import static org.sciborgs1155.robot.shooter.ShooterConstants.PivotConstants.*; import com.revrobotics.CANSparkFlex; import com.revrobotics.CANSparkLowLevel.MotorType; diff --git a/src/main/java/org/sciborgs1155/robot/shooter/pivot/SimPivot.java b/src/main/java/org/sciborgs1155/robot/shooter/pivot/SimPivot.java index a27ff4e6..ab8f4fd9 100644 --- a/src/main/java/org/sciborgs1155/robot/shooter/pivot/SimPivot.java +++ b/src/main/java/org/sciborgs1155/robot/shooter/pivot/SimPivot.java @@ -1,7 +1,7 @@ package org.sciborgs1155.robot.shooter.pivot; import static edu.wpi.first.units.Units.Radians; -import static org.sciborgs1155.robot.shooter.ShooterConstants.Pivot.*; +import static org.sciborgs1155.robot.shooter.ShooterConstants.PivotConstants.*; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId;