Skip to content

Commit d49bc6c

Browse files
ohowe1ohowe1
committed
Testing readyish
1 parent 4bb8c2b commit d49bc6c

File tree

13 files changed

+300
-200
lines changed

13 files changed

+300
-200
lines changed

‎build.gradle‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -29,6 +29,13 @@ deploy {
2929
// getTargetTypeClass is a shortcut to get the class type using a string
3030

3131
frcJava(getArtifactTypeClass('FRCJavaArtifact')) {
32+
// Enable VisualVM connection
33+
jvmArgs.add("-Dcom.sun.management.jmxremote=true")
34+
jvmArgs.add("-Dcom.sun.management.jmxremote.port=1198")
35+
jvmArgs.add("-Dcom.sun.management.jmxremote.local.only=false")
36+
jvmArgs.add("-Dcom.sun.management.jmxremote.ssl=false")
37+
jvmArgs.add("-Dcom.sun.management.jmxremote.authenticate=false")
38+
jvmArgs.add("-Djava.rmi.server.hostname=127.0.0.1") // Replace XX.XX with team number
3239
}
3340

3441
// Static files artifact

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

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -7,12 +7,12 @@ public final class BuildConstants {
77
public static final String MAVEN_GROUP = "";
88
public static final String MAVEN_NAME = "Competition2024";
99
public static final String VERSION = "unspecified";
10-
public static final int GIT_REVISION = 116;
11-
public static final String GIT_SHA = "169b6f70fac563f1a8d2dca9938c53e55ae751a4";
12-
public static final String GIT_DATE = "2024-02-21 09:23:36 MST";
10+
public static final int GIT_REVISION = 117;
11+
public static final String GIT_SHA = "4bb8c2bf00761887ca8846cd361c7484a200594e";
12+
public static final String GIT_DATE = "2024-02-21 11:44:18 MST";
1313
public static final String GIT_BRANCH = "workingongettingready";
14-
public static final String BUILD_DATE = "2024-02-21 10:39:21 MST";
15-
public static final long BUILD_UNIX_TIME = 1708537161942L;
14+
public static final String BUILD_DATE = "2024-02-21 14:46:36 MST";
15+
public static final long BUILD_UNIX_TIME = 1708551996489L;
1616
public static final int DIRTY = 1;
1717

1818
private BuildConstants(){}

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

Lines changed: 20 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -52,6 +52,8 @@ public static class SlapdownConstants {
5252

5353
public static final int ROTATION_LIMIT_SWITCH_ID = 1;
5454

55+
public static final double FEEDER_VOLTAGE = 6.0;
56+
5557
public static final TunablePIDGains ROTATION_GAINS =
5658
new TunablePIDGains("/slapdown/rotation/gains", 0, 0, 0, MiscConstants.TUNING_MODE);
5759
public static final TunableTrapezoidalProfileGains ROTATION_TRAP_GAINS =
@@ -86,8 +88,8 @@ public static class ElevatorConstants {
8688
public static final TunableTrapezoidalProfileGains TRAPEZOIDAL_PROFILE_GAINS =
8789
new TunableTrapezoidalProfileGains(
8890
"/gains/extension",
89-
Units.inchesToMeters(15),
90-
Units.inchesToMeters(40),
91+
Units.inchesToMeters(25),
92+
Units.inchesToMeters(100),
9193
MiscConstants.TUNING_MODE);
9294

9395
// TODO: TUNE FF GAINS
@@ -213,22 +215,30 @@ public static class WristConstants {
213215
public static final Rotation2d WRIST_MAX = new Rotation2d(Units.degreesToRadians(60));
214216
public static final Rotation2d WRIST_MIN = new Rotation2d(0);
215217

218+
public static final double WRIST_OFFSET = 0.0;
219+
public static final int WRIST_ENCODER_PORT = 2;
220+
216221
public static final int WRIST_MOTOR_ID = 2;
217222
public static final boolean INVERTED = true;
218223
public static final int STALL_MOTOR_CURRENT = 45;
219224
public static final int FREE_MOTOR_CURRENT = 25;
220225

221226
// TODO: TUNE PID & TRAP & FF
222227
public static final TunableArmElevatorFFGains WRIST_FF_GAINS =
223-
new TunableArmElevatorFFGains("/wrist/ffGains", 0.078, 0.292, 1.0951, 0.261, MiscConstants.TUNING_MODE);
224-
// public static final TunablePIDGains WRIST_PID_GAINS =
225-
// new TunablePIDGains("/wrist/pidGains", 56.599, 0, 4.8897, MiscConstants.TUNING_MODE);
226-
//
227-
public static final TunablePIDGains WRIST_PID_GAINS =
228+
new TunableArmElevatorFFGains(
229+
"/wrist/ffGains", 0.078, 0.292, 1.0951, 0.261, MiscConstants.TUNING_MODE);
230+
// public static final TunablePIDGains WRIST_PID_GAINS =
231+
// new TunablePIDGains("/wrist/pidGains", 56.599, 0, 4.8897, MiscConstants.TUNING_MODE);
232+
//
233+
public static final TunablePIDGains WRIST_PID_GAINS =
228234
new TunablePIDGains("/wrist/pidGains", 0, 0, 0, MiscConstants.TUNING_MODE);
229235

230236
public static final TunableTrapezoidalProfileGains TRAPEZOIDAL_PROFILE_GAINS =
231-
new TunableTrapezoidalProfileGains("/wrist/trapGains", Units.rotationsToRadians(0.5), Units.rotationsToRadians(0.5), MiscConstants.TUNING_MODE);
237+
new TunableTrapezoidalProfileGains(
238+
"/wrist/trapGains",
239+
Units.rotationsToRadians(0.5),
240+
Units.rotationsToRadians(0.5),
241+
MiscConstants.TUNING_MODE);
232242
}
233243

234244
public static class TransportConstants {
@@ -255,15 +265,9 @@ public static class ShooterConstants {
255265
public static final double SHOOTER_GEAR_RATIO = 1.0 / 2.0;
256266

257267
public static TunablePIDGains SHOOTER_PID_GAINS =
258-
new TunablePIDGains(
259-
"/shooter/pid", 0.005, 0, 0, MiscConstants.TUNING_MODE);
268+
new TunablePIDGains("/shooter/pid", 0.005, 0, 0, MiscConstants.TUNING_MODE);
260269
public static TunableFFGains SHOOTER_FF_GAINS =
261-
new TunableFFGains(
262-
"/shooter/FF",
263-
0.39383,
264-
0.0089833,
265-
0.0010833,
266-
MiscConstants.TUNING_MODE);
270+
new TunableFFGains("/shooter/FF", 0.39383, 0.0089833, 0.0010833, MiscConstants.TUNING_MODE);
267271
}
268272

269273
public static class ClimberConstants {

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

Lines changed: 43 additions & 28 deletions
Original file line numberDiff line numberDiff line change
@@ -26,7 +26,7 @@
2626
import frc.robot.subsystems.elevator.ElevatorSubsystem;
2727
import frc.robot.subsystems.intake.IntakeSubsystem;
2828
import frc.robot.subsystems.shooter.ShooterSubsystem;
29-
import frc.robot.subsystems.slapdown.SlapdownSubsystem;
29+
import frc.robot.subsystems.slapdown.SlapdownSuperstructure;
3030
import frc.robot.subsystems.swerve.SwerveDriveSubsystem;
3131
import frc.robot.subsystems.transport.TransportSubsystem;
3232
import frc.robot.subsystems.wrist.WristSubsystem;
@@ -47,15 +47,13 @@ public class RobotContainer {
4747
// new SwerveDriveSubsystem(photonSubsystem::getEstimatedGlobalPose);
4848
private final SwerveDriveSubsystem driveSubsystem =
4949
new SwerveDriveSubsystem(
50-
(pose) -> {
51-
return List.of();
52-
});
50+
(pose) -> List.of());
5351
private final ElevatorSubsystem elevatorSubsystem = new ElevatorSubsystem();
5452
private final ShooterSubsystem shooterSubsystem = new ShooterSubsystem();
5553
private final WristSubsystem wristSubsystem = new WristSubsystem();
5654
private final TransportSubsystem transportSubsystem = new TransportSubsystem();
5755
private final IntakeSubsystem intakeSubsystem = new IntakeSubsystem();
58-
private final SlapdownSubsystem slapdownSubsystem = new SlapdownSubsystem();
56+
private final SlapdownSuperstructure slapdownSuperstructure = new SlapdownSuperstructure();
5957
private final ClimberSubsystem climberSubsystem = new ClimberSubsystem();
6058

6159
private final SendableChooser<Command> autoCommand = new SendableChooser<>();
@@ -79,11 +77,17 @@ public RobotContainer() {
7977
}
8078

8179
private void configureAutos() {
82-
autoCommand.addOption("Wrist Q Forward", wristSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
83-
autoCommand.addOption("Wrist Q Back", wristSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
84-
autoCommand.addOption("Wrist D Forward", wristSubsystem.sysIdDynamic(SysIdRoutine.Direction.kForward));
85-
autoCommand.addOption("Wrist D Back", wristSubsystem.sysIdDynamic(SysIdRoutine.Direction.kReverse));
86-
autoCommand.addOption("Wrist 5 deg", wristSubsystem.setPositonCommand(new Rotation2d(Units.degreesToRadians(40))));
80+
autoCommand.addOption(
81+
"Wrist Q Forward", wristSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
82+
autoCommand.addOption(
83+
"Wrist Q Back", wristSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
84+
autoCommand.addOption(
85+
"Wrist D Forward", wristSubsystem.sysIdDynamic(SysIdRoutine.Direction.kForward));
86+
autoCommand.addOption(
87+
"Wrist D Back", wristSubsystem.sysIdDynamic(SysIdRoutine.Direction.kReverse));
88+
autoCommand.addOption(
89+
"Wrist 5 deg",
90+
wristSubsystem.setPositonCommand(new Rotation2d(Units.degreesToRadians(40))));
8791
autoCommand.addOption(
8892
"Elevator Quastatic Backward",
8993
elevatorSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
@@ -106,12 +110,6 @@ private void configureAutos() {
106110
"Intake Test Command",
107111
intakeSubsystem.setIntakeVoltageCommand(Constants.IntakeConstants.INTAKE_VOLTAGE));
108112
autoCommand.addOption("Shooter Test Command 4000", shooterSubsystem.runVelocityCommand(100));
109-
autoCommand.addOption("Slap Feed", slapdownSubsystem.setFeederVoltageCommand(5));
110-
autoCommand.addOption(
111-
"Intake feed",
112-
Commands.parallel(
113-
slapdownSubsystem.setFeederVoltageCommand(5),
114-
intakeSubsystem.setIntakeVoltageCommand(Constants.IntakeConstants.INTAKE_VOLTAGE)));
115113
autoCommand.addOption(
116114
"Shooter Quastatic Forward Command",
117115
shooterSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
@@ -155,15 +153,10 @@ private void configureDriverBindings() {
155153
// .onTrue(nearestAmpCommand(DriverStation.getAlliance().get(), driveSubsystem));
156154
driverController
157155
.povDown()
158-
.toggleOnTrue(slapdownSubsystem.setRotationGoalCommand(new Rotation2d(0)));
156+
.toggleOnTrue(slapdownSuperstructure.setDownAndRunCommand());
159157
driverController
160-
.povDown()
161-
.toggleOnTrue(slapdownSubsystem.setFeederVoltageCommand(4)); // TODO: THIS
162-
driverController
163-
.povDown()
164-
.toggleOnFalse(
165-
slapdownSubsystem.setRotationGoalCommand(new Rotation2d(Units.degreesToRadians(90))));
166-
driverController.povDown().toggleOnFalse(slapdownSubsystem.setFeederVoltageCommand(0));
158+
.povUp()
159+
.toggleOnTrue(slapdownSuperstructure.setUpCommand()); // TODO: THIS
167160
driverController.a().onTrue(intakeSubsystem.checkIntakeCommand());
168161
}
169162

@@ -181,7 +174,10 @@ private void configureOperatorBindings() {
181174
// operatorController
182175
// .rightStick()
183176
// .whileTrue(elevatorSubsystem.runElevatorCommand(operatorController.getRightY()));
184-
operatorController.rightTrigger().onTrue(new ShootAtAngleCommand(
177+
operatorController
178+
.rightTrigger()
179+
.onTrue(
180+
new ShootAtAngleCommand(
185181
shooterSubsystem, transportSubsystem, wristSubsystem, SHOOTING_ANGLE));
186182
operatorController
187183
.leftTrigger()
@@ -197,10 +193,29 @@ private void configureOperatorBindings() {
197193
.onTrue(
198194
new AmpPlaceCommand(
199195
elevatorSubsystem, wristSubsystem, shooterSubsystem, transportSubsystem));
200-
operatorController.share().whileTrue(Commands.parallel(elevatorSubsystem.setElevatorPositionCommand(0), slapdownSubsystem.setFeederVoltageCommand(6), intakeSubsystem.setIntakeVoltageCommand(6), transportSubsystem.setVoltageCommand(4.0).until(transportSubsystem::atSensor).andThen(transportSubsystem.setVoltageCommand(0.0))));
201-
// operatorController.share().onTrue(transportSubsystem.setVoltageCommand(6.0).until(transportSubsystem::atSensor));
196+
operatorController
197+
.share()
198+
.whileTrue(
199+
Commands.parallel(
200+
elevatorSubsystem.setElevatorPositionCommand(0),
201+
slapdownSuperstructure.setDownAndRunCommand(),
202+
intakeSubsystem.setIntakeVoltageCommand(6),
203+
transportSubsystem
204+
.setVoltageCommand(4.0)
205+
.until(transportSubsystem::atSensor)
206+
.andThen(transportSubsystem.setVoltageCommand(0.0))));
207+
//
208+
// operatorController.share().onTrue(transportSubsystem.setVoltageCommand(6.0).until(transportSubsystem::atSensor));
202209
double rpm = 10000;
203-
operatorController.options().whileTrue(Commands.parallel(elevatorSubsystem.setElevatorPositionCommand(Units.inchesToMeters(0)), shooterSubsystem.runVelocityCommand(rpm/60), Commands.sequence(Commands.waitUntil(shooterSubsystem::inTolerance), transportSubsystem.setVoltageCommand(12.0))));
210+
operatorController
211+
.options()
212+
.whileTrue(
213+
Commands.parallel(
214+
elevatorSubsystem.setElevatorPositionCommand(Units.inchesToMeters(0)),
215+
shooterSubsystem.runVelocityCommand(rpm / 60),
216+
Commands.sequence(
217+
Commands.waitUntil(shooterSubsystem::inTolerance),
218+
transportSubsystem.setVoltageCommand(12.0))));
204219
}
205220

206221
private void configureDriving() {

‎src/main/java/frc/robot/commands/intake/intakeSlapCommand.java‎

Lines changed: 7 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -4,18 +4,19 @@
44

55
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
66
import frc.robot.subsystems.intake.IntakeSubsystem;
7-
import frc.robot.subsystems.slapdown.SlapdownSubsystem;
7+
import frc.robot.subsystems.slapdown.SlapdownSuperstructure;
88

99
public class intakeSlapCommand extends ParallelCommandGroup {
1010
private final IntakeSubsystem intakeSubsystem;
11-
private final SlapdownSubsystem slapdownSubsystem;
11+
private final SlapdownSuperstructure slapdownSuperstructure;
1212

13-
public intakeSlapCommand(IntakeSubsystem intakeSubsystem, SlapdownSubsystem slapdownSubsystem) {
13+
public intakeSlapCommand(
14+
IntakeSubsystem intakeSubsystem, SlapdownSuperstructure slapdownSuperstructure) {
1415
this.intakeSubsystem = intakeSubsystem;
15-
this.slapdownSubsystem = slapdownSubsystem;
16+
this.slapdownSuperstructure = slapdownSuperstructure;
1617

1718
addCommands(
18-
intakeSubsystem.setIntakeVoltageCommand(INTAKE_VOLTAGE),
19-
slapdownSubsystem.setFeederVoltageCommand(5));
19+
intakeSubsystem.setIntakeVoltageCommand(INTAKE_VOLTAGE));
20+
// slapdownSuperstructure.setFeederVoltageCommand(5));
2021
}
2122
}

‎src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java‎

Lines changed: 34 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,7 @@
1010
import edu.wpi.first.math.trajectory.TrapezoidProfile;
1111
import edu.wpi.first.wpilibj.DigitalInput;
1212
import edu.wpi.first.wpilibj2.command.Command;
13+
import edu.wpi.first.wpilibj2.command.Commands;
1314
import edu.wpi.first.wpilibj2.command.SubsystemBase;
1415
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
1516
import frc.robot.Constants.MiscConstants;
@@ -21,6 +22,7 @@
2122
import frc.robot.utils.Alert;
2223
import frc.robot.utils.ConfigurationUtils;
2324
import frc.robot.utils.ConfigurationUtils.StringFaultRecorder;
25+
import frc.robot.utils.RaiderCommands;
2426

2527
public class ElevatorSubsystem extends SubsystemBase {
2628

@@ -38,7 +40,7 @@ public class ElevatorSubsystem extends SubsystemBase {
3840
private final TelemetryCANSparkMax elevatorMotor =
3941
new TelemetryCANSparkMax(ELEVATOR_MOTOR_ID, MotorType.kBrushless, "/elevator/motor", true);
4042

41-
private TunableTelemetryProfiledPIDController controller =
43+
private final TunableTelemetryProfiledPIDController controller =
4244
new TunableTelemetryProfiledPIDController(
4345
"elevator/controller", PID_GAINS, TRAPEZOIDAL_PROFILE_GAINS);
4446

@@ -55,6 +57,8 @@ public class ElevatorSubsystem extends SubsystemBase {
5557
private final EventTelemetryEntry elevatorEventEntry =
5658
new EventTelemetryEntry("/elevator/events");
5759

60+
private boolean isHomed = false;
61+
5862
public ElevatorSubsystem() {
5963
configMotors();
6064

@@ -117,10 +121,8 @@ private void configMotors() {
117121
elevatorAlert.set(faultRecorder.hasFault());
118122
}
119123

120-
public void atBottomLimit() {
121-
if (!bottomLimit.get()) {
122-
elevatorEncoder.setPosition(0);
123-
}
124+
public boolean atBottomLimit() {
125+
return !bottomLimit.get();
124126
}
125127

126128
public boolean atGoal() {
@@ -144,20 +146,31 @@ public void stopMove() {
144146
elevatorMotor.setVoltage(0);
145147
}
146148

147-
// TODO: Fix
149+
public boolean isHomed() {
150+
return isHomed;
151+
}
152+
148153
public Command setElevatorPositionCommand(double position) {
149-
return this.run(
150-
() -> {
151-
double feedbackOutput = controller.calculate(getPosition());
152-
TrapezoidProfile.State currentSetpoint = controller.getSetpoint();
154+
// If we are not homed, then do not try to run closed loop. that will end poorly
155+
return RaiderCommands.ifCondition(this::isHomed)
156+
.then(
157+
this.run(
158+
() -> {
159+
double feedbackOutput = controller.calculate(getPosition());
160+
TrapezoidProfile.State currentSetpoint = controller.getSetpoint();
161+
162+
setVoltage(feedbackOutput + feedforward.calculate(currentSetpoint.velocity));
163+
})
164+
.beforeStarting(
165+
() -> {
166+
controller.reset(getPosition(), elevatorEncoder.getVelocity());
167+
controller.setGoal(position);
168+
}))
169+
.otherwise(Commands.none());
170+
}
153171

154-
setVoltage(feedbackOutput + feedforward.calculate(currentSetpoint.velocity));
155-
})
156-
.beforeStarting(
157-
() -> {
158-
controller.reset(getPosition(), elevatorEncoder.getVelocity());
159-
controller.setGoal(position);
160-
});
172+
public Command probeHomeCommand() {
173+
return setVoltageCommand(-0.5).unless(this::isHomed).until(this::isHomed);
161174
}
162175

163176
public Command setVoltageCommand(double voltage) {
@@ -175,23 +188,19 @@ public Command sysIdDynamic(SysIdRoutine.Direction direction) {
175188
@Override
176189
public void periodic() {
177190
// TODO: make this or elegant
178-
if (!bottomLimit.get()) {
179-
elevatorEncoder.setPosition(0.0);
191+
if (atBottomLimit()) {
192+
isHomed = true;
193+
setEncoderPosition(0.0);
180194
}
181195
logValues();
182196
}
183197

184198
private void logValues() {
185199
elevatorMotor.logValues();
186-
bottomLimitEntry.append(!bottomLimit.get());
200+
bottomLimitEntry.append(atBottomLimit());
187201

188202
if (FF_GAINS.hasChanged()) {
189203
feedforward = FF_GAINS.createElevatorFeedforward();
190204
}
191-
if (PID_GAINS.hasChanged() || TRAPEZOIDAL_PROFILE_GAINS.hasChanged()) {
192-
controller =
193-
new TunableTelemetryProfiledPIDController(
194-
"elevator/controller", PID_GAINS, TRAPEZOIDAL_PROFILE_GAINS);
195-
}
196205
}
197206
}

‎src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java‎

Lines changed: 3 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -27,9 +27,7 @@ public class ShooterSubsystem extends SubsystemBase {
2727

2828
private final SysIdRoutine shooterSysId =
2929
new SysIdRoutine(
30-
new SysIdRoutine.Config(
31-
Volts.per(Second).of(0.5), Volts.of(10), Seconds.of(12), null
32-
),
30+
new SysIdRoutine.Config(Volts.per(Second).of(0.5), Volts.of(10), Seconds.of(12), null),
3331
new SysIdRoutine.Mechanism(
3432
(voltage) -> setFlyVoltage(voltage.in(Volts)),
3533
null, // No log consumer, since data is recorded by URCL
@@ -123,7 +121,8 @@ public double getVelocity() {
123121
}
124122

125123
public boolean inTolerance() {
126-
return Math.abs(getVelocity() - pidController.getSetpoint()) / (pidController.getSetpoint()) < 0.01;
124+
return Math.abs(getVelocity() - pidController.getSetpoint()) / (pidController.getSetpoint())
125+
< 0.01;
127126
}
128127

129128
public Command setVoltageCommand(double voltage) {

0 commit comments

Comments
 (0)