Skip to content

Commit

Permalink
assorted changes
Browse files Browse the repository at this point in the history 
Signed-off-by: a1cd <[email protected]>
  • Loading branch information
@a1cd
a1cd committed Apr 5, 2024
1 parent 7060bdb commit a7f966a
Show file tree
Hide file tree
Showing 2 changed files with 26 additions and 56 deletions.
70 changes: 16 additions & 54 deletions src/main/java/frc/robot/RobotContainer.java
View file
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
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

0 comments on commit a7f966a

Please sign in to comment.