Skip to content

Commit 915c089

Browse files
PotatoBoyH4PotatoBoyH4
committed
made proggress, do not use the algae
1 parent 3537a7d commit 915c089

17 files changed

+800
-156
lines changed

‎build.gradle‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -81,7 +81,7 @@ dependencies {
8181

8282
implementation 'ch.qos.logback:logback-classic:1.3.5' //logging
8383
implementation("com.opencsv:opencsv:5.6")
84-
implementation 'org.strykeforce:wallEYE:25.0.0'
84+
implementation 'org.strykeforce:wallEYE:25.0.1'
8585
implementation 'net.jafama:jafama:2.3.2' //fastMath
8686
}
8787

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

Lines changed: 12 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -9,11 +9,13 @@
99
import edu.wpi.first.units.measure.Angle;
1010
import edu.wpi.first.wpilibj.Joystick;
1111
import edu.wpi.first.wpilibj.XboxController;
12+
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
1213
import edu.wpi.first.wpilibj2.command.Command;
1314
import edu.wpi.first.wpilibj2.command.Commands;
1415
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
1516
import edu.wpi.first.wpilibj2.command.button.Trigger;
1617
import frc.robot.commands.algae.OpenLoopAlgaeCommand;
18+
import frc.robot.commands.algae.ToggleHasAlgaeCommand;
1719
import frc.robot.commands.biscuit.HoldBiscuitCommand;
1820
import frc.robot.commands.biscuit.JogBiscuitCommand;
1921
import frc.robot.commands.coral.EnableEjectBeamCommand;
@@ -131,7 +133,7 @@ public RobotContainer() {
131133
ledIO = new LEDIO();
132134
ledSubsystem = new LEDSubsystem();
133135

134-
visionSubsystem = new VisionSubsystem();
136+
visionSubsystem = new VisionSubsystem(driveSubsystem);
135137

136138
tagAlignSubsystem = new TagAlignSubsystem(driveSubsystem, visionSubsystem);
137139

@@ -154,6 +156,7 @@ public RobotContainer() {
154156
configureTelemetry();
155157
configureDriverBindings();
156158
configureOperatorBindings();
159+
configurePitDashboard();
157160
}
158161

159162
private void configureTelemetry() {
@@ -325,6 +328,14 @@ private void configureTestOperatorBindings() {
325328
new SetElevatorPositionCommand(elevatorSubsystem, ElevatorConstants.kL4CoralSetpoint));
326329
}
327330

331+
public void configurePitDashboard() {
332+
333+
Shuffleboard.getTab("Pit")
334+
.add("Toggle HasAlgae", new ToggleHasAlgaeCommand(algaeSubsystem))
335+
.withPosition(3, 2)
336+
.withSize(1, 1);
337+
}
338+
328339
public Command getAutonomousCommand() {
329340
return Commands.print("No autonomous command configured");
330341
}

‎src/main/java/frc/robot/commands/algae/ToggleHasAlgaeCommand.java‎

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,19 @@
1+
package frc.robot.commands.algae;
2+
3+
import edu.wpi.first.wpilibj2.command.InstantCommand;
4+
import frc.robot.subsystems.algae.AlgaeSubsystem;
5+
import frc.robot.subsystems.algae.AlgaeSubsystem.AlgaeStates;
6+
7+
public class ToggleHasAlgaeCommand extends InstantCommand {
8+
private AlgaeSubsystem algaeSubsystem;
9+
10+
public ToggleHasAlgaeCommand(AlgaeSubsystem algaeSubsystem) {
11+
this.algaeSubsystem = algaeSubsystem;
12+
}
13+
14+
@Override
15+
public void initialize() {
16+
algaeSubsystem.setState(
17+
algaeSubsystem.getState() == AlgaeStates.EMPTY ? AlgaeStates.HAS_ALGAE : AlgaeStates.EMPTY);
18+
}
19+
}

‎src/main/java/frc/robot/commands/robotState/GetAlgaeCommand.java‎

Lines changed: 0 additions & 21 deletions
This file was deleted.

‎src/main/java/frc/robot/commands/robotState/ToggleAlgaeHeightCommand.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,6 @@ public ToggleAlgaeHeightCommand(RobotStateSubsystem robotState) {
1212

1313
@Override
1414
public void initialize() {
15-
robotState.toggleAlgaeHeight(null);
15+
robotState.toggleAlgaeHeight();
1616
}
1717
}

‎src/main/java/frc/robot/constants/BiscuitConstants.java‎

Lines changed: 12 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -31,6 +31,8 @@ public class BiscuitConstants {
3131
public static double kCloseEnough = 0.05; // This was a little out of wack.
3232
public static final Angle kMaxFwd = Rotations.of(100);
3333
public static final Angle kMaxRev = Rotations.of(-100);
34+
public static final double kSafeToStowUpper = 40;
35+
public static final double kSafeToStowLower = -5;
3436

3537
// Setpoints
3638
// Idle
@@ -39,11 +41,11 @@ public class BiscuitConstants {
3941
public static final Angle kPrestageSetpoint = kStowSetpoint;
4042

4143
// Algae removal
42-
public static final Angle kL2AlgaeSetpoint = Rotations.of(120.12604);
43-
public static final Angle kL3AlgaeSetpoint = Rotations.of(120.12604);
44+
public static final Angle kL2AlgaeSetpoint = Rotations.of(19.11865);
45+
public static final Angle kL3AlgaeSetpoint = Rotations.of(19.11865);
4446

45-
public static final Angle kL2AlgaeRemovalSetpoint = Rotations.of(120.12604);
46-
public static final Angle kL3AlgaeRemovalSetpoint = Rotations.of(120.12604);
47+
public static final Angle kL2AlgaeRemovalSetpoint = Rotations.of(19.11865);
48+
public static final Angle kL3AlgaeRemovalSetpoint = Rotations.of(19.11865);
4749

4850
public static final Angle kSafeAlgaeRemovalSetpoint = Rotations.of(0.0);
4951
public static final Angle kSafeAlgaeRemovalRotateSetpoint = Rotations.of(0.0);
@@ -55,14 +57,14 @@ public class BiscuitConstants {
5557
public static final Angle kL4CoralSetpoint = kStowSetpoint;
5658

5759
// Algae obtaining
58-
public static final Angle kFloorAlgaeSetpoint = Rotations.of(320.74);
59-
public static final Angle kMicAlgaeSetpoint = Rotations.of(324.33);
60-
public static final Angle kHpAlgaeSetpoint = Rotations.of(106.6608);
60+
public static final Angle kFloorAlgaeSetpoint = Rotations.of(51.04735);
61+
public static final Angle kMicAlgaeSetpoint = Rotations.of(51.61872);
62+
public static final Angle kHpAlgaeSetpoint = Rotations.of(16.97559);
6163

6264
// Algae scoring
63-
public static final Angle kProcessorSetpoint = Rotations.of(319.6908);
64-
public static final Angle kBargeSetpoint = Rotations.of(77.5906);
65-
public static final Angle kBargeBackwardsSetpoint = Rotations.of(-77.5906);
65+
public static final Angle kProcessorSetpoint = Rotations.of(50.88037);
66+
public static final Angle kBargeSetpoint = Rotations.of(12.3489);
67+
public static final Angle kBargeBackwardSetpoint = Rotations.of(-12.3489);
6668

6769
// jogging
6870
public static final double kJogAmountUp = 10;

‎src/main/java/frc/robot/constants/DriveConstants.java‎

Lines changed: 14 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,8 @@
1010
import com.ctre.phoenix6.configs.Slot0Configs;
1111
import com.ctre.phoenix6.configs.TalonFXConfiguration;
1212
import com.ctre.phoenix6.signals.NeutralModeValue;
13+
import edu.wpi.first.math.geometry.Pose2d;
14+
import edu.wpi.first.math.geometry.Rotation2d;
1315
import edu.wpi.first.math.geometry.Translation2d;
1416

1517
public class DriveConstants {
@@ -21,9 +23,16 @@ public class DriveConstants {
2123
public static final double kRateLimitFwdStr = 3.5;
2224
public static final double kRateLimitYaw = 8.0;
2325

24-
public static final double kDriveGearRatio = 6.5;
26+
public static final double kDriveMotorOutputGear = 22;
27+
public static final double kDriveInputGear = 52;
28+
public static final double kBevelInputGear = 15;
29+
public static final double kBevelOutputGear = 45;
30+
31+
public static final double kDriveGearRatio =
32+
(kDriveMotorOutputGear / kDriveInputGear) * (kBevelInputGear / kBevelOutputGear);
33+
2534
public static final double kWheelDiameterInches = 4.0;
26-
public static final double kMaxSpeedMetersPerSecond = 12.0;
35+
public static final double kMaxSpeedMetersPerSecond = 3.384;
2736
public static final double kSpeedStillThreshold = 0.1; // meters per second
2837
public static final double kGyroRateStillThreshold = 10.0; // 25 5 degrees per second
2938
public static final double kGyroDifferentThreshold = 5.0; // 5 degrees
@@ -61,6 +70,9 @@ public static Translation2d[] getWheelLocationMeters() {
6170
public static final double kRecoverTemp = 1290;
6271
public static final double kNotifyTemp = 1295;
6372

73+
public static final Pose2d kResetOdomPose =
74+
new Pose2d(new Translation2d(0.5, 3.62), Rotation2d.fromDegrees(67));
75+
6476
// public static TalonFXSConfiguration
6577
// getAzimuthTalonConfig() { // will be changed to a TalonFXConfiguration
6678
// // constructor sets encoder to Quad/CTRE_MagEncoder_Relative

‎src/main/java/frc/robot/constants/TagServoingConstants.java‎

Lines changed: 17 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -6,11 +6,16 @@
66
public class TagServoingConstants {
77
// Cameras
88
public static final int kLeftServoCam = 0; // Score RIGHT coral
9-
public static final int kRightServoCam = 1; // Score LEFT coral
9+
public static final int kRightServoCam = 2; // Score LEFT coral
10+
11+
// Offsets
12+
public static final double kLeftCamOffset = VisionConstants.kCam1Pose.getY();
13+
public static final double kRightCamOffset = VisionConstants.kCam2Pose.getY();
1014

1115
// Targets
1216
public static final double kHorizontalTarget = 800;
13-
public static final double kAreaTarget = 800;
17+
public static final double kLeftCamDiagTarget = 1180;
18+
public static final double kRightCamDiagTarget = 985;
1419

1520
public static final double[] kAngleTarget = {
1621
Units.degreesToRadians(0),
@@ -26,19 +31,21 @@ public class TagServoingConstants {
2631
public static final int[] kRedTargetTag = {7, 8, 9, 10, 11, 6};
2732

2833
// Tag align
29-
public static final double kAreaCloseEnough = 0;
30-
public static final double kHorizontalCloseEnough = 0;
34+
public static final double kHorizontalCloseEnough = 20;
3135
public static final double kAngleCloseEnough = Units.degreesToRadians(3.0);
36+
public static final double kDiagCloseEnough = 20;
37+
public static final double kNoUpdateMicrosec = 500_000;
3238

3339
// Drive
3440
public static final double kInitialDriveRadius = 1.5;
35-
public static final double kStopXDriveRadius = 1.2; // Should be closer to reef than target pose
36-
public static final double kDriveCloseEnough = 0.3;
41+
public static final double kStopXDriveRadius =
42+
kInitialDriveRadius; // Should be closer to reef than target pose
43+
public static final double kMinStopXDriveRadius = 1.35;
44+
public static final double kDriveCloseEnough = 0.1;
45+
public static final double kMinVelX = 0.85;
3746

3847
// Reef
39-
public static final Translation2d kBlueReefPose =
40-
new Translation2d(Units.inchesToMeters(223.5), Units.inchesToMeters(158.5));
48+
public static final Translation2d kBlueReefPose = new Translation2d(4.524, 4.033);
4149

42-
public static final Translation2d kRedReefPose =
43-
kBlueReefPose.plus(new Translation2d(Units.inchesToMeters(337.39), 0));
50+
public static final Translation2d kRedReefPose = new Translation2d(13.084, 4.033);
4451
}

‎src/main/java/frc/robot/constants/VisionConstants.java‎

Lines changed: 44 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@ public final class VisionConstants {
1919
public static final double kMaxAmbig = 1.0;
2020
public static final int kMaxTimesOffWheels = 5;
2121
public static final double kBumperPixelLine = 87; // 100
22+
public static final double kRobotHeight = 0.5;
2223

2324
// public static final double kThetaStdDevUsed = Units.degreesToRadians(0.02);
2425
// public static final double kThetaStdDevRejected = Units.degreesToRadians(360);
@@ -29,71 +30,93 @@ public final class VisionConstants {
2930
public static final double kOffsetOnVelFilter = 0.10;
3031
public static final double kSquaredCoeffOnVelFilter = 0.1;
3132

32-
public static Matrix<N3, N1> kStateStdDevs = VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(0));
33+
public static Matrix<N3, N1> kStateStdDevs = VecBuilder.fill(0.1, 0.1, 0);
3334

3435
public static final double kTimeStampOffset = 0.0;
3536

3637
// StdDev scaling
3738
public static final double singleTagCoeff = 25.0 / 100.0;
3839
public static final double multiTagCoeff = 18.0 / 100.0;
3940
public static final double baseNumber = Math.E;
40-
public static final double powerNumber = 4.0;
41+
public static final double powerNumber = 2.0;
42+
public static final double baseTrust = 3.0;
43+
4144
public static final double FOV45MultiTagCoeff = 16.0 / 100.0;
4245
public static final double FOV45powerNumber = 4.5;
43-
public static final double FOV45SinlgeTagCoeff = 21.0 / 100.0;
46+
public static final double FOV45SingleTagCoeff = 21.0 / 100.0;
47+
public static final double FOV45BaseTrust = 2.0;
48+
4449
public static final double FOV58MJPGMultiTagCoeff = 16.0 / 100.0;
4550
public static final double FOV58MJPGPowerNumber = 3.5;
4651
public static final double FOV58MJPGSingleTagCoeff = 21.0 / 100.0;
52+
public static final double FOV58MJPGBaseTrust = 3.0;
53+
4754
public static final double FOV58YUYVMultiTagCoeff = 17.0 / 100.0;
48-
public static final double FOV58YUYVPowerNumber = 4.0;
55+
public static final double FOV58YUYVPowerNumber = 2.0;
4956
public static final double FOV58YUYVSingleTagCoeff = 22.0 / 100.0;
57+
public static final double FOV58YUYVBaseTrust = 3.0;
58+
59+
public static final double FOV75YUYVMultiTagCoeff = 17.0 / 100.0;
60+
public static final double FOV75YUYVPowerNumber = 2.0;
61+
public static final double FOV75YUYVSingleTagCoeff = 22.0 / 100.0;
62+
public static final double FOV75YUYVBaseTrust = 3.0;
63+
64+
// Gyro error scaling
65+
public static final double kYawErrorThreshold = Units.degreesToRadians(45);
5066

5167
// Constants for cameras
52-
public static final int kNumCams = 4;
68+
public static final int kNumCams = 5;
69+
public static final int kNumPis = 3;
70+
public static final int[] kUdpIndex = {0, 1, 2};
71+
72+
// Camera Ports
73+
public static final int[] kCamPorts = {5802, 5802, 5803, 5803, 5804};
5374

5475
// Names
55-
public static final String kCam1Name = "Shooter";
56-
public static final String kCam2Name = "Intake";
57-
public static final String kCam3Name = "AngledShooterLeft";
58-
public static final String kCam4Name = "AngledShooterRight";
76+
public static final String kCam1Name = "Left Servo";
77+
public static final String kCam2Name = "Left High";
78+
public static final String kCam3Name = "Right Servo";
79+
public static final String kCam4Name = "Right High";
80+
public static final String kCam5Name = "Rear";
5981

60-
public static final String kPi1Name = "Shooter";
61-
public static final String kPi2Name = "Intake";
62-
public static final String kPi3Name = "AngledShooters";
82+
public static final String kPi1Name = "Left";
83+
public static final String kPi2Name = "Right";
84+
public static final String kPi3Name = "Rear";
6385

6486
// Indexs
6587
public static final int kCam1Idx = 0;
66-
public static final int kCam2Idx = 0;
88+
public static final int kCam2Idx = 1;
6789
public static final int kCam3Idx = 0;
6890
public static final int kCam4Idx = 1;
91+
public static final int kCam5Idx = 0;
6992

7093
public static final double kLoopTime = 0.02;
7194
public static final int kCircularBufferSize = 1000;
7295
// Poses
7396
public static final Pose3d kCam1Pose =
74-
new Pose3d(
75-
new Translation3d(-0.27, 0.055, 0.20),
76-
new Rotation3d(0, Units.degreesToRadians(20.0), Units.degreesToRadians(180.0)));
97+
new Pose3d(new Translation3d(0.28, 0.02, 0.30), new Rotation3d());
98+
7799
public static final Pose3d kCam2Pose =
78100
new Pose3d(
79101
new Translation3d(-0.21, -0.31, 0.44),
80102
new Rotation3d(0, Units.degreesToRadians(20.0), Units.degreesToRadians(0.0)));
81103
public static final Pose3d kCam3Pose =
82-
new Pose3d(
83-
new Translation3d(-0.23, 0.33, 0.56),
84-
new Rotation3d(0, Units.degreesToRadians(20.0), Units.degreesToRadians(-132.0)));
104+
new Pose3d(new Translation3d(0.09, -0.31, 0.36), new Rotation3d());
85105
public static final Pose3d kCam4Pose =
86106
new Pose3d(
87107
new Translation3d(-0.22, -0.335, 0.50),
88108
new Rotation3d(0, Units.degreesToRadians(20.0), Units.degreesToRadians(138.0)));
89-
109+
public static final Pose3d kCam5Pose =
110+
new Pose3d(
111+
new Translation3d(-0.22, -0.335, 0.50),
112+
new Rotation3d(0, Units.degreesToRadians(20.0), Units.degreesToRadians(138.0)));
90113
// Increase these numbers to trust sensor readings from encoders and gyros less. This matrix is
91114
// in the form [theta], with units in radians.
92115
public static Matrix<N1, N1> kLocalMeasurementStdDevs =
93116
VecBuilder.fill(Units.degreesToRadians(0.01));
94117

95118
// Increase these numbers to trust global measurements from vision less. This matrix is in the
96-
// form [x, y, theta], with units in meters and radians.
119+
// form [x, y, theta]ᵀ, with units in meters and radians.
97120
// Vision Odometry Standard devs
98121
public static Matrix<N3, N1> kVisionMeasurementStdDevs =
99122
VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(360));

‎src/main/java/frc/robot/subsystems/algae/AlgaeSubsystem.java‎

Lines changed: 9 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -20,7 +20,7 @@ public class AlgaeSubsystem extends MeasurableSubsystem implements ClosedLoopSpe
2020
private final AlgaeIOInputs inputs = new AlgaeIOInputs();
2121
private AngularVelocity desiredSpeed = RotationsPerSecond.of(0);
2222

23-
private AlgaeStates curState = AlgaeStates.IDLE;
23+
private AlgaeStates curState = AlgaeStates.EMPTY;
2424

2525
public AlgaeSubsystem(AlgaeIO io) {
2626
this.io = io;
@@ -84,16 +84,16 @@ public void periodic() {
8484

8585
switch (curState) {
8686
case HAS_ALGAE -> {
87-
if (!inputs.isLimitSwitchClosed) {
88-
setState(AlgaeStates.EMPTY);
89-
setSpeed(RotationsPerSecond.of(0));
90-
}
87+
// if (!inputs.isLimitSwitchClosed) {
88+
// setState(AlgaeStates.EMPTY);
89+
// setSpeed(RotationsPerSecond.of(0));
90+
// }
9191
}
9292
case EMPTY -> {
93-
if (inputs.isLimitSwitchClosed) { // FIXME: correct?
94-
hold();
95-
setState(AlgaeStates.HAS_ALGAE);
96-
}
93+
// if (inputs.isLimitSwitchClosed) { // FIXME: correct?
94+
// hold();
95+
// setState(AlgaeStates.HAS_ALGAE);
96+
// }
9797
}
9898
case IDLE -> {}
9999
}

0 commit comments

Comments
 (0)