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); + } }