incorp changes from other branches

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

Hide file tree

Showing 2 changed files with 37 additions and 28 deletions.
diff --git a/src/main/java/frc/team3128/subsystems/Amper.java b/src/main/java/frc/team3128/subsystems/Amper.java
@@ -16,7 +16,8 @@
 public class Amper extends ElevatorTemplate {
 
     public enum Setpoint {
-        EXTENDED(20),
+        FULLEXTEND(21.25),
+        PARTEXTEND(21.25*0.8),
         RETRACTED(0);
 
         private double setpoint;
@@ -52,10 +53,10 @@ protected void configMotors() {
         ELEV_MOTOR.setInverted(true);
 
         ELEV_MOTOR.setNeutralMode(Neutral.COAST);
-        // ROLLER_MOTOR.setNeutralMode(Neutral.COAST);
+        ROLLER_MOTOR.setNeutralMode(Neutral.COAST);
 
-        // ELEV_MOTOR.setStatusFrames(SparkMaxConfig.POSITION);
-        // ROLLER_MOTOR.setDefaultStatusFrames();
+        ELEV_MOTOR.setStatusFrames(SparkMaxConfig.POSITION);
+        ROLLER_MOTOR.setDefaultStatusFrames();
     }
 
     public void setVoltage(double volts) {
@@ -79,30 +80,28 @@ public void initShuffleboard(){
         NAR_Shuffleboard.addData(getName(), "Velocity", ()-> getVelocity(), 1,0);
     }
 
-    // public Command runRollers() {
-    //     return runRollers(ROLLER_POWER);
-    // }
+    public Command runRollers() {
+        return runRollers(ROLLER_POWER);
+    }
+
+    public Command stopRollers(){
+        return runRollers(0);
+    }
 
-    // public Command stopRollers(){
-    //     return runRollers(0);
-    // }
+    public Command runRollers(double power) {
+        return runOnce(() -> ROLLER_MOTOR.set(power));
+    }  
 
-    // public Command runRollers(double power) {
-    //     return runOnce(() -> ROLLER_MOTOR.set(power));
-    // }  
+    public Command partExtend() {
+        return moveElevator(Setpoint.PARTEXTEND.setpoint);
+    }
 
-    public Command extend() {
-        return sequence(
-             moveElevator(Setpoint.EXTENDED.setpoint)
-            // runRollers()
-        );
+    public Command fullExtend(){
+        return moveElevator(Setpoint.FULLEXTEND.setpoint);
     }
 
     public Command retract() {
-        return sequence(
-            moveElevator(Setpoint.RETRACTED.setpoint)
-            // stopRollers()
-        );
+        return moveElevator(Setpoint.RETRACTED.setpoint);
     }
 
 }
diff --git a/src/main/java/frc/team3128/subsystems/Intake.java b/src/main/java/frc/team3128/subsystems/Intake.java
@@ -8,6 +8,8 @@
 import common.hardware.motorcontroller.NAR_Motor.Neutral;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
 import static edu.wpi.first.wpilibj2.command.Commands.*;
 
 public class Intake extends PivotTemplate{
@@ -40,6 +42,9 @@ private Intake() {
         setConstraints(MIN_SETPOINT, MAX_SETPOINT);
         setSafetyThresh(5);
         initShuffleboard();
+
+        this.setSafetyThresh(1000);
+
     }
 
     @Override
@@ -49,11 +54,11 @@ protected void configMotors() {
         PIVOT_MOTOR.setNeutralMode(Neutral.COAST);
         PIVOT_MOTOR.setStatusFrames(SparkMaxConfig.POSITION);
 
-        ROLLER_MOTOR.setInverted(false);
-        ROLLER_MOTOR.enableVoltageCompensation(VOLT_COMP);
-        ROLLER_MOTOR.setNeutralMode(Neutral.COAST);
-        ROLLER_MOTOR.setCurrentLimit(CURRENT_LIMIT);
-        ROLLER_MOTOR.setStatusFrames(SparkMaxConfig.VELOCITY);
+        // ROLLER_MOTOR.setInverted(false);
+        // ROLLER_MOTOR.enableVoltageCompensation(VOLT_COMP);
+        // ROLLER_MOTOR.setNeutralMode(Neutral.COAST);
+        // ROLLER_MOTOR.setCurrentLimit(CURRENT_LIMIT);
+        // ROLLER_MOTOR.setStatusFrames(SparkMaxConfig.VELOCITY);
     }
 
     public Command retract() {
@@ -67,6 +72,11 @@ public Command retract() {
         );
     }
 
+    @Override
+    public double getMeasurement() {
+        return -1 * motors[0].getPosition();
+    }
+
     public Command pivotTo (Setpoint setpoint) {
         return pivotTo(setpoint.angle);
     }
@@ -87,4 +97,4 @@ public Command runRollers(double power) {
         return runOnce(()-> ROLLER_MOTOR.set(power));
     }
 
-}
+}