Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

better intake that actually works #48

Merged
merged 5 commits into from
Feb 24, 2025
Merged

better intake that actually works #48

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
195a51f
Made command to position coral in 2nd iterations intake
Lucasszela Feb 7, 2025
ec0be3c
fix: Make the BetterIntake build
msoucy Feb 8, 2025
22a9aa3
fix: Logging data
msoucy Feb 8, 2025
2c34772
removed extra motor and changed speed
Liammurray19 Feb 22, 2025
111bbcb
better intake that actually works
Liammurray19 Feb 22, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/CommandSequences.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
14 changes: 5 additions & 9 deletions src/main/java/frc/robot/maps/ColdStart.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;
Expand Down Expand Up @@ -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
Expand All @@ -220,4 +215,5 @@ public void setupLogging() {
Logger.recordMetadata("RobotMap", this.getClass().getSimpleName());
new PowerDistribution(1, ModuleType.kRev); // Enables power distribution logging
}

}
5 changes: 2 additions & 3 deletions src/main/java/frc/robot/maps/subsystems/CoralManipMap.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

Expand Down
55 changes: 40 additions & 15 deletions src/main/java/frc/robot/subsystems/CoralManip.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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);
}
}