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

Jump to bottom

assorted fixes #126

Merged
merged 1 commit into from
Apr 11, 2024
Merged

assorted fixes #126

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
70 changes: 16 additions & 54 deletions src/main/java/frc/robot/RobotContainer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -72,8 +72,7 @@
import static edu.wpi.first.wpilibj2.command.Commands.*;
import static frc.robot.commands.FeederCommands.feedToBeamBreak;
import static frc.robot.commands.FeederCommands.feedToShooter;
import static frc.robot.commands.IntakeCommands.intakeCommand;
import static frc.robot.commands.IntakeCommands.smartIntakeCommand;
import static frc.robot.commands.IntakeCommands.*;
import static frc.robot.commands.RumbleCommands.*;
import static frc.robot.commands.ShooterCommands.*;

Expand All @@ -94,10 +93,10 @@ public class RobotContainer {
private final ControllerRumble driverRumble = new ControllerRumble(0);
private final ControllerRumble operatorRumble = new ControllerRumble(1);

// private final ModeHelper modeHelper = new ModeHelper(this);
// Controller
private final CommandXboxController driverController = new CommandXboxController(0);
// Controller
private final CommandXboxController driverController = new CommandXboxController(0);
private final CommandXboxController operatorController = new CommandXboxController(1);

// Dashboard inputs
private final LoggedDashboardChooser<Command> autoChooser;
private final LoggedDashboardChooser<SmartCommandsMode> smartCommandsMode =
Expand All @@ -106,6 +105,7 @@ public class RobotContainer {
new LoggedDashboardNumber("Flywheel Speed", 1500.0);
private final ReactionObject reactions;
LoggedDashboardNumber angleOffsetInput = new LoggedDashboardNumber("Angle Offset", 0.0);

// Subsystems
public Drive drive;
LoggedDashboardBoolean invertX = new LoggedDashboardBoolean("Invert X Axis", false);
Expand All @@ -130,10 +130,10 @@ public RobotContainer() {
new ModuleIOSparkMax(3){},
new VisionIOReal[]{
new VisionIOReal("ShootSideCamera"),
new VisionIOReal("RightCamera") //fixme: rename camera
new VisionIOReal("RightCamera")
},
new LimelightNoteDetection());
shooter = new Shooter(new ShooterIOTalonFX(), new HoodIOSparkMax()); // new HoodIOSparkMax() {}
shooter = new Shooter(new ShooterIOTalonFX(), new HoodIOSparkMax());
feeder = new Feeder(new FeederIOTalonFX());
intake = new Intake(new IntakeIOSparkMax());
climb = new Climb(new ClimbIOSparkMax());
Expand Down Expand Up @@ -192,17 +192,6 @@ public String getCameraName() {
driverController::getLeftY,
driverController::getLeftX,
driverController::getRightX);
// NamedCommands.registerCommand(
// "AutoShoot",
// parallel(
// ShooterCommands.autoAim(shooter, drive),
// sequence(
// waitUntil(shooter::allAtSetpoint),
// feedToShooter(feeder)
// )
// )
// );


NamedCommands.registerCommand(
"Aim Drivetrain",
Expand Down Expand Up @@ -252,7 +241,7 @@ public String getCameraName() {
none(),
race(
feedToBeamBreak(feeder).withTimeout(5),
IntakeCommands.flushIntakeWithoutTheArmExtendedOutward(intake, feeder)
flushIntakeWithoutTheArmExtendedOutward(intake, feeder)
),
feeder::getBeamBroken
)).withTimeout(3.0)
Expand Down Expand Up @@ -296,12 +285,12 @@ private void configureButtonBindings() {
() -> (-driverController.getLeftX() * (invertY.get()?-1:1)),
() -> (-driverController.getRightX()) * (invertOmega.get()?-1:1)));
intake.setDefaultCommand(IntakeCommands.idleCommand(intake));
feeder.setDefaultCommand(new RunCommand(() -> feeder.runVolts(0.0), feeder));
feeder.setDefaultCommand(FeederCommands.idleFeeder(feeder));
shooter.setDefaultCommand(
either(
ShooterCommands.shooterIdle(shooter),
sequence(
ShooterCommands.shooterIdle(shooter).until(shooter::hoodAtSetpoint),
ShooterCommands.shooterIdle(shooter).until(shooter::hoodAtSetpoint).withTimeout(.5),
ShooterCommands.simpleHoodZero(shooter),
ShooterCommands.shooterIdle(shooter)
).withName("Default Command"),
Expand Down Expand Up @@ -332,11 +321,6 @@ private void configureButtonBindings() {
driverController.leftBumper().whileTrue(command.getCommand());

// ---- INTAKE COMMANDS ----
// operatorController
// .povLeft()
// .whileTrue(
// IntakeCommands.intakeBack(intake)
// );
driverController
.leftTrigger()
.whileTrue(
Expand All @@ -349,7 +333,7 @@ private void configureButtonBindings() {
none(),
race(
feedToBeamBreak(feeder).withTimeout(5),
IntakeCommands.flushIntakeWithoutTheArmExtendedOutward(intake, feeder)
flushIntakeWithoutTheArmExtendedOutward(intake, feeder)
),
feeder::getBeamBroken
)
Expand Down Expand Up @@ -377,10 +361,9 @@ private void configureButtonBindings() {
).andThen(FeederCommands.feedToBeamBreak(feeder))
)
.onFalse(
parallel(
ShooterCommands.humanPlayerIntake(shooter),
FeederCommands.humanPlayerIntake(feeder)
).until(() -> !feeder.getBeamBroken())
FeederCommands.humanPlayerIntake(feeder)
.withTimeout(5.0)
.until(() -> !feeder.getBeamBroken())
);
operatorController
.povUp()
Expand Down Expand Up @@ -427,39 +410,18 @@ private void configureButtonBindings() {
ShooterCommands.simpleHoodZero(shooter)
.withTimeout(4.0)
);
// driverController // fixme move to operator controls
// .povUp()
// .whileTrue(newAmpShoot(shooter)
// .alongWith(feedToShooter(feeder))
// .onlyWhile(feeder::getBeamBroken)
// .andThen(ShooterCommands.ampAngle(shooter)));
driverController
.povUp()
.whileTrue(
sequence(
FeederCommands.feedToShooter(feeder)
.alongWith(ShooterCommands.ampSpin(shooter)).withTimeout(0.2),
FeederCommands.feedToShooter(feeder)
.alongWith(ShooterCommands.ampSpin(shooter)).withTimeout(0.2),
ShooterCommands.ampAng(shooter)
.alongWith(ShooterCommands.ampGo(shooter, 600))
.withTimeout(0.25)
.andThen(ShooterCommands.setAmpAngle(shooter, -0.4))
)


// ShooterCommands.ampSpin(shooter)
// .alongWith(ShooterCommands.ampAng(shooter)
// .alongWith(feedToShooter(feeder))
// .onlyWhile(feeder::getBeamBroken))
);

// NEW OPERATOR CONTROLS
// leftbumper zero
// x justShoot
// y aim
// povup humanplayerintake
// down flush

//zero shooter
break;
case DriveMotors:
drive.setDefaultCommand(
Expand Down
12 changes: 10 additions & 2 deletions src/main/java/frc/robot/subsystems/drive/Drive.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -328,22 +328,29 @@ public void periodic() {

case 1:
PhotonTrackedTarget target = estimatedRobotPose.targetsUsed.get(0);
var mult = target.getPoseAmbiguity() * 25;
var mult = target.getPoseAmbiguity() * 500;
Logger.recordOutput("Vision/" + visionIO[i].getCameraName() + "/Single Tag Matrix Multiplier/Step 0", mult);
mult*=mult;
Logger.recordOutput("Vision/" + visionIO[i].getCameraName() + "/Single Tag Matrix Multiplier/Step 1", mult);
if (
(pose.getX() <= 16.5) &&
(pose.getX() > 0) &&
(pose.getY() <= 8.2) &&
(pose.getY() > 0)
) mult = mult / 4;
Logger.recordOutput("Vision/" + visionIO[i].getCameraName() + "/Single Tag Matrix Multiplier/Step 2", mult);
if (target.getPoseAmbiguity()<0.4)
mult*=Math.abs(estimatedRobotPose.estimatedPose.getZ())+0.01;
mult*=Math.pow(Math.abs(estimatedRobotPose.estimatedPose.getZ()),2)+0.01;
Logger.recordOutput("Vision/" + visionIO[i].getCameraName() + "/Single Tag Matrix Multiplier/Step 3", mult);
if (target.getArea()>0.05)
mult/= (target.getArea()*target.getArea())*8*8;
Logger.recordOutput("Vision/" + visionIO[i].getCameraName() + "/Single Tag Matrix Multiplier/Step 4", mult);
for (int j = 0; j < visionInputs.length; j++) {
if (i==j) break;
if (visionInputs[j].cameraResult.getTargets().size()>visionInputs[i].cameraResult.getTargets().size() && visionInputs[j].timestampSeconds != timestampSeconds[j])
mult += 0.5;
}
Logger.recordOutput("Vision/" + visionIO[i].getCameraName() + "/Single Tag Matrix Multiplier/Step 5", mult);
Logger.recordOutput("Vision/" + visionIO[i].getCameraName() + "/Single Tag Matrix Multiplier", mult);
visionMatrix = new Matrix<>(Nat.N3(), Nat.N1(), new double[]{8 * mult, 8 * mult, 12 * mult});
break;
Expand All @@ -352,6 +359,7 @@ public void periodic() {
MultiTargetPNPResult multiTagResult = visionInput.cameraResult.getMultiTagResult();
double meterErrorEstimation = (multiTagResult.estimatedPose.bestReprojErr / visionInput.cameraResult.getBestTarget().getArea()) * 0.045;
// meterErrorEstimation = 1;
Logger.recordOutput("Vision/" + visionIO[i].getCameraName() + "/Multi Tag meterErrorEstimation", meterErrorEstimation);
visionMatrix = new Matrix<>(Nat.N3(), Nat.N1(), new double[]{1.75 * meterErrorEstimation, 5 * meterErrorEstimation, 3 * meterErrorEstimation});
break;

Expand Down
Loading