From 9f8975316882e44165280104676e2f2b71fc4937 Mon Sep 17 00:00:00 2001
From: "Team 3128: Controls"
 <35188310+ControlsNarwhal@users.noreply.github.com>
Date: Fri, 4 Oct 2024 18:04:09 -0700
Subject: [PATCH] incorp changes from other branches

---
 .../java/frc/team3128/subsystems/Amper.java   | 43 +++++++++----------
 .../java/frc/team3128/subsystems/Intake.java  | 22 +++++++---
 2 files changed, 37 insertions(+), 28 deletions(-)

diff --git a/src/main/java/frc/team3128/subsystems/Amper.java b/src/main/java/frc/team3128/subsystems/Amper.java
index 15a2e58..e843902 100644
--- 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);
     }
 
 }
\ No newline at end of file
diff --git a/src/main/java/frc/team3128/subsystems/Intake.java b/src/main/java/frc/team3128/subsystems/Intake.java
index 5a48278..23a3a4a 100644
--- 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));
     }
 
-}
+}
\ No newline at end of file