Skip to content

Commit e671bec

Browse files
Nins97Nins97
committed
slapdown and wrist stuff
1 parent d49bc6c commit e671bec

File tree

9 files changed

+83
-86
lines changed

9 files changed

+83
-86
lines changed

‎.run/Build & Deploy Robot for Debugging.run.xml‎

Lines changed: 1 addition & 24 deletions
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,7 @@
22
<configuration default="false" name="Build &amp; Deploy Robot for Debugging" type="GradleRunConfiguration" factoryName="Gradle">
33
<ExternalSystemSettings>
44
<option name="executionName" />
5-
<option name="externalProjectPath" value="$PROJECT_DIR$" />
5+
<option name="externalProjectPath" value="$PROJECT_DIR$/Competition2024" />
66
<option name="externalSystemIdString" value="GRADLE" />
77
<option name="scriptParameters" value="-PdebugMode=true" />
88
<option name="taskDescriptions">
@@ -18,29 +18,6 @@
1818
<ExternalSystemDebugServerProcess>true</ExternalSystemDebugServerProcess>
1919
<ExternalSystemReattachDebugProcess>true</ExternalSystemReattachDebugProcess>
2020
<DebugAllEnabled>false</DebugAllEnabled>
21-
<RunAsTest>false</RunAsTest>
22-
<method v="2" />
23-
</configuration>
24-
<configuration default="false" name="Build &amp; Deploy Robot for Debugging" type="GradleRunConfiguration" factoryName="Gradle">
25-
<ExternalSystemSettings>
26-
<option name="executionName" />
27-
<option name="externalProjectPath" value="$PROJECT_DIR$" />
28-
<option name="externalSystemIdString" value="GRADLE" />
29-
<option name="scriptParameters" value="-PdebugMode=true" />
30-
<option name="taskDescriptions">
31-
<list />
32-
</option>
33-
<option name="taskNames">
34-
<list>
35-
<option value="deploy" />
36-
</list>
37-
</option>
38-
<option name="vmOptions" />
39-
</ExternalSystemSettings>
40-
<ExternalSystemDebugServerProcess>true</ExternalSystemDebugServerProcess>
41-
<ExternalSystemReattachDebugProcess>true</ExternalSystemReattachDebugProcess>
42-
<DebugAllEnabled>false</DebugAllEnabled>
43-
<RunAsTest>false</RunAsTest>
4421
<method v="2" />
4522
</configuration>
4623
</component>

‎.run/Build Robot.run.xml‎

Lines changed: 1 addition & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -4,27 +4,7 @@
44
<option name="executionName" />
55
<option name="externalProjectPath" value="$PROJECT_DIR$" />
66
<option name="externalSystemIdString" value="GRADLE" />
7-
<option name="scriptParameters" value="build" />
8-
<option name="taskDescriptions">
9-
<list />
10-
</option>
11-
<option name="taskNames">
12-
<list />
13-
</option>
14-
<option name="vmOptions" />
15-
</ExternalSystemSettings>
16-
<ExternalSystemDebugServerProcess>true</ExternalSystemDebugServerProcess>
17-
<ExternalSystemReattachDebugProcess>true</ExternalSystemReattachDebugProcess>
18-
<DebugAllEnabled>false</DebugAllEnabled>
19-
<RunAsTest>false</RunAsTest>
20-
<method v="2" />
21-
</configuration>
22-
<configuration default="false" name="Build Robot" type="GradleRunConfiguration" factoryName="Gradle">
23-
<ExternalSystemSettings>
24-
<option name="executionName" />
25-
<option name="externalProjectPath" value="$PROJECT_DIR$" />
26-
<option name="externalSystemIdString" value="GRADLE" />
27-
<option name="scriptParameters" />
7+
<option name="scriptParameters" value="" />
288
<option name="taskDescriptions">
299
<list />
3010
</option>
@@ -38,7 +18,6 @@
3818
<ExternalSystemDebugServerProcess>true</ExternalSystemDebugServerProcess>
3919
<ExternalSystemReattachDebugProcess>true</ExternalSystemReattachDebugProcess>
4020
<DebugAllEnabled>false</DebugAllEnabled>
41-
<RunAsTest>false</RunAsTest>
4221
<method v="2" />
4322
</configuration>
4423
</component>

‎build.gradle‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -35,7 +35,7 @@ deploy {
3535
jvmArgs.add("-Dcom.sun.management.jmxremote.local.only=false")
3636
jvmArgs.add("-Dcom.sun.management.jmxremote.ssl=false")
3737
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
38+
jvmArgs.add("-Djava.rmi.server.hostname=10.37.29.2") // Replace XX.XX with team number
3939
}
4040

4141
// 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 = 117;
11-
public static final String GIT_SHA = "4bb8c2bf00761887ca8846cd361c7484a200594e";
12-
public static final String GIT_DATE = "2024-02-21 11:44:18 MST";
10+
public static final int GIT_REVISION = 118;
11+
public static final String GIT_SHA = "d49bc6cb82317d462eab6bcff46fa6a1d442efd4";
12+
public static final String GIT_DATE = "2024-02-21 14:54:22 MST";
1313
public static final String GIT_BRANCH = "workingongettingready";
14-
public static final String BUILD_DATE = "2024-02-21 14:46:36 MST";
15-
public static final long BUILD_UNIX_TIME = 1708551996489L;
14+
public static final String BUILD_DATE = "2024-02-21 19:00:25 MST";
15+
public static final long BUILD_UNIX_TIME = 1708567225933L;
1616
public static final int DIRTY = 1;
1717

1818
private BuildConstants(){}

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

Lines changed: 13 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -46,22 +46,21 @@ public static class SlapdownConstants {
4646
public static final int ROTATION_STALL_MOTOR_CURRENT = 20;
4747
public static final int ROTATION_FREE_MOTOR_CURRENT = 10;
4848

49-
public static final double ROTATION_UP_ANGLE = Units.degreesToRadians(-24.404689 - 90.0);
50-
public static final double ROTATION_DOWN_ANGLE =
51-
ROTATION_UP_ANGLE + Units.degreesToRadians(128.817943);
49+
public static final double ROTATION_UP_ANGLE = -0.0648 - Units.degreesToRadians(90);
50+
public static final double ROTATION_DOWN_ANGLE = 2.3 - Units.degreesToRadians(90);
5251

5352
public static final int ROTATION_LIMIT_SWITCH_ID = 1;
5453

5554
public static final double FEEDER_VOLTAGE = 6.0;
5655

5756
public static final TunablePIDGains ROTATION_GAINS =
58-
new TunablePIDGains("/slapdown/rotation/gains", 0, 0, 0, MiscConstants.TUNING_MODE);
57+
new TunablePIDGains("/slapdown/rotation/gains", 4.0, 0, 0, MiscConstants.TUNING_MODE);
5958
public static final TunableTrapezoidalProfileGains ROTATION_TRAP_GAINS =
6059
new TunableTrapezoidalProfileGains(
61-
"/slapdown/rotation/trapGains", 0, 0, MiscConstants.TUNING_MODE);
60+
"/slapdown/rotation/trapGains", 25, 40, MiscConstants.TUNING_MODE);
6261
public static final TunableArmElevatorFFGains ROTATION_FF_GAINS =
6362
new TunableArmElevatorFFGains(
64-
"/slapdown/rotation/FFGains", 0, 0, 0, 0, MiscConstants.TUNING_MODE);
63+
"/slapdown/rotation/FFGains", 0.10403, 0.17546, 0.61704, 0.084257, MiscConstants.TUNING_MODE);
6564
}
6665

6766
public static class ElevatorConstants {
@@ -84,12 +83,12 @@ public static class ElevatorConstants {
8483

8584
// TODO: Do These PID GAINS
8685
public static final TunablePIDGains PID_GAINS =
87-
new TunablePIDGains("gains/elevator", 57, 0, 0, MiscConstants.TUNING_MODE);
86+
new TunablePIDGains("gains/elevator", 20.0, 0, 0, MiscConstants.TUNING_MODE);
8887
public static final TunableTrapezoidalProfileGains TRAPEZOIDAL_PROFILE_GAINS =
8988
new TunableTrapezoidalProfileGains(
9089
"/gains/extension",
91-
Units.inchesToMeters(25),
92-
Units.inchesToMeters(100),
90+
Units.inchesToMeters(28),
91+
Units.inchesToMeters(125),
9392
MiscConstants.TUNING_MODE);
9493

9594
// TODO: TUNE FF GAINS
@@ -213,9 +212,9 @@ public static class WristConstants {
213212

214213
// TODO FIND WRIST CLAMPS
215214
public static final Rotation2d WRIST_MAX = new Rotation2d(Units.degreesToRadians(60));
216-
public static final Rotation2d WRIST_MIN = new Rotation2d(0);
215+
public static final Rotation2d WRIST_MIN = Rotation2d.fromRadians(-0.027);
217216

218-
public static final double WRIST_OFFSET = 0.0;
217+
public static final double WRIST_OFFSET = -0.288 + Units.degreesToRadians(90.0);
219218
public static final int WRIST_ENCODER_PORT = 2;
220219

221220
public static final int WRIST_MOTOR_ID = 2;
@@ -226,12 +225,12 @@ public static class WristConstants {
226225
// TODO: TUNE PID & TRAP & FF
227226
public static final TunableArmElevatorFFGains WRIST_FF_GAINS =
228227
new TunableArmElevatorFFGains(
229-
"/wrist/ffGains", 0.078, 0.292, 1.0951, 0.261, MiscConstants.TUNING_MODE);
228+
"/wrist/ffGains", .16624, 0.35068, 1.0512, 0.2769, MiscConstants.TUNING_MODE);
230229
// public static final TunablePIDGains WRIST_PID_GAINS =
231230
// new TunablePIDGains("/wrist/pidGains", 56.599, 0, 4.8897, MiscConstants.TUNING_MODE);
232231
//
233232
public static final TunablePIDGains WRIST_PID_GAINS =
234-
new TunablePIDGains("/wrist/pidGains", 0, 0, 0, MiscConstants.TUNING_MODE);
233+
new TunablePIDGains("/wrist/pidGains", 2, 0, 0, MiscConstants.TUNING_MODE);
235234

236235
public static final TunableTrapezoidalProfileGains TRAPEZOIDAL_PROFILE_GAINS =
237236
new TunableTrapezoidalProfileGains(
@@ -246,7 +245,7 @@ public static class TransportConstants {
246245
public static final boolean INVERTED = false;
247246
public static final int STALL_MOTOR_CURRENT = 45;
248247
public static final int FREE_MOTOR_CURRENT = 25;
249-
public static final double TRANSPORT_VOLTAGE = 2.0;
248+
public static final double TRANSPORT_VOLTAGE = 6.0;
250249
public static final int SHOOTER_SENSOR_ID = 8;
251250
}
252251

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

Lines changed: 20 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -77,6 +77,14 @@ public RobotContainer() {
7777
}
7878

7979
private void configureAutos() {
80+
autoCommand.addOption("Slapdown Q Forward", slapdownSuperstructure.getSlapdownRotationSubsystem().sysIdQuasistatic(SysIdRoutine.Direction.kForward));
81+
autoCommand.addOption("Slapdown Q Back", slapdownSuperstructure.getSlapdownRotationSubsystem().sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
82+
autoCommand.addOption("Slapdown D Forward", slapdownSuperstructure.getSlapdownRotationSubsystem().sysIdDynamic(SysIdRoutine.Direction.kForward));
83+
autoCommand.addOption("Slapdown D Back", slapdownSuperstructure.getSlapdownRotationSubsystem().sysIdDynamic(SysIdRoutine.Direction.kReverse));
84+
autoCommand.addOption("slapdown bottom", slapdownSuperstructure.setDownAndRunCommand());
85+
autoCommand.addOption("slapdown top", slapdownSuperstructure.setUpCommand());
86+
87+
autoCommand.addOption("Probe Elevator", elevatorSubsystem.probeHomeCommand());
8088
autoCommand.addOption(
8189
"Wrist Q Forward", wristSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
8290
autoCommand.addOption(
@@ -87,7 +95,7 @@ private void configureAutos() {
8795
"Wrist D Back", wristSubsystem.sysIdDynamic(SysIdRoutine.Direction.kReverse));
8896
autoCommand.addOption(
8997
"Wrist 5 deg",
90-
wristSubsystem.setPositonCommand(new Rotation2d(Units.degreesToRadians(40))));
98+
wristSubsystem.setPositonCommand(new Rotation2d(Units.degreesToRadians(5))));
9199
autoCommand.addOption(
92100
"Elevator Quastatic Backward",
93101
elevatorSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
@@ -113,6 +121,10 @@ private void configureAutos() {
113121
autoCommand.addOption(
114122
"Shooter Quastatic Forward Command",
115123
shooterSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
124+
autoCommand.addOption("WristElevatorZero", Commands.parallel(
125+
wristSubsystem.setPositonCommand(Constants.WristConstants.WRIST_MIN), elevatorSubsystem.setElevatorPositionCommand(0.0)));
126+
autoCommand.addOption("WristElevatorNot", Commands.parallel(wristSubsystem.setPositonCommand(Rotation2d.fromDegrees(40.0)), elevatorSubsystem.setElevatorPositionCommand(Units.inchesToMeters(8.0))));
127+
116128
autoCommand.addOption(
117129
"Shooter Quastatic Backward Command",
118130
shooterSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
@@ -136,7 +148,7 @@ private void configureAutos() {
136148

137149
private void configureDriverBindings() {
138150
configureDriving();
139-
151+
driverController.leftTrigger().onTrue(transportSubsystem.setVoltageCommand(Constants.TransportConstants.TRANSPORT_VOLTAGE).until(()->!transportSubsystem.atSensor()).andThen(transportSubsystem.setVoltageCommand(0)));
140152
driverController
141153
.home()
142154
.onTrue(
@@ -158,9 +170,14 @@ private void configureDriverBindings() {
158170
.povUp()
159171
.toggleOnTrue(slapdownSuperstructure.setUpCommand()); // TODO: THIS
160172
driverController.a().onTrue(intakeSubsystem.checkIntakeCommand());
173+
Command intakeAndFeedUntilDone = Commands.parallel(intakeSubsystem.setIntakeVoltageCommand(Constants.IntakeConstants.INTAKE_VOLTAGE), transportSubsystem.setVoltageCommand(Constants.TransportConstants.TRANSPORT_VOLTAGE)).until(transportSubsystem::atSensor).unless(transportSubsystem::atSensor);
174+
driverController.leftBumper().whileTrue(Commands.parallel(slapdownSuperstructure.setDownAndRunCommand(), intakeAndFeedUntilDone.asProxy(), elevatorSubsystem.setElevatorPositionCommand(0), wristSubsystem.setPositonCommand(new Rotation2d(0))));
175+
driverController.leftBumper().onFalse(slapdownSuperstructure.setUpCommand());
161176
}
162177

163178
private void configureOperatorBindings() {
179+
operatorController.triangle().onTrue(Commands.sequence(shooterSubsystem.runVelocityCommand(10000.0/60)));
180+
operatorController.x().onTrue(shooterSubsystem.setVoltageCommand(0));
164181
operatorController
165182
.leftTrigger()
166183
.onTrue(
@@ -204,6 +221,7 @@ private void configureOperatorBindings() {
204221
.setVoltageCommand(4.0)
205222
.until(transportSubsystem::atSensor)
206223
.andThen(transportSubsystem.setVoltageCommand(0.0))));
224+
207225
//
208226
// operatorController.share().onTrue(transportSubsystem.setVoltageCommand(6.0).until(transportSubsystem::atSensor));
209227
double rpm = 10000;

‎src/main/java/frc/robot/subsystems/slapdown/SlapdownRotationSubsystem.java‎

Lines changed: 19 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
package frc.robot.subsystems.slapdown;
22

3-
import static edu.wpi.first.units.Units.Volts;
3+
import static edu.wpi.first.units.Units.*;
44
import static frc.robot.Constants.SlapdownConstants.*;
55

66
import com.revrobotics.CANSparkBase.IdleMode;
@@ -16,6 +16,8 @@
1616
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
1717
import frc.robot.Constants;
1818
import frc.robot.telemetry.tunable.TunableTelemetryProfiledPIDController;
19+
import frc.robot.telemetry.types.BooleanTelemetryEntry;
20+
import frc.robot.telemetry.types.DoubleTelemetryEntry;
1921
import frc.robot.telemetry.types.EventTelemetryEntry;
2022
import frc.robot.telemetry.wrappers.TelemetryCANSparkMax;
2123
import frc.robot.utils.Alert;
@@ -37,13 +39,17 @@ public class SlapdownRotationSubsystem extends SubsystemBase {
3739

3840
private final SysIdRoutine slapdownRotationSysId =
3941
new SysIdRoutine(
40-
new SysIdRoutine.Config(),
42+
new SysIdRoutine.Config(Volts.per(Second).of(0.5), Volts.of(2), Seconds.of(5), null),
4143
new SysIdRoutine.Mechanism(
4244
(voltage) -> setRotationVoltage(voltage.in(Volts)),
4345
null, // No log consumer, since data is recorded by URCL
4446
this));
4547

4648
private final ArmFeedforward rotationFF = ROTATION_FF_GAINS.createArmFeedforward();
49+
private final BooleanTelemetryEntry atLimitEntry = new BooleanTelemetryEntry("/slapdown/rotation/atLimit", true);
50+
51+
private final BooleanTelemetryEntry isHomedEntry = new BooleanTelemetryEntry("/slapdown/rotation/isHomed", true);
52+
private final DoubleTelemetryEntry voltageRequestEntry = new DoubleTelemetryEntry("/slapdown/rotation/voltageReq", Constants.MiscConstants.TUNING_MODE);
4753
private final TunableTelemetryProfiledPIDController rotationController =
4854
new TunableTelemetryProfiledPIDController(
4955
"/slapdown/rotation/controller", ROTATION_GAINS, ROTATION_TRAP_GAINS);
@@ -123,6 +129,7 @@ private void configMotors() {
123129

124130
public void setRotationVoltage(double voltage) {
125131
rotationMotor.setVoltage(voltage);
132+
voltageRequestEntry.append(voltage);
126133
}
127134

128135
private double getPosition() {
@@ -141,13 +148,17 @@ public Command setRotationGoalCommand(Rotation2d goal) {
141148
return RaiderCommands.ifCondition(this::isHomed).then(
142149
this.run(
143150
() -> {
144-
rotationController.setGoal(goal.getRadians());
145151
double feedbackOutput = rotationController.calculate(getPosition());
146152
TrapezoidProfile.State currentSetpoint = rotationController.getSetpoint();
147153

148154
setRotationVoltage(
149-
feedbackOutput + rotationFF.calculate(getPosition(), currentSetpoint.velocity));
150-
})
155+
feedbackOutput + rotationFF.calculate(currentSetpoint.position, currentSetpoint.velocity));
156+
}).beforeStarting(
157+
() -> {
158+
rotationController.setGoal(goal.getRadians());
159+
rotationController.reset(getPosition(), rotationEncoder.getVelocity());
160+
}
161+
)
151162
).otherwise(Commands.none());
152163
}
153164

@@ -172,7 +183,10 @@ public void periodic() {
172183
if (atLimit()) {
173184
rotationEncoder.setPosition(ROTATION_UP_ANGLE);
174185
isHomed = true;
186+
175187
}
188+
isHomedEntry.append(isHomed);
189+
atLimitEntry.append(atLimit());
176190

177191
rotationMotor.logValues();
178192
}

‎src/main/java/frc/robot/subsystems/slapdown/SlapdownSuperstructure.java‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,9 @@ public Command setDownAndRunCommand() {
1616
feederSubsystem.setVoltageCommand(FEEDER_VOLTAGE),
1717
rotationSubsystem.setRotationGoalCommand(Rotation2d.fromRadians(ROTATION_DOWN_ANGLE)));
1818
}
19+
public SlapdownRotationSubsystem getSlapdownRotationSubsystem(){
20+
return rotationSubsystem;
21+
}
1922

2023
public Command setUpCommand() {
2124
return Commands.parallel(

0 commit comments

Comments
 (0)