Skip to content

Commit

Permalink
logic changes and pivot constraint under the stage
Browse files Browse the repository at this point in the history
  • Loading branch information
@eshen7
eshen7 committed Feb 13, 2024
1 parent 891b0c4 commit d23a4b3
Show file tree
Hide file tree
Showing 8 changed files with 77 additions and 31 deletions.
27 changes: 13 additions & 14 deletions src/main/java/team3647/frc2024/auto/AutoCommands.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -91,14 +91,22 @@ public Command four_S1N1N2N3(Alliance color) {
}

public Command masterSuperstructureSequence() {
return Commands.parallel(
Commands.sequence(scorePreload(), continuouslyIntakeForShoot(swerve::getOdoPose)),
superstructure.spinUp(),
superstructure.prep());
return Commands.sequence(
scorePreload(),
Commands.parallel(
continuouslyIntakeForShoot(swerve::getOdoPose),
superstructure.spinUp(),
superstructure.autoFeed(
() ->
swerve.getOdoPose().getX()
< AutoConstants.kDrivetrainXShootingThreshold),
superstructure.prep()));
}

public Command scorePreload() {
return Commands.parallel(
superstructure.spinUp(),
superstructure.prep(),
Commands.sequence(Commands.waitSeconds(0.5), superstructure.feed()))
.withTimeout(0.8);
}
Expand Down Expand Up @@ -136,16 +144,7 @@ public Command shoot() {
public Command continuouslyIntakeForShoot(Supplier<Pose2d> drivePose) {
return Commands.sequence(
superstructure.intake().until(() -> superstructure.getPiece()),
superstructure.passToShooter(),
Commands.deadline(
Commands.sequence(
Commands.waitUntil(
() ->
drivePose.get().getX()
< AutoConstants
.kDrivetrainXShootingThreshold),
Commands.waitSeconds(0.5),
superstructure.feed().withTimeout(0.5))))
superstructure.passToShooterNoKicker())
.repeatedly();
}

Expand Down
4 changes: 2 additions & 2 deletions src/main/java/team3647/frc2024/commands/KickerCommands.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,11 +13,11 @@ public Command kick() {
}

public Command slowFeed() {
return Commands.run(() -> kicker.openLoop(0.1), kicker);
return Commands.run(() -> kicker.openLoop(0.2), kicker);
}

public Command unkick() {
return Commands.run(() -> kicker.openLoop(-0.1), kicker);
return Commands.run(() -> kicker.openLoop(-0.2), kicker);
}

public Command oscillate() {
Expand Down
1 change: 1 addition & 0 deletions src/main/java/team3647/frc2024/constants/PivotConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,7 @@ public class PivotConstants {

public static final double kMinDegree = 15;
public static final double kMaxDegree = 61.2;
public static final double kMaxDegreeUnderStage = 30;

private static final double masterKP = 0.3;
private static final double masterKI = 0;
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/team3647/frc2024/constants/TunerConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,7 +51,7 @@ public class TunerConstants {
public static final boolean kInvertLeftSide = false;
public static final boolean kInvertRightSide = true;

public static final String kCANbusName = "";
public static final String kCANbusName = "drive";
public static final int kPigeonId = 16;

// These are only used for simulation
Expand Down
7 changes: 3 additions & 4 deletions src/main/java/team3647/frc2024/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,6 @@
import team3647.frc2024.constants.GlobalConstants;
import team3647.frc2024.constants.IntakeConstants;
import team3647.frc2024.constants.KickerConstants;
import team3647.frc2024.constants.LEDConstants;
import team3647.frc2024.constants.PivotConstants;
import team3647.frc2024.constants.ShooterConstants;
import team3647.frc2024.constants.SwerveDriveConstants;
Expand All @@ -24,7 +23,6 @@
import team3647.frc2024.constants.WristConstants;
import team3647.frc2024.subsystems.Intake;
import team3647.frc2024.subsystems.Kicker;
import team3647.frc2024.subsystems.LEDs;
import team3647.frc2024.subsystems.Pivot;
import team3647.frc2024.subsystems.ShooterLeft;
import team3647.frc2024.subsystems.ShooterRight;
Expand Down Expand Up @@ -173,7 +171,6 @@ public Command getAutonomousCommand() {
public final SwerveDrive swerve =
new SwerveDrive(
TunerConstants.DrivetrainConstants,
SwerveDriveConstants.kDriveKinematics,
SwerveDriveConstants.kDrivePossibleMaxSpeedMPS,
SwerveDriveConstants.kRotPossibleMaxSpeedRadPerSec,
GlobalConstants.kDt,
Expand Down Expand Up @@ -226,6 +223,8 @@ public Command getAutonomousCommand() {
PivotConstants.kNativePosToDegrees,
PivotConstants.kMinDegree,
PivotConstants.kMaxDegree,
PivotConstants.kMaxDegreeUnderStage,
swerve::getOdoPose,
PivotConstants.nominalVoltage,
PivotConstants.maxKG,
0.02,
Expand Down Expand Up @@ -258,7 +257,7 @@ public Command getAutonomousCommand() {
new VisionController(
swerve::addVisionData, swerve::shouldAddData, backLeft, backRight, left, right);

private final LEDs LEDs = new LEDs(LEDConstants.m_candle);
// private final LEDs LEDs = new LEDs(LEDConstants.m_candle);

public final NeuralDetector detector = new NeuralDetectorPhotonVision(VisionConstants.driver);

Expand Down
25 changes: 24 additions & 1 deletion src/main/java/team3647/frc2024/subsystems/Pivot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,17 +3,23 @@
import com.ctre.phoenix6.hardware.TalonFX;
import com.playingwithfusion.TimeOfFlight;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.util.Units;
import java.util.function.Supplier;
import org.littletonrobotics.junction.Logger;
import team3647.lib.TalonFXSubsystem;

public class Pivot extends TalonFXSubsystem {

private final double minAngle;
private final double maxAngle;
private double maxAngle;
private final double maxAngleNormal;
private final double maxAngleUnderStage;

private final Supplier<Pose2d> drivePose;

private final double maxKG;

Expand All @@ -29,6 +35,8 @@ public Pivot(
double ticksToMeters,
double minAngle,
double maxAngle,
double maxAngleUnderStage,
Supplier<Pose2d> drivePose,
double nominalVoltage,
double maxKG,
double kDt,
Expand All @@ -38,6 +46,9 @@ public Pivot(
super.addFollower(slave, false);
this.minAngle = minAngle;
this.maxAngle = maxAngle;
this.maxAngleNormal = maxAngle;
this.maxAngleUnderStage = maxAngleUnderStage;
this.drivePose = drivePose;
this.maxKG = maxKG;
this.tofBack = tofBack;
this.tofFront = tofFront;
Expand All @@ -51,6 +62,18 @@ public void setEncoder(double degree) {
@Override
public void periodic() {
super.periodic();
if (((drivePose.get().getX() > 3.5 && drivePose.get().getX() < 6.2)
|| (drivePose.get().getX() > 10.2 && drivePose.get().getX() < 12.9))
&& ((Math.abs(drivePose.get().getY() - 4)
< ((drivePose.get().getX() - 2.9) * 1 / 1.73)
&& drivePose.get().getX() < 6.2)
|| (Math.abs(drivePose.get().getY() - 4)
< ((13.6 - drivePose.get().getX()) * 1 / 1.73)
&& drivePose.get().getX() > 10.2))) {
this.maxAngle = maxAngleUnderStage;
} else {
this.maxAngle = maxAngleNormal;
}
Logger.recordOutput(
"Pivot/Pose",
new Pose3d(
Expand Down
35 changes: 32 additions & 3 deletions src/main/java/team3647/frc2024/subsystems/Superstructure.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import java.util.function.BooleanSupplier;
import java.util.function.DoubleSupplier;
import team3647.frc2024.commands.IntakeCommands;
import team3647.frc2024.commands.KickerCommands;
Expand Down Expand Up @@ -132,9 +133,20 @@ public Command intake() {

public Command passToShooter() {
return Commands.parallel(
intakeCommands.kill(),
wristCommands.setAngle(() -> inverseKinematics.getWristHandoffAngleByPivot()),
Commands.sequence(Commands.waitSeconds(0.5), shootThrough()));
intakeCommands.kill(),
wristCommands.setAngle(
() -> inverseKinematics.getWristHandoffAngleByPivot()))
.withTimeout(0.5)
.andThen(shootThrough());
}

public Command passToShooterNoKicker() {
return Commands.parallel(
intakeCommands.kill(),
wristCommands.setAngle(
() -> inverseKinematics.getWristHandoffAngleByPivot()))
.withTimeout(0.5)
.andThen(shootThroughNoKicker());
}

public Command shootThrough() {
Expand All @@ -143,6 +155,10 @@ public Command shootThrough() {
.andThen(stowIntakeAndIndex());
}

public Command shootThroughNoKicker() {
return intakeCommands.intake().until(() -> pivot.backPiece()).andThen(stowIntake());
}

public Command stowIntake() {
return Commands.parallel(
wristCommands
Expand All @@ -151,6 +167,19 @@ public Command stowIntake() {
intakeCommands.kill());
}

public Command autoFeed(BooleanSupplier goodToGo) {
return Commands.run(
() -> {
if (goodToGo.getAsBoolean()) {
feed();
} else if (!frontPiece() && !pivot.backPiece()) {
feed();
} else {
index();
}
});
}

public Command index() {
if (pivot.frontPiece()) {
return slightReverse().until(() -> pivot.backPiece());
Expand Down
7 changes: 1 addition & 6 deletions src/main/java/team3647/frc2024/subsystems/SwerveDrive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,6 @@
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.units.Measure;
Expand All @@ -32,8 +31,6 @@

public class SwerveDrive extends SwerveDrivetrain implements PeriodicSubsystem {

private final SwerveDriveKinematics kinematics;

public final Field2d field = new Field2d();

private final double maxSpeedMpS;
Expand Down Expand Up @@ -84,13 +81,11 @@ public static class PeriodicIO {

public SwerveDrive(
SwerveDrivetrainConstants swerveDriveConstants,
SwerveDriveKinematics kinematics,
double maxSpeedMpS,
double maxRotRadPerSec,
double kDt,
SwerveModuleConstants... swerveModuleConstants) {
super(swerveDriveConstants, swerveModuleConstants);
this.kinematics = kinematics;
this.maxSpeedMpS = maxSpeedMpS;
this.maxRotRadPerSec = maxRotRadPerSec;
this.kDt = kDt;
Expand Down Expand Up @@ -257,7 +252,7 @@ public SwerveModulePosition[] getModulePositions() {
}

public ChassisSpeeds getChassisSpeeds() {
return this.kinematics.toChassisSpeeds(getModuleStates());
return this.m_kinematics.toChassisSpeeds(getModuleStates());
}

public double getAccel() {
Expand Down

0 comments on commit d23a4b3

Please sign in to comment.