Merge pull request #48 from chopshop-166/BetterIntake

better intake that actually works
chopshop-166 · Feb 24, 2025 · ce48124 · ce48124
2 parents 80902f9 + 111bbcb
commit ce48124
Show file tree

Hide file tree

Showing 4 changed files with 49 additions and 29 deletions.
diff --git a/src/main/java/frc/robot/CommandSequences.java b/src/main/java/frc/robot/CommandSequences.java
@@ -42,11 +42,11 @@ public Command intake() {
         return armRotate.moveIntaking().andThen(led.elevatorToPreset(),
                 elevator.moveTo(ElevatorPresets.INTAKE), led.elevatorAtPreset(),
                 armRotate.moveTo(ArmRotatePresets.INTAKE),
-                led.intaking(), coralManip.intake(), led.gamePieceAcquired());
+                led.intaking(), coralManip.betterintake(), led.gamePieceAcquired());
     }
 
     public Command intakeBottom() {
-        return armRotate.moveTo(ArmRotatePresets.INTAKE).alongWith(coralManip.intake());
+        return armRotate.moveTo(ArmRotatePresets.INTAKE).alongWith(coralManip.betterintake());
     }
 
     // Moves elevator to set coral preset

diff --git a/src/main/java/frc/robot/maps/ColdStart.java b/src/main/java/frc/robot/maps/ColdStart.java
@@ -14,6 +14,8 @@
 import com.chopshop166.chopshoplib.motors.SmartMotorControllerGroup;
 import com.chopshop166.chopshoplib.sensors.CSEncoder;
 import com.chopshop166.chopshoplib.sensors.IEncoder;
+import com.chopshop166.chopshoplib.motors.validators.EncoderValidator;
+import com.chopshop166.chopshoplib.sensors.CtreEncoder;
 import com.chopshop166.chopshoplib.sensors.gyro.PigeonGyro2;
 import com.chopshop166.chopshoplib.states.PIDValues;
 import com.pathplanner.lib.config.ModuleConfig;
@@ -38,6 +40,8 @@
 import edu.wpi.first.wpilibj.PowerDistribution;
 import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
 import frc.robot.maps.subsystems.ArmRotateMap;
+import edu.wpi.first.wpilibj.PowerDistribution;
+import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
 import frc.robot.maps.subsystems.CoralManipMap;
 import frc.robot.maps.subsystems.DeepClimbMap;
 import frc.robot.maps.subsystems.ElevatorMap;
@@ -203,15 +207,6 @@ public DeepClimbMap getDeepClimbMap() {
 
     }
 
-    // @Override
-    // public LedMapBase getLedMap() {
-    // var result = new WPILedMap(1, 1);
-    // var leds = result.ledBuffer;
-
-    // SegmentConfig underglow = leds.segment(1).tags();
-    // return result;
-    // }
-
     @Override
     public void setupLogging() {
         // Logger.addDataReceiver(new WPILOGWriter("/media/sda1/")); // Log to a USB
@@ -220,4 +215,5 @@ public void setupLogging() {
         Logger.recordMetadata("RobotMap", this.getClass().getSimpleName());
         new PowerDistribution(1, ModuleType.kRev); // Enables power distribution logging
     }
+
 }
diff --git a/src/main/java/frc/robot/maps/subsystems/CoralManipMap.java b/src/main/java/frc/robot/maps/subsystems/CoralManipMap.java
@@ -22,14 +22,13 @@ public CoralManipMap(SmartMotorController motor, BooleanSupplier sensor) {
 
     @Override
     public void updateData(Data data) {
-        data.leftMotor.updateData(motor);
+        data.motor.updateData(motor);
         data.gamePieceDetected = sensor.getAsBoolean();
 
     }
 
     public static class Data extends DataWrapper {
-        public MotorControllerData leftMotor = new MotorControllerData();
-        public MotorControllerData rightMotor = new MotorControllerData();
+        public MotorControllerData motor = new MotorControllerData();
         public boolean gamePieceDetected;
     }
 

diff --git a/src/main/java/frc/robot/subsystems/CoralManip.java b/src/main/java/frc/robot/subsystems/CoralManip.java
@@ -2,8 +2,11 @@
 
 import static edu.wpi.first.wpilibj2.command.Commands.waitSeconds;
 
+import org.littletonrobotics.junction.Logger;
+
 import com.chopshop166.chopshoplib.logging.LoggedSubsystem;
 
+import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.maps.subsystems.CoralManipMap;
 import frc.robot.maps.subsystems.CoralManipMap.Data;
@@ -14,53 +17,75 @@ public class CoralManip extends LoggedSubsystem<Data, CoralManipMap> {
     private final double RELEASE_SPEEDLEFT = -0.1;
     private final double INTAKE_SPEED = -0.3;
     private final double RELEASE_DELAY = 1;
-    // private final double SLOW_SPEED = .
-    private final double DELAY = 0.5;
-    private final double HOLD_SPEED = -0.05;
+    private final double ALIGNMENT_DISTANCE = 0.0;
+    private final double ALIGNMENT_SPEED = 0.1;
 
     public CoralManip(CoralManipMap coralManipMap) {
         super(new Data(), coralManipMap);
     }
 
     public Command scoreL1() {
         return run(() -> {
-            getData().leftMotor.setpoint = RELEASE_SPEEDLEFT;
-            getData().rightMotor.setpoint = RELEASE_SPEEDRIGHT;
+            getData().motor.setpoint = RELEASE_SPEEDLEFT;
+
         }).until(() -> !getData().gamePieceDetected).andThen(waitSeconds(RELEASE_DELAY), safeStateCmd());
     }
 
     public Command scoreL4() {
         return run(() -> {
-            getData().leftMotor.setpoint = RELEASE_SPEEDLEFT;
-            getData().rightMotor.setpoint = RELEASE_SPEEDLEFT;
+            getData().motor.setpoint = RELEASE_SPEEDLEFT;
+
         }).until(() -> !getData().gamePieceDetected).andThen(waitSeconds(RELEASE_DELAY), safeStateCmd());
     }
 
     public Command score() {
         return run(() -> {
-            getData().leftMotor.setpoint = RELEASE_SPEEDRIGHT;
-            getData().rightMotor.setpoint = RELEASE_SPEEDRIGHT;
+            getData().motor.setpoint = RELEASE_SPEEDRIGHT;
+
         }).until(() -> !getData().gamePieceDetected).andThen(waitSeconds(RELEASE_DELAY), safeStateCmd());
     }
 
     public Command feed() {
         return runSafe(() -> {
-            getData().leftMotor.setpoint = RELEASE_SPEEDLEFT;
-            getData().rightMotor.setpoint = RELEASE_SPEEDLEFT;
+            getData().motor.setpoint = RELEASE_SPEEDLEFT;
+
         });
     }
 
     public Command intake() {
         return runSafe(() -> {
-            getData().leftMotor.setpoint = INTAKE_SPEED;
-            getData().rightMotor.setpoint = INTAKE_SPEED;
+            getData().motor.setpoint = INTAKE_SPEED;
+
         }).until(() -> getData().gamePieceDetected);
     }
 
+    public Command betterintake() {
+        return run(() -> {
+            getData().motor.setpoint = INTAKE_SPEED;
+        }).until(() -> getData().gamePieceDetected).andThen(
+                startSafe(() -> {
+                    getData().motor.setpoint = ALIGNMENT_SPEED;
+                }).until(() -> !getData().gamePieceDetected),
+                startSafe(() -> {
+                    getData().motor.setpoint = ALIGNMENT_SPEED;
+                }).until(() -> getData().gamePieceDetected));
+
+    }
+
     @Override
     public void safeState() {
-        getData().leftMotor.setpoint = 0;
-        getData().rightMotor.setpoint = 0;
+        getData().motor.setpoint = 0;
     }
 
+    @Override
+    public void reset() {
+        getMap().motor.getEncoder().reset();
+    }
+
+    @Override
+    public void periodic() {
+        super.periodic();
+        Logger.recordOutput("Intake encoder distance", getMap().motor.getEncoder().getDistance());
+        Logger.recordOutput("Game piece detected", getData().gamePieceDetected);
+    }
 }