more incorp

Team3128 · Oct 5, 2024 · e260e01 · e260e01
1 parent 9f89753
commit e260e01
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 10 deletions.
diff --git a/src/main/java/frc/team3128/Constants.java b/src/main/java/frc/team3128/Constants.java
@@ -457,8 +457,8 @@ public static class AmperConstants {
         public static final int ELEV_MOTOR_ID = 21;
         public static final NAR_CANSpark ELEV_MOTOR = new NAR_CANSpark(ELEV_MOTOR_ID);
 
-        // public static final int ROLLER_MOTOR_ID = 20;
-        // public static final NAR_CANSpark ROLLER_MOTOR = new NAR_CANSpark(ROLLER_MOTOR_ID, ControllerType.CAN_SPARK_FLEX);
+        public static final int ROLLER_MOTOR_ID = 20;
+        public static final NAR_CANSpark ROLLER_MOTOR = new NAR_CANSpark(ROLLER_MOTOR_ID, ControllerType.CAN_SPARK_FLEX);
 
         public static final PIDFFConfig PIDConstants = new PIDFFConfig(0.75, 0, 0, 0.21115, 0.00182, 0.00182, 0.0);
         public static final double MAX_VELOCTIY = 10000000;

diff --git a/src/main/java/frc/team3128/commands/CmdManager.java b/src/main/java/frc/team3128/commands/CmdManager.java
@@ -91,19 +91,22 @@ public static Command ramShootNoStop(boolean once) {
     //     ).andThen(ramShoot(once)).andThen(runOnce(() -> CmdSwerveDrive.disableTurn()));
     // }
 
-    public static Command rampUpAmp() {
+    public static Command ampUp(){
         return sequence(
-            shooter.shoot(AMP_RPM),
-            amper.extend()
+            amper.partExtend(),
+            amper.runRollers(),
+            shooter.runShooter(0.3)
         );
     }
 
-    public static Command amp(boolean once) {
+    public static Command ampFinAndDown(){
         return sequence(
-            rampUpAmp(),
-            waitUntil(() -> shooter.atSetpoint() && amper.atSetpoint()),
-            kick(once),
-            waitSeconds(0.2),
+            amper.fullExtend(),
+            waitUntil(() -> amper.atSetpoint()),
+            shooter.runKickMotor(1),
+            waitSeconds(1),
+            amper.stopRollers(),
+            shooter.stopMotors(),
             amper.retract()
         );
     }