Skip to content

Workingsidedetection (#11) #13

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

Merged
michael-m-2983 merged 2 commits into from
Mar 4, 2025
Merged

Workingsidedetection (#11) #13

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
f58841d
Workingsidedetection (#11)
michael-m-2983 Feb 27, 2025
c0f90c4
Merge branch 'workingsidedetection' into tmp-main
michael-m-2983 Mar 4, 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
18 changes: 9 additions & 9 deletions src/main/java/frc/robot/Constants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -99,11 +99,11 @@ public static ArrayList<Pose2d> getSourcePoses() {
}

public static Pose2d getReefPose() {
Pose2d reef = new Pose2d(Units.inchesToMeters(176.746), Units.inchesToMeters(158.501), new Rotation2d());
if (getAlliance() == Alliance.Red) {
AllianceFlipUtil.flip(reef);
}
return reef;
Pose2d reef = new Pose2d(Units.inchesToMeters(176.746), Units.inchesToMeters(158.501), new Rotation2d());
if(getAlliance() == Alliance.Red) {
AllianceFlipUtil.flip(reef);
}
return reef;
}
}

Expand Down Expand Up @@ -216,7 +216,7 @@ public static class Climber {
public static double GEAR_RATIO = 45.0;

public static double DEPLOY_SOFT_LIMIT = -6.0;
public static double CLIMB_SOFT_LIMIT = -2.8;
public static double CLIMB_SOFT_LIMIT = -3.10;
}

public static class Coral {
Expand Down Expand Up @@ -264,7 +264,7 @@ public static class Roll {
public static final int MOTOR_PORT = 15;
public static final boolean MOTOR_INVERTED = false;
public static final boolean ENCODER_INVERTED = false;
public static final double ENCODER_OFFSET_VOLTS = -1.82;
public static final double ENCODER_OFFSET_VOLTS = -1.85;
public static boolean DBG_DISABLED = false;

public static final ProfiledPIDController PID = new ProfiledPIDController(
Expand All @@ -289,7 +289,7 @@ public static class Pitch {
public static final int MOTOR_PORT = 16;
public static final boolean MOTOR_INVERTED = true;
public static final boolean ENCODER_INVERTED = true;
public static final double ENCODER_OFFSET_VOLTS = -2.046;
public static final double ENCODER_OFFSET_VOLTS = -2.7;
public static boolean DBG_DISABLED = false;

public static final double MAXIMUM_ANGLE = Units.degreesToRadians(115.0);
Expand Down Expand Up @@ -387,7 +387,7 @@ public static final class Follower {
public static final int MOTOR_PORT = 11;
public static final boolean INVERTED = true;
}

public static boolean DBG_DISABLED = false;

public static double MOTOR_REVOLUTIONS_PER_METER = 32.81;
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/RobotContainer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -76,6 +76,7 @@ public class RobotContainer {
public final LimelightAssistance limelightAssistance = new LimelightAssistance(swerveDriveSubsystem);
// private final LimeLightSubsystem limeLightSubsystem = new
// LimeLightSubsystem();


private final UsbCamera intakeCam = CameraServer.startAutomaticCapture();
private final DriveCommand normalDrive = new DriveCommand(swerveDriveSubsystem, driverXbox.getHID());
Expand Down Expand Up @@ -131,7 +132,6 @@ public CoralPresets get() {
// 4. Rotate wrist
private Command getGoToLockedPresetCommand() {
return new InstantCommand(() -> {
// coralSubsystem.autoSetMirror();
SmartDashboard.putString("Going to", currentLockedPresetSupplier.get().toString());
}).andThen(new StowArm(coralSubsystem))
.andThen(new MoveElevator(coralSubsystem, currentLockedPresetSupplier))
Expand Down
109 changes: 51 additions & 58 deletions src/main/java/frc/robot/subsystems/ClimberSubsystem.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,72 +20,65 @@
import frc.robot.Constants.Climber;

public class ClimberSubsystem extends SubsystemBase {
private final SparkMax climberMotor = new SparkMax(Climber.MOTOR_PORT, MotorType.kBrushless);
private SparkMaxConfig climberConfig = new SparkMaxConfig();
private RelativeEncoder climberEncoder;
XboxController operator;
private final SparkMax climberMotor = new SparkMax(Climber.MOTOR_PORT, MotorType.kBrushless);
private SparkMaxConfig climberConfig = new SparkMaxConfig();
private RelativeEncoder climberEncoder;
XboxController operator;

boolean isTested = false;
boolean isTested = false;

public ClimberSubsystem(XboxController operator) {
this.operator = operator;
climberMotor.configure(climberConfig.idleMode(IdleMode.kBrake)
.apply(new SoftLimitConfig().reverseSoftLimit(Constants.Climber.DEPLOY_SOFT_LIMIT)
.reverseSoftLimitEnabled(true).forwardSoftLimit(0.0)
.forwardSoftLimitEnabled(true))
.apply(
new EncoderConfig()
.positionConversionFactor(
1.0 / Constants.Climber.GEAR_RATIO)
.velocityConversionFactor(
1.0 / Constants.Climber.GEAR_RATIO)),
ResetMode.kResetSafeParameters,
PersistMode.kPersistParameters);
public ClimberSubsystem(XboxController operator) {
this.operator = operator;
climberMotor.configure(climberConfig.idleMode(IdleMode.kBrake)
.apply(new SoftLimitConfig().reverseSoftLimit(Constants.Climber.DEPLOY_SOFT_LIMIT)
.reverseSoftLimitEnabled(true).forwardSoftLimit(0.0)
.forwardSoftLimitEnabled(true))
.apply(
new EncoderConfig().positionConversionFactor(1.0 / Constants.Climber.GEAR_RATIO)
.velocityConversionFactor(
1.0 / Constants.Climber.GEAR_RATIO)),
ResetMode.kResetSafeParameters,
PersistMode.kPersistParameters);

climberEncoder = climberMotor.getEncoder();
}

private double output = 0.0;
private boolean deployed = false;
climberEncoder = climberMotor.getEncoder();
}

@Override
public void periodic() {
SmartDashboard.putNumber("Climber Encoder", climberEncoder.getPosition());
if (climberEncoder.getPosition() < Constants.Climber.CLIMB_SOFT_LIMIT &&
!deployed) {
deployed = true;
climberMotor.configure(climberConfig.apply(climberConfig.softLimit.forwardSoftLimit(
Constants.Climber.CLIMB_SOFT_LIMIT)),
ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters);
}
private double output = 0.0;
private boolean deployed = false;

if (DriverStation.isTestEnabled() & !isTested) {
climberMotor.configure(
climberConfig.apply(
climberConfig.softLimit.forwardSoftLimitEnabled(false)
.reverseSoftLimitEnabled(false)),
ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters);
isTested = false;
}

if (!Constants.Climber.DBG_DISABLED)
climberMotor.set(operator.getLeftBumper()
? (MathUtil.applyDeadband(
operator.getLeftY(), 0.05)
* (operator.getLeftY() < 0.0 ? 0.7 : 1.0))
: 0.0);
@Override
public void periodic() {
SmartDashboard.putNumber("Climber Encoder", climberEncoder.getPosition());
if (climberEncoder.getPosition() < Constants.Climber.CLIMB_SOFT_LIMIT &&
!deployed) {
deployed = true;
climberMotor.configure(climberConfig.apply(climberConfig.softLimit.forwardSoftLimit(
Constants.Climber.CLIMB_SOFT_LIMIT)),
ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters);
}

public void resetClimberDeploy() {
climberMotor.configure(climberConfig.apply(climberConfig.softLimit.forwardSoftLimit(0.0)),
ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters);
if (DriverStation.isTestEnabled() & !isTested) {
climberMotor.configure(
climberConfig.apply(
climberConfig.softLimit.forwardSoftLimitEnabled(false).reverseSoftLimitEnabled(false)),
ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters);
isTested = false;
}

public void setOutput(double output) {
this.output = output;
}
if (!Constants.Climber.DBG_DISABLED)
climberMotor.set(operator.getLeftBumper() ? (MathUtil.applyDeadband(operator.getLeftY(), 0.05)) : 0.0);
}

public double getOutput() {
return output;
}
public void resetClimberDeploy() {
climberMotor.configure(climberConfig.apply(climberConfig.softLimit.forwardSoftLimit(0.0)),
ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters);
}

public void setOutput(double output) {
this.output = output;
}

public double getOutput() {
return output;
}
}
1 change: 1 addition & 0 deletions src/main/java/frc/robot/subsystems/coral/CoralArm.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -449,6 +449,7 @@ public double readRollEncoderVelocity() {

public double readPivotEncoderPosition() {
return Robot.isSimulation() ? (simPivotPhysics.getAngleRads() - Math.PI * 0.5)
: Units.rotationsToRadians((pivotEncoder.getPosition().getValueAsDouble()
: Units.rotationsToRadians((pivotEncoder.getAbsolutePosition().getValueAsDouble()
* (Constants.Coral.Pivot.ENCODER_INVERTED ? -1.0 : 1.0)));
}
Expand Down
16 changes: 7 additions & 9 deletions src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -55,11 +55,11 @@ public class CoralSubsystem extends SubsystemBase {

public enum CoralPresets {
LEVEL_1(0.05, Units.radiansToDegrees(0.635), Units.radiansToDegrees(1.0), Units.radiansToDegrees(1.636)),
LEVEL_2(0.247 - 0.085, 16.532, 90, 98.068),
LEVEL_3(0.650 - 0.085, 16.532, 90, 98.068),
LEVEL_4(1.342 - 0.04, 21.0, 90, 110.062),
INTAKE(0.03, 19.5, 90, 34.0),
STOW(0.03, 0.0, 0.0, 0.0),
LEVEL_2(0.247-0.02, 16.532, 90, 98.068),
LEVEL_3(0.650-0.02, 16.532, 90, 98.068),
LEVEL_4(1.342, 20.0, 90, 110.062),
INTAKE(0.05, 18.0, 90, 30.0),
STOW(0.05, 0.0, 0.0, 0.0),

CUSTOM(Double.NaN, Double.NaN, Double.NaN, Double.NaN);

Expand Down Expand Up @@ -278,7 +278,6 @@ public void autoSetMirrorIntake() {
.getDistance(closestSource.getTranslation()) ? MirrorPresets.LEFT : MirrorPresets.RIGHT;

System.out.println("Mirror Side" + mirrorSetting.name());

// this.mirrorSetting = (this.leftUltrasonic.get() < this.rightUltrasonic.get())
// ? MirrorPresets.LEFT
// : MirrorPresets.RIGHT;
Expand All @@ -288,9 +287,8 @@ public void autoSetMirrorScoring() {
Pose2d robotPose = swerveSubsystem.getPose();
Pose2d left = robotPose.transformBy(new Transform2d(0, -0.2, new Rotation2d()));
Pose2d right = robotPose.transformBy(new Transform2d(0, 0.2, new Rotation2d()));

this.mirrorSetting = left.getTranslation().getDistance(FieldConstants.getReefPose().getTranslation()) < right
.getTranslation()

this.mirrorSetting = left.getTranslation().getDistance(FieldConstants.getReefPose().getTranslation()) < right.getTranslation()
.getDistance(FieldConstants.getReefPose().getTranslation()) ? MirrorPresets.LEFT : MirrorPresets.RIGHT;

System.out.println("Mirror Side" + mirrorSetting.name());
Expand Down
6 changes: 2 additions & 4 deletions src/main/java/frc/robot/util/LimelightContainer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -113,18 +113,16 @@ public void estimateMT1Odometry(SwerveDrivePoseEstimator odometry, ChassisSpeeds
doRejectUpdate = true;
}

if(mt1.avgTagDist < Units.feetToMeters(10)){
doRejectUpdate = true;
}
if(mt1.avgTagDist < Units.feetToMeters(10))

if (Math.abs(navx.getRate()) > 720) {
doRejectUpdate = true;
}


if((Math.abs(mt1.pose.getX()-odometry.getEstimatedPosition().getX())>1.5)||(Math.abs(mt1.pose.getY()-odometry.getEstimatedPosition().getY())>1.5)){
doRejectUpdate = true;
}

if (!doRejectUpdate) {

odometry.setVisionMeasurementStdDevs(VecBuilder.fill(3, 3, 9999));
Expand Down