added climb commands

SciBorgs · Jan 22, 2024 · 2d0f591 · 2d0f591
1 parent 76d80fb
commit 2d0f591
Show file tree

Hide file tree

Showing 8 changed files with 51 additions and 22 deletions.
diff --git 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
@@ -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> 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> 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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;