Skip to content

Commit b4ea0a6

Browse files
Swordman51Swordman51
committed
changed stuff
changed ground intake, indexer, pivot arm, vision constants, visionIOPhoton and drive controls
1 parent 9baf59e commit b4ea0a6

File tree

7 files changed

+56
-10
lines changed

7 files changed

+56
-10
lines changed

‎src/main/java/frc/robot/RobotContainer.java

+10-3
Original file line numberDiff line numberDiff line change
@@ -274,6 +274,10 @@ private void configureButtonBindings() {
274274
drive.resetYaw();
275275
}, drive));
276276

277+
DRIVE_HOLD_STOP.onTrue(new InstantCommand(() -> {
278+
drive.stopWithX();
279+
}, drive));
280+
277281
// Drive Modes
278282
DRIVE_ROBOT_RELATIVE.whileTrue(DriveCommands.joystickDrive(
279283
drive,
@@ -333,6 +337,10 @@ private void configureButtonBindings() {
333337
SHOOTER_UNJAM.whileTrue(
334338
(indexer.manualCommand(IndexerConstants.INDEXER_OUT_VOLTAGE / 2)
335339
.alongWith(shooter.runVoltage(ShooterConstants.SHOOTER_UNJAM_VOLTAGE))));
340+
SHOOTER_PREPARE_THEN_SHOOT.whileTrue(shooter.runVoltage(ShooterConstants.SHOOTER_FULL_VOLTAGE));
341+
SHOOTER_PREPARE_THEN_SHOOT.onFalse(new WaitCommand(1)
342+
.deadlineWith(shooter.runVoltage(ShooterConstants.SHOOTER_FULL_VOLTAGE))
343+
.deadlineWith(indexer.manualCommand(IndexerConstants.INDEXER_IN_VOLTAGE)));
336344

337345
new Trigger(() -> (int) Timer.getMatchTime() == 30.0).onTrue(getRumbleDriver());
338346
new Trigger(indexer::isIntaked).onTrue(getRumbleOperator());
@@ -473,11 +481,10 @@ public Command shootAnywhereAuto() {
473481
}
474482

475483
public Command justShootAuto() {
476-
return shootSpeaker().onlyIf(DriveCommands::getPivotSideAngle)
477-
.andThen(shootSpeakerSide().onlyIf(() -> !DriveCommands.getPivotSideAngle()));
484+
return shootSpeakerSide();
478485
}
479486

480-
public Command prepShooter() {
487+
public Command `prepShooter() {
481488
return shooter.runVoltageBoth(rightShooterVolts::get, leftShooterVolts::get);
482489
}
483490

‎src/main/java/frc/robot/subsystems/groundIntake/GroundIntake.java

+4
Original file line numberDiff line numberDiff line change
@@ -27,6 +27,7 @@ public GroundIntake (GroundIntakeIO io) {
2727
logP = new LoggedDashboardNumber("GroundIntake/P", io.getP());
2828
logI = new LoggedDashboardNumber("GroundIntake/I", io.getI());
2929
logD = new LoggedDashboardNumber("GroundIntake/D", io.getD());
30+
3031
}
3132

3233
public void periodic() {
@@ -42,6 +43,9 @@ public void periodic() {
4243
io.setD(logD.get());
4344
}
4445
Logger.processInputs("GroundIntake", inputs);
46+
47+
48+
Logger.recordOutput("GroundIntake/GIntakeMotorConnected", inputs.velocityRadsPerSec != 0);
4549
}
4650

4751
public void setVoltage(double voltage) {

‎src/main/java/frc/robot/subsystems/indexer/Indexer.java

+5-1
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,7 @@
1010
import edu.wpi.first.wpilibj2.command.FunctionalCommand;
1111
import edu.wpi.first.wpilibj2.command.SubsystemBase;
1212
import frc.robot.subsystems.indexer.IndexerIOInputsAutoLogged;
13+
import frc.robot.subsystems.pivotArm.PivotArmConstants;
1314

1415
import java.util.function.DoubleSupplier;
1516

@@ -72,6 +73,8 @@ else if(noteState == NoteState.OVERSHOOT && !inputs.breakBeam) { // Note is eith
7273
} else {
7374
noteState = NoteState.MIDDLE;
7475
}
76+
77+
7578
}
7679

7780
// Updates the amount of time that the note has been in the intake for
@@ -81,8 +84,9 @@ else if(noteState == NoteState.OVERSHOOT && !inputs.breakBeam) { // Note is eith
8184
else {
8285
timeInIntake = 0;
8386
}
84-
87+
Logger.processInputs("Indexer", inputs);
8588
Logger.recordOutput("Indexer/State", noteState.name());
89+
Logger.recordOutput("Indexer/IndexerMotorConnected", inputs.velocityRadsPerSec != 0);
8690
}
8791

8892
public void setVoltage(double voltage) {

‎src/main/java/frc/robot/subsystems/pivotArm/PivotArm.java

+2
Original file line numberDiff line numberDiff line change
@@ -126,6 +126,8 @@ public void periodic() {
126126

127127
// Log Inputs
128128
Logger.processInputs("PivotArm", inputs);
129+
130+
Logger.recordOutput("PivotArm/PivotAbsoluteEncoderConnected", inputs.angleRads != PivotArmConstants.PIVOT_ARM_OFFSET);
129131
}
130132

131133
public void setBrake(boolean brake) {

‎src/main/java/frc/robot/subsystems/vision/VisionConstants.java

+2-2
Original file line numberDiff line numberDiff line change
@@ -56,8 +56,8 @@ public class VisionConstants {
5656
// The standard deviations of our vision estimated poses, which affect
5757
// correction rate
5858
// (Fake values. Experiment and determine estimation noise on an actual robot.)
59-
public static final Matrix<N3, N1> kSingleTagStdDevs = VecBuilder.fill(4, 4, 8);
60-
public static final Matrix<N3, N1> kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1);
59+
public static final Matrix<N3, N1> kSingleTagStdDevs = VecBuilder.fill(2, 2, 8);
60+
public static final Matrix<N3, N1> kMultiTagStdDevs = VecBuilder.fill(0.2, 0.2, 1);
6161

6262
public static Transform3d getSimVersion(Transform3d real) {
6363
return new Transform3d(

‎src/main/java/frc/robot/subsystems/vision/VisionIOPhoton.java

+27-4
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@
1919
import edu.wpi.first.math.geometry.Pose2d;
2020
import edu.wpi.first.math.geometry.Pose3d;
2121
import edu.wpi.first.net.PortForwarder;
22+
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
2223

2324
public class VisionIOPhoton implements VisionIO {
2425

@@ -53,6 +54,8 @@ public VisionIOPhoton() {
5354
camera3Estimator = new PhotonPoseEstimator(kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, camera3,
5455
cam3RobotToCam);
5556
camera3Estimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
57+
58+
SmartDashboard.putBoolean("KillSideCams", false);
5659
}
5760

5861
@Override
@@ -68,14 +71,20 @@ public void updateInputs(VisionIOInputs inputs, Pose2d currentEstimate) {
6871
inputs.timestamp = estimateLatestTimestamp(results);
6972

7073
if (hasEstimate(results)) {
71-
//inputs.results = results;
74+
// inputs.results = results;
7275
inputs.estimate = getEstimatesArray(results, photonEstimators);
7376
inputs.hasEstimate = true;
74-
77+
7578
int[][] cameraTargets = getCameraTargets(results);
7679
inputs.camera1Targets = cameraTargets[0];
77-
inputs.camera2Targets = cameraTargets[1];
78-
inputs.camera3Targets = cameraTargets[2];
80+
81+
if (SmartDashboard.getBoolean("KillSideCams", false)) {
82+
inputs.camera2Targets = new int[0];
83+
inputs.camera3Targets = new int[0];
84+
} else {
85+
inputs.camera2Targets = cameraTargets[1];
86+
inputs.camera3Targets = cameraTargets[2];
87+
}
7988

8089
Pose3d[] tags = getTargetsPositions(results);
8190
Logger.recordOutput("Vision/Targets3D", tags);
@@ -93,6 +102,14 @@ public void updateInputs(VisionIOInputs inputs, Pose2d currentEstimate) {
93102
}
94103

95104
private PhotonPipelineResult[] getAprilTagResults() {
105+
if (SmartDashboard.getBoolean("KillSideCams", false)) {
106+
PhotonPipelineResult cam1_result = getLatestResult(camera1);
107+
108+
printStuff("cam1", cam1_result);
109+
110+
return new PhotonPipelineResult[] { cam1_result };
111+
}
112+
96113
PhotonPipelineResult cam1_result = getLatestResult(camera1);
97114
PhotonPipelineResult cam2_result = getLatestResult(camera2);
98115
PhotonPipelineResult cam3_result = getLatestResult(camera3);
@@ -115,6 +132,12 @@ private void printStuff(String name, PhotonPipelineResult result) {
115132
}
116133

117134
private PhotonPoseEstimator[] getAprilTagEstimators(Pose2d currentEstimate) {
135+
if (SmartDashboard.getBoolean("KillSideCams", false)) {
136+
camera1Estimator.setReferencePose(currentEstimate);
137+
138+
return new PhotonPoseEstimator[] { camera1Estimator };
139+
}
140+
118141
camera1Estimator.setReferencePose(currentEstimate);
119142
camera2Estimator.setReferencePose(currentEstimate);
120143
camera3Estimator.setReferencePose(currentEstimate);

‎src/main/java/frc/robot/util/drive/DriveControls.java

+6
Original file line numberDiff line numberDiff line change
@@ -24,6 +24,7 @@ public class DriveControls {
2424
public static DoubleSupplier DRIVE_ROTATE;
2525
public static Trigger DRIVE_SLOW;
2626
public static Trigger DRIVE_STOP;
27+
public static Trigger DRIVE_HOLD_STOP;
2728

2829
// drive modes
2930
public static Trigger DRIVE_ROBOT_RELATIVE;
@@ -64,6 +65,7 @@ public class DriveControls {
6465
public static Trigger SHOOTER_FULL_SEND_INTAKE;
6566
public static Trigger SHOOTER_FULL_SEND;
6667
public static Trigger SHOOTER_UNJAM;
68+
public static Trigger SHOOTER_PREPARE_THEN_SHOOT;
6769

6870
// Setup the controls
6971
public static void configureControls() {
@@ -77,6 +79,7 @@ public static void configureControls() {
7779
// Driver Settings
7880
DRIVE_SLOW = driver.start();
7981
DRIVE_STOP = driver.x();
82+
DRIVE_HOLD_STOP = driver.a();
8083

8184
// Driver Modes
8285
DRIVE_ROBOT_RELATIVE = driver.y();
@@ -100,6 +103,7 @@ public static void configureControls() {
100103
// Driver Settings
101104
DRIVE_SLOW = driver.start();
102105
DRIVE_STOP = driver.x();
106+
DRIVE_HOLD_STOP = EMPTY_TRIGGER;
103107

104108
// Driver Modes
105109
DRIVE_ROBOT_RELATIVE = driver.y();
@@ -142,6 +146,7 @@ public static void configureControls() {
142146
SHOOTER_FULL_SEND_INTAKE = operator.getLefJoystucikPress();
143147
SHOOTER_FULL_SEND = operator.getDPad(DPad.DOWN);
144148
SHOOTER_UNJAM = operator.getDPad(DPad.RIGHT);
149+
SHOOTER_PREPARE_THEN_SHOOT = operator.back();
145150
break;
146151
case PROGRAMMERS:
147152
default:
@@ -167,6 +172,7 @@ public static void configureControls() {
167172
SHOOTER_FULL_SEND_INTAKE = EMPTY_TRIGGER;
168173
SHOOTER_FULL_SEND = EMPTY_TRIGGER;
169174
SHOOTER_UNJAM = EMPTY_TRIGGER;
175+
SHOOTER_PREPARE_THEN_SHOOT = EMPTY_TRIGGER;
170176
break;
171177

172178
//bottom right Left joystick to intake

0 commit comments

Comments
 (0)