From 195a51fe5033132dcf66ee09591094c65e265a01 Mon Sep 17 00:00:00 2001 From: lucasszela Date: Fri, 7 Feb 2025 18:58:30 -0500 Subject: [PATCH 1/5] Made command to position coral in 2nd iterations intake --- .../java/frc/robot/subsystems/CoralManip.java | 24 ++++++++++++++++--- 1 file changed, 21 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/CoralManip.java b/src/main/java/frc/robot/subsystems/CoralManip.java index c053004..d90a447 100644 --- a/src/main/java/frc/robot/subsystems/CoralManip.java +++ b/src/main/java/frc/robot/subsystems/CoralManip.java @@ -4,6 +4,7 @@ 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,9 +15,8 @@ public class CoralManip extends LoggedSubsystem { 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); @@ -57,6 +57,24 @@ public Command intake() { }).until(() -> getData().gamePieceDetected); } + public Command betterintake() { + return run(() -> { + getData().leftMotor.setpoint = INTAKE_SPEED; + getData().rightMotor.setpoint = INTAKE_SPEED; + }).until(() -> getData().gamePieceDetected).andThen(() -> { + getMap().leftMotor.getEncoder().reset(); + getMap().rightMotor.getEncoder().reset(); + safeState(); + }, + startSafe(() -> { + getData().leftMotor.setpoint = ALIGNMENT_SPEED; + getData().rightMotor.setpoint = ALIGNMENT_SPEED; + }).until(() -> { + return getMap().leftMotor.getEncoder().getDistance() < ALIGNMENT_DISTANCE; + return getMap().rightMotor.getEncoder().getDistance() < ALIGNMENT_DISTANCE; + })); + } + @Override public void safeState() { getData().leftMotor.setpoint = 0; From ec0be3ce37b82d5b8e9feb9d5ea235c21beb80e9 Mon Sep 17 00:00:00 2001 From: Matt Soucy Date: Sat, 8 Feb 2025 10:57:20 -0500 Subject: [PATCH 2/5] fix: Make the BetterIntake build --- src/main/java/frc/robot/subsystems/CoralManip.java | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/CoralManip.java b/src/main/java/frc/robot/subsystems/CoralManip.java index d90a447..6a7474c 100644 --- a/src/main/java/frc/robot/subsystems/CoralManip.java +++ b/src/main/java/frc/robot/subsystems/CoralManip.java @@ -61,17 +61,13 @@ public Command betterintake() { return run(() -> { getData().leftMotor.setpoint = INTAKE_SPEED; getData().rightMotor.setpoint = INTAKE_SPEED; - }).until(() -> getData().gamePieceDetected).andThen(() -> { - getMap().leftMotor.getEncoder().reset(); - getMap().rightMotor.getEncoder().reset(); - safeState(); - }, - startSafe(() -> { + }).until(() -> getData().gamePieceDetected).andThen( + resetCmd(), safeStateCmd(), startSafe(() -> { getData().leftMotor.setpoint = ALIGNMENT_SPEED; getData().rightMotor.setpoint = ALIGNMENT_SPEED; }).until(() -> { - return getMap().leftMotor.getEncoder().getDistance() < ALIGNMENT_DISTANCE; - return getMap().rightMotor.getEncoder().getDistance() < ALIGNMENT_DISTANCE; + return getMap().leftMotor.getEncoder().getDistance() < ALIGNMENT_DISTANCE + && getMap().rightMotor.getEncoder().getDistance() < ALIGNMENT_DISTANCE; })); } From 22a9aa36c2b92c97365c5cbad924480c109f0657 Mon Sep 17 00:00:00 2001 From: Matt Soucy Date: Sat, 8 Feb 2025 13:30:19 -0500 Subject: [PATCH 3/5] fix: Logging data --- src/main/java/frc/robot/maps/ColdStart.java | 14 +++++--------- 1 file changed, 5 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/maps/ColdStart.java b/src/main/java/frc/robot/maps/ColdStart.java index 6c5bffd..b73d54b 100644 --- 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 } + } From 2c347727397d0f62dfb7aef2234e669f579269ba Mon Sep 17 00:00:00 2001 From: Liam Murray Date: Sat, 22 Feb 2025 11:39:21 -0500 Subject: [PATCH 4/5] removed extra motor and changed speed --- src/main/java/frc/robot/CommandSequences.java | 4 +- .../robot/maps/subsystems/CoralManipMap.java | 5 +- .../java/frc/robot/subsystems/CoralManip.java | 48 +++++++++++-------- 3 files changed, 33 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/robot/CommandSequences.java b/src/main/java/frc/robot/CommandSequences.java index c2b6d7e..d86f35e 100644 --- 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 index dd43977..8a32c8e 100644 --- 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 index 6a7474c..6e71724 100644 --- 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,7 +18,7 @@ public class CoralManip extends LoggedSubsystem { 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); @@ -24,57 +26,65 @@ public CoralManip(CoralManipMap 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); + } } From 111bbcb134162f9aa54519ebad7d50404340f9a1 Mon Sep 17 00:00:00 2001 From: Liam Murray Date: Sat, 22 Feb 2025 12:01:33 -0500 Subject: [PATCH 5/5] better intake that actually works --- src/main/java/frc/robot/subsystems/CoralManip.java | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/CoralManip.java b/src/main/java/frc/robot/subsystems/CoralManip.java index 6e71724..3e35d6a 100644 --- a/src/main/java/frc/robot/subsystems/CoralManip.java +++ b/src/main/java/frc/robot/subsystems/CoralManip.java @@ -63,12 +63,13 @@ public Command betterintake() { return run(() -> { getData().motor.setpoint = INTAKE_SPEED; }).until(() -> getData().gamePieceDetected).andThen( - resetCmd(), safeStateCmd(), startSafe(() -> { + startSafe(() -> { getData().motor.setpoint = ALIGNMENT_SPEED; - }).until(() -> { - return getMap().motor.getEncoder().getDistance() > ALIGNMENT_DISTANCE; + }).until(() -> !getData().gamePieceDetected), + startSafe(() -> { + getData().motor.setpoint = ALIGNMENT_SPEED; + }).until(() -> getData().gamePieceDetected)); - })); } @Override