removed extra motor and changed speed

chopshop-166 · Feb 22, 2025 · a7df5cc · a7df5cc
1 parent beaad2e
commit a7df5cc
Show file tree

Hide file tree

Showing 3 changed files with 33 additions and 24 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/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,6 +2,8 @@
 
 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;
@@ -16,65 +18,73 @@ public class CoralManip extends LoggedSubsystem<Data, CoralManipMap> {
     private final double INTAKE_SPEED = -0.3;
     private final double RELEASE_DELAY = 1;
     private final double ALIGNMENT_DISTANCE = 0.0;
-    private final double ALIGNMENT_SPEED = -0.1;
+    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().leftMotor.setpoint = INTAKE_SPEED;
-            getData().rightMotor.setpoint = INTAKE_SPEED;
+            getData().motor.setpoint = INTAKE_SPEED;
         }).until(() -> getData().gamePieceDetected).andThen(
                 resetCmd(), safeStateCmd(), startSafe(() -> {
-                    getData().leftMotor.setpoint = ALIGNMENT_SPEED;
-                    getData().rightMotor.setpoint = ALIGNMENT_SPEED;
+                    getData().motor.setpoint = ALIGNMENT_SPEED;
                 }).until(() -> {
-                    return getMap().leftMotor.getEncoder().getDistance() < ALIGNMENT_DISTANCE
-                            && getMap().rightMotor.getEncoder().getDistance() < ALIGNMENT_DISTANCE;
+                    return getMap().motor.getEncoder().getDistance() > ALIGNMENT_DISTANCE;
+
                 }));
     }
 
     @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);
+    }
 }