Not Functional; Hanger Update

redshiftrobotics · Mar 1, 2024 · 39b12b3 · 39b12b3
1 parent 06f6fb1
commit 39b12b3
Show file tree

Hide file tree

Showing 3 changed files with 37 additions and 9 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -63,10 +63,13 @@ public static class DriverConstants {
         public static final double[] maxSpeedOptionsRotation = {0.1, 0.75, 1};
     }
 
-    public static class HangerContstants {
+    public static class HangerConstants {
         public static final double HANGER_PID_P = 0;
         public static final double HANGER_PID_I = 0;
         public static final double HANGER_PID_D = 0;
+
+        public static final double HANGER_MOTOR_P = 0.3;
+        public static final double HANGER_MOTOR_N = -0.3;
     }
 
     public static class RobotMovementConstants {

diff --git a/src/main/java/frc/robot/commands/HangerMove.java b/src/main/java/frc/robot/commands/HangerMove.java
@@ -67,6 +67,6 @@ public boolean isFinished() {
      */
     @Override
     public void end(boolean interrupted) {
-        hangerMotor.stop();
+        hangerMotorL.stop();
     }
 }
diff --git a/src/main/java/frc/robot/subsystems/Hanger.java b/src/main/java/frc/robot/subsystems/Hanger.java
@@ -6,24 +6,49 @@
 import com.revrobotics.CANSparkLowLevel.MotorType;
 
 import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants.HangerConstants;
 
 
 public class Hanger extends SubsystemBase {
 
-    private final CANSparkMax hangerMotor;
-    private final CANcoder hangerEncoder;
+    private final CANSparkMax hangerMotorL;
+    private final CANSparkMax hangerMotorR;
+    private final CANcoder hangerEncoderL;
+    private final CANcoder hangerEncoderR;
     private final PIDController hangerPIDController;
-    private final StatusSignal<Double> hangerPosition;
+    private final StatusSignal<Double> hangerPositionL;
+    private final StatusSignal<Double> hangerPositionR;
+    private final DigitalInput magnet;
 
-    public Hanger(int hangerMotorID, int hangerEncoderID, int HANGER_PID_P, int HANGER_PID_I, int HANGER_PID_D) {
+    public Hanger(int hangerMotorLID, int hangerMotorRID, int hangerEncoderLID, int hangerEncoderRID, int HANGER_PID_P, int HANGER_PID_I, int HANGER_PID_D) {
 
-        hangerMotor = new CANSparkMax(hangerMotorID, MotorType.kBrushless);
-        hangerEncoder = new CANcoder(hangerEncoderID);
+        hangerMotorL = new CANSparkMax(hangerMotorLID, MotorType.kBrushless);
+        hangerMotorR = new CANSparkMax(hangerMotorRID, MotorType.kBrushless);
+        hangerEncoderL = new CANcoder(hangerEncoderLID);
+        hangerEncoderR = new CANcoder(hangerEncoderRID);
+        DigitalInput magnet = new DigitalInput(0);
 
         hangerPIDController = new PIDController(HANGER_PID_P, HANGER_PID_I, HANGER_PID_D);
 
-        hangerPosition = hangerEncoder.getAbsolutePosition();
+        hangerPositionL = hangerEncoderL.getAbsolutePosition();
+        hangerPositionR = hangerEncoderR.getAbsolutePosition();
+    }
+
+    public void extendRope() {
+        hangerMotorL.set(HangerConstants.HANGER_MOTOR_N);
+    }
+
+    public void intakeRope() {
+        if (magnet.get()) {
+            hangerMotorL.set(0);
+            hangerMotorR.set(0);
+        }
+        else {
+            hangerMotorL.set(HangerConstants.HANGER_MOTOR_P);
+            hangerMotorR.set(HangerConstants.HANGER_MOTOR_P);
+        }
     }
 
     @Override