Skip to content

Commit 6db5e6b

Browse files
mwitcpalekmwitcpalek
committed
update elbow zero, add closed-loop forks, test climb sequence
1 parent 8568969 commit 6db5e6b

24 files changed

+366
-142
lines changed

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

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -108,6 +108,7 @@ public void disabledExit() {}
108108

109109
@Override
110110
public void autonomousInit() {
111+
if (!m_robotContainer.hasElbowZeroed()) m_robotContainer.zeroElbow();
111112
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
112113

113114
if (m_autonomousCommand != null) {
@@ -126,6 +127,9 @@ public void teleopInit() {
126127
if (m_autonomousCommand != null) {
127128
m_autonomousCommand.cancel();
128129
}
130+
if (!m_robotContainer.hasElbowZeroed()) {
131+
m_robotContainer.zeroElbow();
132+
}
129133
}
130134

131135
@Override

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

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

55
package frc.robot;
66

7+
import edu.wpi.first.math.geometry.Rotation2d;
78
import edu.wpi.first.networktables.GenericEntry;
89
import edu.wpi.first.wpilibj.DriverStation.Alliance;
910
import edu.wpi.first.wpilibj.Joystick;
@@ -25,17 +26,24 @@
2526
import frc.robot.commands.drive.DriveAutonCommand;
2627
import frc.robot.commands.drive.DriveTeleopCommand;
2728
import frc.robot.commands.drive.ResetGyroCommand;
29+
import frc.robot.commands.drive.XLockCommand;
2830
import frc.robot.commands.elbow.ClosedLoopElbowCommand;
2931
import frc.robot.commands.elbow.HoldElbowCommand;
3032
import frc.robot.commands.elbow.JogElbowClosedLoopCommand;
33+
import frc.robot.commands.elbow.OpenLoopElbowCommand;
3134
import frc.robot.commands.elbow.ZeroElbowCommand;
3235
import frc.robot.commands.magazine.OpenLoopMagazineCommand;
36+
import frc.robot.commands.robotState.AmpCommand;
3337
import frc.robot.commands.robotState.ClimbCommand;
3438
import frc.robot.commands.robotState.DecendCommand;
39+
import frc.robot.commands.robotState.FullTrapClimbCommand;
40+
import frc.robot.commands.robotState.IntakeCommand;
3541
import frc.robot.commands.robotState.PostClimbStowCommand;
3642
import frc.robot.commands.robotState.PrepClimbCommand;
43+
import frc.robot.commands.robotState.ReleaseNoteCommand;
3744
import frc.robot.commands.robotState.ScoreTrapCommand;
3845
import frc.robot.commands.robotState.StowCommand;
46+
import frc.robot.commands.robotState.SubWooferCommand;
3947
import frc.robot.commands.robotState.TrapCommand;
4048
import frc.robot.commands.robotState.TunedShotCommand;
4149
import frc.robot.commands.robotState.TuningOffCommand;
@@ -64,6 +72,7 @@
6472
import frc.robot.subsystems.vision.VisionSubsystem;
6573
import frc.robot.subsystems.wrist.WristIOSRX;
6674
import frc.robot.subsystems.wrist.WristSubsystem;
75+
import java.util.Map;
6776
import org.strykeforce.telemetry.TelemetryController;
6877
import org.strykeforce.telemetry.TelemetryService;
6978

@@ -132,8 +141,8 @@ public RobotContainer() {
132141
// testAutonPath.generateTrajectory();
133142

134143
configureDriverBindings();
135-
// configureOperatorBindings();
136-
configureClimbTestBindings();
144+
configureOperatorBindings();
145+
// configureClimbTestBindings();
137146
// configureMatchDashboard();
138147
configurePitDashboard();
139148
configureTuningDashboard();
@@ -143,6 +152,14 @@ public RobotContainer() {
143152
// configurePitDashboard();
144153
}
145154

155+
public boolean hasElbowZeroed() {
156+
return elbowSubsystem.hasZeroed();
157+
}
158+
159+
public void zeroElbow() {
160+
elbowSubsystem.zero();
161+
}
162+
146163
public void configurePitDashboard() {
147164
Shuffleboard.getTab("Pit")
148165
.add(
@@ -208,6 +225,18 @@ public void configurePitDashboard() {
208225
climbSubsystem))
209226
.withSize(1, 1)
210227
.withPosition(6, 2);
228+
229+
Shuffleboard.getTab("Pit")
230+
.add(
231+
"Full Trap Sequence",
232+
new FullTrapClimbCommand(robotStateSubsystem, climbSubsystem, superStructure))
233+
.withSize(1, 1)
234+
.withPosition(7, 2);
235+
236+
Shuffleboard.getTab("Pit")
237+
.add("Zero Climb", new ZeroClimbCommand(climbSubsystem))
238+
.withSize(1, 1)
239+
.withPosition(0, 4);
211240
}
212241
/*
213242
private void configureMatchDashboard() {
@@ -269,7 +298,6 @@ public void configureTelemetry() {
269298
robotStateSubsystem.registerWith(telemetryService);
270299
telemetryService.start();
271300
}
272-
/*
273301

274302
public void setAllianceColor(Alliance alliance) {
275303
this.alliance = alliance;
@@ -307,7 +335,7 @@ private void configureOperatorBindings() {
307335
.onTrue(new OpenLoopElbowCommand(elbowSubsystem, -0.1))
308336
.onFalse(new OpenLoopElbowCommand(elbowSubsystem, 0));
309337

310-
// Open Loop Magazine
338+
// Amp Prep
311339
new JoystickButton(xboxController, XboxController.Button.kA.value)
312340
.onTrue(
313341
new AmpCommand(
@@ -318,10 +346,11 @@ private void configureOperatorBindings() {
318346

319347
// new JoystickButton(xboxController, XboxController.Button.kX.value).onTrue(testAutonPath);
320348

321-
new JoystickButton(xboxController, XboxController.Button.kY.value)
322-
.onTrue(
323-
new PodiumCommand(
324-
robotStateSubsystem, superStructure, magazineSubsystem, intakeSubsystem));
349+
// Podium Prep
350+
// new JoystickButton(xboxController, XboxController.Button.kY.value)
351+
// .onTrue(
352+
// new PodiumCommand(
353+
// robotStateSubsystem, superStructure, magazineSubsystem, intakeSubsystem));
325354

326355
// SubWoofer
327356
new JoystickButton(xboxController, XboxController.Button.kX.value)
@@ -332,31 +361,12 @@ private void configureOperatorBindings() {
332361
.onTrue(
333362
new StowCommand(
334363
robotStateSubsystem, superStructure, magazineSubsystem, intakeSubsystem));
335-
// // Amp Command
336-
// new JoystickButton(xboxController, XboxController.Button.kX.value)
337-
// .onTrue(new AmpCommand(robotStateSubsystem, superStructure, magazineSubsystem));
338-
339-
// // Intake Command
340-
// new JoystickButton(xboxController, XboxController.Button.kX.value)
341-
// .onTrue(
342-
// new IntakeCommand(
343-
// robotStateSubsystem, superStructure, magazineSubsystem, intakeSubsystem));
344-
345-
// // Open Loop Intake Command
346-
// new JoystickButton(xboxController, XboxController.Button.kA.value)
347-
// .onTrue(new OpenLoopIntakeCommand(intakeSubsystem, 50));
348-
349-
// new JoystickButton(xboxController, XboxController.Button.kB.value)
350-
// .onTrue(new OpenLoopIntakeCommand(intakeSubsystem, 0.0));
351-
352-
// // Stow Command
353-
// new JoystickButton(xboxController, XboxController.Button.kX.value)
354-
// .onTrue(
355-
// new StowCommand(
356-
// robotStateSubsystem, superStructure, magazineSubsystem, intakeSubsystem));
357-
}
358364

359-
*/
365+
// // Climb Prep
366+
// new JoystickButton(xboxController, XboxController.Button.kStart.value).onTrue(new
367+
// PrepClimbCommand(robotStateSubsystem, climbSubsystem, superStructure));
368+
369+
}
360370

361371
private void configureDriverBindings() {
362372
FlyskyJoystick flysky = new FlyskyJoystick(driveJoystick);
@@ -369,21 +379,9 @@ private void configureDriverBindings() {
369379
driveSubsystem,
370380
robotStateSubsystem));
371381

372-
// // Vision Shoot Command
373-
// new JoystickButton(driveJoystick, Button.SWD.id)
374-
// .onTrue(new VisionShootCommand(robotStateSubsystem, superStructure,
375-
// magazineSubsystem));
376-
377-
// // Stow Command
378-
// new JoystickButton(driveJoystick, Button.SWD.id)
379-
// .onTrue(
380-
// new StowCommand(
381-
// robotStateSubsystem, superStructure, magazineSubsystem, intakeSubsystem));
382-
383382
// Reset Gyro Command
384383
new JoystickButton(driveJoystick, Button.M_SWC.id).onTrue(new ResetGyroCommand(driveSubsystem));
385384

386-
/*
387385
// Intake
388386
new JoystickButton(driveJoystick, Button.SWB_DWN.id)
389387
.onTrue(
@@ -398,6 +396,7 @@ private void configureDriverBindings() {
398396
.onTrue(new XLockCommand(driveSubsystem))
399397
.onFalse(new XLockCommand(driveSubsystem));
400398

399+
// Stow Command
401400
new JoystickButton(driveJoystick, Button.SWA.id)
402401
.onTrue(
403402
new StowCommand(
@@ -406,31 +405,24 @@ private void configureDriverBindings() {
406405
new StowCommand(
407406
robotStateSubsystem, superStructure, magazineSubsystem, intakeSubsystem));
408407

409-
// Shoot
410-
new JoystickButton(driveJoystick, Button.M_SWH.id)
411-
.onTrue(new SubWooferCommand(robotStateSubsystem, superStructure, magazineSubsystem));
408+
// // Vision Shoot
409+
// new JoystickButton(driveJoystick, Button.M_SWH.id)
410+
// .onTrue(new VisionShootCommand(robotStateSubsystem, superStructure, magazineSubsystem,
411+
// intakeSubsystem));
412412

413413
// Release Game Piece Command
414414
new JoystickButton(driveJoystick, Button.M_SWE.id)
415415
.onTrue(new ReleaseNoteCommand(robotStateSubsystem, superStructure, magazineSubsystem));
416416

417+
// Subwoofer Shoot
417418
new JoystickButton(driveJoystick, Button.SWG_UP.id)
418-
.onTrue(
419-
new VisionShootCommand(
420-
robotStateSubsystem, superStructure, magazineSubsystem, intakeSubsystem));
419+
.onTrue(new SubWooferCommand(robotStateSubsystem, superStructure, magazineSubsystem));
421420
new JoystickButton(driveJoystick, Button.SWG_UP.id)
422-
.onFalse(
423-
new VisionShootCommand(
424-
robotStateSubsystem, superStructure, magazineSubsystem, intakeSubsystem));
421+
.onFalse(new SubWooferCommand(robotStateSubsystem, superStructure, magazineSubsystem));
425422
new JoystickButton(driveJoystick, Button.SWG_DWN.id)
426-
.onTrue(
427-
new VisionShootCommand(
428-
robotStateSubsystem, superStructure, magazineSubsystem, intakeSubsystem));
423+
.onTrue(new SubWooferCommand(robotStateSubsystem, superStructure, magazineSubsystem));
429424
new JoystickButton(driveJoystick, Button.SWG_DWN.id)
430-
.onFalse(
431-
new VisionShootCommand(
432-
robotStateSubsystem, superStructure, magazineSubsystem, intakeSubsystem));
433-
*/
425+
.onFalse(new SubWooferCommand(robotStateSubsystem, superStructure, magazineSubsystem));
434426
}
435427

436428
public void configureClimbTestBindings() {

‎src/main/java/frc/robot/commands/climb/ZeroClimbCommand.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,7 @@ public ZeroClimbCommand(ClimbSubsystem climbSubsystem) {
1515

1616
@Override
1717
public void initialize() {
18-
climbSubsystem.zero();
18+
climbSubsystem.zeroAll();
1919
}
2020

2121
@Override

‎src/main/java/frc/robot/commands/elbow/ZeroElbowCommand.java‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -21,4 +21,9 @@ public void initialize() {
2121
public boolean isFinished() {
2222
return elbowSubsystem.getState() != ElbowStates.ZEROING;
2323
}
24+
25+
@Override
26+
public boolean runsWhenDisabled() {
27+
return true;
28+
}
2429
}

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -20,7 +20,7 @@ public ClimbCommand(
2020

2121
@Override
2222
public void initialize() {
23-
robotStateSubsystem.climb();
23+
robotStateSubsystem.climb(false);
2424
}
2525

2626
@Override

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

Lines changed: 29 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
package frc.robot.commands.robotState;
2+
3+
import edu.wpi.first.wpilibj2.command.Command;
4+
import frc.robot.subsystems.climb.ClimbSubsystem;
5+
import frc.robot.subsystems.robotState.RobotStateSubsystem;
6+
import frc.robot.subsystems.robotState.RobotStateSubsystem.RobotStates;
7+
import frc.robot.subsystems.superStructure.SuperStructure;
8+
9+
public class FullTrapClimbCommand extends Command {
10+
private RobotStateSubsystem robotStateSubsystem;
11+
12+
public FullTrapClimbCommand(
13+
RobotStateSubsystem robotStateSubsystem,
14+
ClimbSubsystem climbSubsystem,
15+
SuperStructure superStructure) {
16+
this.robotStateSubsystem = robotStateSubsystem;
17+
addRequirements(climbSubsystem, superStructure);
18+
}
19+
20+
@Override
21+
public void initialize() {
22+
robotStateSubsystem.climb(true);
23+
}
24+
25+
@Override
26+
public boolean isFinished() {
27+
return robotStateSubsystem.getState() == RobotStates.TRAP;
28+
}
29+
}

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

Lines changed: 15 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -28,13 +28,15 @@ public final class ClimbConstants {
2828
public static final double kZeroStableCounts = 3;
2929
public static final double kZeroSpeedThreshold = 1;
3030

31-
public static final double kLeftForkZero = 0.0;
32-
public static final double kRightForkZero = 0.0;
31+
// Forks
32+
public static final double kZeroForkPct = -0.3;
33+
public static final double kZeroForkMaxVel = 1;
34+
public static final int kForkZeroStableCounts = 3;
3335
public static final double kCloseEnoughForks = 100;
34-
public static final double kLeftExtendPos = 100;
35-
public static final double kRightExtendPos = 100;
36-
public static final double kLeftRetractPos = 0.0;
37-
public static final double kRightRetractPos = 0.0;
36+
public static final double kLeftExtendPos = 500;
37+
public static final double kRightExtendPos = 500;
38+
public static final double kLeftRetractPos = 10;
39+
public static final double kRightRetractPos = 10;
3840

3941
// PRE-CLIMB
4042
public static final double kLeftClimbPrepPos = 70.0;
@@ -148,25 +150,25 @@ public static CurrentLimitsConfigs getRunCurrentLimit() {
148150
public static TalonSRXConfiguration getForkConfiguration() {
149151
TalonSRXConfiguration config = new TalonSRXConfiguration();
150152

151-
config.forwardSoftLimitThreshold = 0;
152-
config.forwardSoftLimitEnable = false; // fixme
153+
config.forwardSoftLimitThreshold = 510;
154+
config.forwardSoftLimitEnable = true; // fixme
153155

154156
config.reverseSoftLimitThreshold = 0.0;
155-
config.reverseSoftLimitEnable = false; // fixme
157+
config.reverseSoftLimitEnable = true; // fixme
156158

157159
config.continuousCurrentLimit = 2;
158160
config.peakCurrentLimit = 2;
159161
config.peakCurrentDuration = 100;
160162

161-
config.slot0.kP = 0.0;
163+
config.slot0.kP = 20.0;
162164
config.slot0.kI = 0.0;
163165
config.slot0.kD = 0.0;
164-
config.slot0.kF = 0.0;
166+
config.slot0.kF = 12.0;
165167
config.slot0.integralZone = 0;
166168
config.slot0.maxIntegralAccumulator = 0;
167169
config.slot0.allowableClosedloopError = 0;
168-
config.motionCruiseVelocity = 0;
169-
config.motionAcceleration = 0;
170+
config.motionCruiseVelocity = 75;
171+
config.motionAcceleration = 500;
170172
config.neutralDeadband = 0.01;
171173
config.velocityMeasurementWindow = 64;
172174
config.velocityMeasurementPeriod = SensorVelocityMeasPeriod.Period_100Ms;

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

Lines changed: 8 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -22,12 +22,15 @@ public final class ElbowConstants {
2222
public static final double kFxChain = 50.0 / 24.0;
2323

2424
public static final double kElbowTestPos = 0.0;
25+
public static final double kZeroPos = 25.0;
26+
public static final double kZeroOffset = 0.00276; // 1 degree = 0.00276
2527

28+
// Zero Recovery Constants
29+
public static final double kZeroVelocity = 0.05;
2630
public static final double kMinVelocityZeroing = 1;
2731
public static final int kMinStableZeroCounts = 5;
28-
public static final double kZeroVelocity = 0.05;
29-
public static final double kZeroPos = 25.0;
30-
public static final double kZeroOffset = 0.00276; // 1 degree = 0.00276
32+
public static final int kStableCountsAbsEncoder = 3;
33+
public static final double kCloseEnoughAbs = 0.0001;
3134

3235
public static CANcoderConfiguration getCanCoderConfig() {
3336
CANcoderConfiguration config = new CANcoderConfiguration();
@@ -46,10 +49,10 @@ public static TalonFXConfiguration getFxConfiguration() {
4649
config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true;
4750
config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = 34;
4851
config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true;
49-
config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = -60;
52+
config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = -100;
5053

5154
Slot0Configs slot0 = new Slot0Configs();
52-
slot0.kP = 1.2;
55+
slot0.kP = 2.0; // 1.2
5356
slot0.kI = 2.0;
5457
slot0.kD = 0.2;
5558
slot0.kS = 0.0;

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

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,7 @@ public final class IntakeConstants {
1515
public static final double kIntakePercentOutput = -0.5;
1616
public static final int kIntakeFxId = 20;
1717
public static final int kBeamBreakStableCounts = 2;
18+
public static final double kEjectPercent = 0.5;
1819

1920
public static TalonFXConfiguration getFXConfig() {
2021
TalonFXConfiguration fxConfig = new TalonFXConfiguration();

0 commit comments

Comments
 (0)