Skip to content

Commit 7640ec6

Browse files
mwitcpalekmwitcpalek
authored
Merge pull request #91 from strykeforce/profiledPIDAutoPlace
New Auto Place Strategy
2 parents ecbb17d + 838d883 commit 7640ec6

File tree

10 files changed

+282
-97
lines changed

10 files changed

+282
-97
lines changed

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

Lines changed: 18 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -187,7 +187,7 @@ public static final class DriveConstants {
187187

188188
public static final double kDriveGearRatio =
189189
(kDriveMotorOutputGear / kDriveInputGear) * (kBevelInputGear / kBevelOutputGear);
190-
public static double kMaxSpeedToAutoDrive = 1.5; // FIXME WRoNG VAL
190+
public static double kMaxSpeedToAutoDrive = 1.0; // FIXME WRoNG VAL 1.5
191191
public static double kPathErrorThreshold = 0.04; // FIXME WRONG VAL 0.03
192192
public static double kPathErrorOmegaThresholdDegrees = 5; // FIXME WRONG VAL
193193

@@ -222,8 +222,8 @@ public static Translation2d[] getWheelLocationMeters() {
222222
public static final double kAutoBalanceSlowDriveVel = 0.6; // 0.5
223223
public static final double kAutoBalanceFinalDriveVel = 1.0; // 0.5 0.75
224224
public static final double kAutoBalanceSlowdownTimeSec = 1.15; // 2.3 1.6 1.3
225-
public static final double kAutoBalanceStopThresholdDegrees = 2.0; // 1 0.6 1.5
226-
public static final double kAutoBalanceEdgeTriggerThreshold = 5; // 5
225+
public static final double kAutoBalanceStopThresholdDegrees = 1.5; // 1 0.6 1.5
226+
public static final double kAutoBalanceEdgeTriggerThreshold = 3; // 5
227227
public static final double kAutoBalanceAvgRollCount = 7; // 5 10 7
228228
public static final double kAutoBalanceLoopFixTimer = 0.140;
229229

@@ -297,16 +297,25 @@ public static TalonFXConfiguration getDriveTalonConfig() {
297297
public static final double kMaxAccelOmega = 5.0; // 3.14
298298

299299
// AUTODRIVE ProfiledPID Constants
300-
public static final double kPAutoDrive = 3.0; // 1
300+
public static final double kPAutoDrive = 3; // 3
301301
public static final double kIAutoDrive = 0.0000;
302302
public static final double kDAutoDrive = 0.00; // kPHolonomic/100
303303

304-
public static final double kAutoDriveMaxVelocity = 2;
304+
public static final double kAutoDriveMaxVelocity = 2; //
305305
public static final double kAutoDriveMaxAccel = 2;
306306
// Default safety path constants
307307
public static final Pose2d startPose2d = new Pose2d(0, 0, Rotation2d.fromDegrees(0));
308308
public static final Pose2d endPose2d = new Pose2d(-1, 0, Rotation2d.fromDegrees(0));
309309

310+
public static final double kAutoDriveAutoYawMax = 30;
311+
public static final double kMaxSpeedForCamUpdate =
312+
0.75; // Max Speed(MPS) The robot can be at while the camera updates to autodrive.
313+
public static final double kPastBumpIndicateX = 12;
314+
public static final double kArmToAutoDriveDelaySec =
315+
0; // time delay between arm and autodrive starting
316+
317+
public static final double kAutoDriveAutoYawCloseEnough = 1;
318+
310319
public static ArrayList<Translation2d> getDefaultInternalWaypoints() {
311320
ArrayList<Translation2d> waypoints = new ArrayList<>();
312321
waypoints.add(new Translation2d(-0.5, 0));
@@ -348,7 +357,9 @@ public static final class VisionConstants {
348357

349358
public static final double kCameraOffset = .335; // was .273 on driveChasis
350359
public static final double kCameraAngleOffset = 0; // DEGREES was 24 on driveChasis
351-
public static final double kLastUpdateCloseEnoughThreshold = 1; // IN SECONDS
360+
public static final double kLastUpdateCloseEnoughThreshold = 2.0; // IN SECONDS
361+
public static final double kLastUpdateCloseEnoughThresholdYaw = 1.0;
362+
public static final double kDifferenceCloseEnoughThreshold = .1;
352363
public static int kBufferLookupOffset = 2;
353364

354365
public static Matrix<N3, N1> kStateStdDevs =
@@ -398,8 +409,8 @@ public static class ElevatorConstants {
398409
public static final double kLevelThreeCubeElevator = kLevelThreeConeElevator;
399410
public static final double kShelfElevator = -2_000; // -18_606
400411
public static final double kShelfExitElevator = kShelfElevator + 4000; // +3000
401-
public static final double kAutoHighCubeElevator = -2_000;
402412
public static final double kShelfMinimumShelfPosition = -43_000;
413+
public static final double kAutoHighCubeElevator = -2_000;
403414
public static final double kAutoLevelTwoConeElevator = -23_674;
404415
public static final double kAutoLevelThreeConeElevator = -1_500;
405416
public static final double kAutoLevelTwoCubeElevator = -2_000;

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

Lines changed: 21 additions & 60 deletions
Original file line numberDiff line numberDiff line change
@@ -17,7 +17,6 @@
1717
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
1818
import edu.wpi.first.wpilibj2.command.button.Trigger;
1919
import frc.robot.Constants.IntakeConstants;
20-
import frc.robot.commands.RGBlights.RGBsetPieceCommand;
2120
import frc.robot.commands.auto.AutoCommandInterface;
2221
import frc.robot.commands.auto.TestBalanceCommand;
2322
import frc.robot.commands.drive.DriveTeleopCommand;
@@ -27,7 +26,6 @@
2726
import frc.robot.commands.drive.xLockCommand;
2827
import frc.robot.commands.elbow.ElbowHoldPosCommand;
2928
import frc.robot.commands.elbow.JogElbowCommand;
30-
import frc.robot.commands.elbow.ZeroElbowCommand;
3129
import frc.robot.commands.elevator.AdjustElevatorCommand;
3230
import frc.robot.commands.elevator.ElevatorSpeedCommand;
3331
import frc.robot.commands.elevator.HoldPositionCommand;
@@ -36,13 +34,15 @@
3634
import frc.robot.commands.hand.GrabCubeCommand;
3735
import frc.robot.commands.hand.HandLeftSpeedCommand;
3836
import frc.robot.commands.hand.HandLeftToPositionCommand;
37+
import frc.robot.commands.hand.ReleaseGamepieceCommand;
3938
import frc.robot.commands.hand.ToggleHandCommand;
4039
import frc.robot.commands.hand.ZeroHandCommand;
4140
import frc.robot.commands.intake.IntakeExtendCommand;
4241
import frc.robot.commands.intake.IntakeOpenLoopCommand;
4342
import frc.robot.commands.robotState.AutoPlaceCommand;
4443
import frc.robot.commands.robotState.FloorPickupCommand;
4544
import frc.robot.commands.robotState.ManualScoreCommand;
45+
import frc.robot.commands.robotState.RecoverGamepieceCommand;
4646
import frc.robot.commands.robotState.RetrieveGamePieceCommand;
4747
import frc.robot.commands.robotState.SetGamePieceCommand;
4848
import frc.robot.commands.robotState.SetLevelAndColCommandGroup;
@@ -158,7 +158,7 @@ public RobotContainer() {
158158
driveSubsystem.setRobotStateSubsystem(robotStateSubsystem);
159159

160160
driveSubsystem.setVisionSubsystem(visionSubsystem);
161-
visionSubsystem.setFillBuffers(false); // FIXME TRUE
161+
visionSubsystem.setFillBuffers(true); // FIXME TRUE
162162

163163
// FIX ME
164164
robotStateSubsystem.setAllianceColor(Alliance.Blue);
@@ -318,71 +318,26 @@ private void configureDriverButtonBindings() {
318318
.onTrue(
319319
new AutoPlaceCommand(driveSubsystem, robotStateSubsystem, armSubsystem, handSubsystem))
320320
.onFalse(new InterruptDriveCommand(driveSubsystem));
321-
// .onTrue(new DriveToPlaceNotPathCommand(driveSubsystem, robotStateSubsystem));
322-
// new JoystickButton(driveJoystick, InterlinkButton.X.id)
323-
// .onTrue(new xLockCommand(driveSubsystem));
324-
new JoystickButton(driveJoystick, InterlinkButton.X.id)
325-
.onTrue(new ZeroElbowCommand(elbowSubsystem));
326-
// new JoystickButton(driveJoystick, InterlinkButton.HAMBURGER.id)
327-
// .onTrue(new ResetOdometryCommand(driveSubsystem, robotStateSubsystem));
328-
// new JoystickButton(driveJoystick, InterlinkButton.HAMBURGER.id).onTrue(threePiecePath);
321+
// new JoystickButton(driveJoystick, Trim.RIGHT_X_POS.id).onTrue(balancepath); // TESTING AT
322+
// LAKEVIEW PRACTICE FIELD
329323

330-
// new JoystickButton(driveJoystick, Trim.RIGHT_X_POS.id) // 3578
331-
// .onTrue(
332-
// new AutoPlaceCommandGroup(
333-
// driveSubsystem, robotStateSubsystem, armSubsystem, handSubsystem));
334-
// new JoystickButton(driveJoystick, Trim.RIGHT_X_POS.id)
335-
// .onTrue(new AutoBalanceCommand(false, driveSubsystem, robotStateSubsystem));
336-
// new JoystickButton(driveJoystick, Trim.RIGHT_X_POS.id)
337-
// .onTrue(new AutoBalanceCommand(false, driveSubsystem, robotStateSubsystem));
338-
new JoystickButton(driveJoystick, Trim.RIGHT_X_POS.id).onTrue(balancepath);
339-
// .onTrue(new DriveToPlaceNotPathCommand(driveSubsystem, robotStateSubsystem));
340-
new JoystickButton(driveJoystick, InterlinkButton.X.id)
341-
.onTrue(new ZeroElbowCommand(elbowSubsystem));
342-
// new JoystickButton(driveJoystick, InterlinkButton.HAMBURGER.id)
343-
// .onTrue(new ResetOdometryCommand(driveSubsystem, robotStateSubsystem));
344-
// new JoystickButton(driveJoystick, InterlinkButton.HAMBURGER.id).onTrue(threePiecePath);
324+
// new JoystickButton(driveJoystick, InterlinkButton.X.id)
325+
// .onTrue(new ZeroElbowCommand(elbowSubsystem));
345326

346-
// new JoystickButton(driveJoystick, Trim.RIGHT_X_POS.id) // 3578
347-
// .onTrue(
348-
// new AutoPlaceCommandGroup(
349-
// driveSubsystem, robotStateSubsystem, armSubsystem, handSubsystem));
350-
// new JoystickButton(driveJoystick, Trim.RIGHT_X_POS.id)
351-
// .onTrue(new AutoBalanceCommand(false, driveSubsystem, robotStateSubsystem));
352-
// new JoystickButton(driveJoystick, Trim.RIGHT_X_POS.id)
353-
// .onTrue(new AutoBalanceCommand(false, driveSubsystem, robotStateSubsystem));
354-
new JoystickButton(driveJoystick, Trim.RIGHT_X_POS.id).onTrue(balancepath);
355-
// .onTrue(new DriveToPlaceNotPathCommand(driveSubsystem, robotStateSubsystem));
356327
new JoystickButton(driveJoystick, InterlinkButton.X.id)
357328
.onTrue(new xLockCommand(driveSubsystem));
358-
// new JoystickButton(driveJoystick, InterlinkButton.HAMBURGER.id)
359-
// .onTrue(new ResetOdometryCommand(driveSubsystem, robotStateSubsystem));
360-
// new JoystickButton(driveJoystick, InterlinkButton.HAMBURGER.id).onTrue(threePiecePath);
361-
362-
// new JoystickButton(driveJoystick, InterlinkButton.HAMBURGER.id)
363-
// .onTrue(new DriveAutonCommand(driveSubsystem, "straightPathX", true, true));
364-
// Requires swerve migration to new Pose2D
365-
// new JoystickButton(joystick, InterlinkButton.HAMBURGER.id).whenPressed(() ->
366-
// {driveSubsystem.resetOdometry(new Pose2d());},driveSubsystem);
367-
// new JoystickButton(driveJoystick, InterlinkButton.HAMBURGER.id)
368-
// .onTrue(twoPieceAutoPlacePathCommandGroup)
369-
// .onTrue(
370-
// new InstantCommand(() -> robotStateSubsystem.setAutoMode(true),
371-
// robotStateSubsystem));
372-
// new JoystickButton(driveJoystick, InterlinkButton.HAMBURGER.id)
373-
// .onTrue(new ShootGamepieceCommand(handSubsystem, robotStateSubsystem));
374329

375330
// Hand
376331
/*new JoystickButton(driveJoystick, Shoulder.LEFT_DOWN.id)
377332
.onTrue(
378333
new HandToPositionCommand(handSubsystem, Constants.HandConstants.kCubeGrabbingPosition))
379334
.onFalse(new HandToPositionCommand(handSubsystem, Constants.HandConstants.kMaxRev));*/
380335
new JoystickButton(driveJoystick, Shoulder.LEFT_DOWN.id)
381-
.onTrue(new ToggleHandCommand(handSubsystem, robotStateSubsystem, armSubsystem))
382-
.onFalse(new ToggleHandCommand(handSubsystem, robotStateSubsystem, armSubsystem));
336+
.onTrue(new ReleaseGamepieceCommand(handSubsystem, robotStateSubsystem, armSubsystem))
337+
.onFalse(new ReleaseGamepieceCommand(handSubsystem, robotStateSubsystem, armSubsystem));
383338
new JoystickButton(driveJoystick, Shoulder.LEFT_UP.id)
384-
.onTrue(new ToggleHandCommand(handSubsystem, robotStateSubsystem, armSubsystem))
385-
.onFalse(new ToggleHandCommand(handSubsystem, robotStateSubsystem, armSubsystem));
339+
.onTrue(new ReleaseGamepieceCommand(handSubsystem, robotStateSubsystem, armSubsystem))
340+
.onFalse(new ReleaseGamepieceCommand(handSubsystem, robotStateSubsystem, armSubsystem));
386341

387342
// // Shoulder
388343
// new JoystickButton(driveJoystick, Trim.LEFT_X_NEG.id)
@@ -487,6 +442,12 @@ public void configureOperatorButtonBindings() {
487442
new JoystickButton(xboxController, XboxController.Button.kLeftBumper.value)
488443
.onTrue(
489444
new SetLevelAndColCommandGroup(robotStateSubsystem, TargetLevel.MID, TargetCol.LEFT));
445+
new JoystickButton(xboxController, XboxController.Button.kStart.value)
446+
.onTrue(new RecoverGamepieceCommand(robotStateSubsystem, handSubsystem));
447+
new JoystickButton(xboxController, XboxController.Button.kBack.value)
448+
.onTrue(
449+
new StowRobotCommand(
450+
robotStateSubsystem, armSubsystem, intakeSubsystem, handSubsystem));
490451

491452
Trigger leftTrigger =
492453
new Trigger(() -> xboxController.getLeftTriggerAxis() >= 0.1)
@@ -528,11 +489,11 @@ public void configureOperatorButtonBindings() {
528489
new JoystickButton(xboxController, XboxController.Button.kB.value)
529490
.onTrue(new SetGamePieceCommand(robotStateSubsystem, GamePiece.NONE));
530491

531-
new JoystickButton(xboxController, XboxController.Button.kBack.value)
532-
.onTrue(new RGBsetPieceCommand(rgbLightsSubsystem, GamePiece.CUBE));
492+
// new JoystickButton(xboxController, XboxController.Button.kBack.value)
493+
// .onTrue(new RGBsetPieceCommand(rgbLightsSubsystem, GamePiece.CUBE));
533494

534-
new JoystickButton(xboxController, XboxController.Button.kStart.value)
535-
.onTrue(new RGBsetPieceCommand(rgbLightsSubsystem, GamePiece.CONE));
495+
// new JoystickButton(xboxController, XboxController.Button.kStart.value)
496+
// .onTrue(new RGBsetPieceCommand(rgbLightsSubsystem, GamePiece.CONE));
536497

537498
// Adjust elevator
538499
Trigger leftUp =

‎src/main/java/frc/robot/commands/drive/InterruptDriveCommand.java‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -2,10 +2,18 @@
22

33
import edu.wpi.first.wpilibj2.command.InstantCommand;
44
import frc.robot.subsystems.DriveSubsystem;
5+
import org.slf4j.Logger;
6+
import org.slf4j.LoggerFactory;
57

68
public class InterruptDriveCommand extends InstantCommand {
9+
private Logger logger = LoggerFactory.getLogger(InterruptDriveCommand.class);
710

811
public InterruptDriveCommand(DriveSubsystem driveSubsystem) {
912
addRequirements(driveSubsystem);
1013
}
14+
15+
@Override
16+
public void initialize() {
17+
logger.info("AutoDrive Deadman switch released.");
18+
}
1119
}

‎src/main/java/frc/robot/commands/hand/ReleaseGamepieceCommand.java‎

Lines changed: 30 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,30 @@
1+
package frc.robot.commands.hand;
2+
3+
import edu.wpi.first.wpilibj2.command.CommandBase;
4+
import frc.robot.subsystems.ArmSubsystem;
5+
import frc.robot.subsystems.HandSubsystem;
6+
import frc.robot.subsystems.RobotStateSubsystem;
7+
8+
public class ReleaseGamepieceCommand extends CommandBase {
9+
private HandSubsystem handSubsystem;
10+
private RobotStateSubsystem robotStateSubsystem;
11+
12+
public ReleaseGamepieceCommand(
13+
HandSubsystem handSubsystem,
14+
RobotStateSubsystem robotStateSubsystem,
15+
ArmSubsystem armSubsystem) {
16+
this.handSubsystem = handSubsystem;
17+
this.robotStateSubsystem = robotStateSubsystem;
18+
addRequirements(handSubsystem, armSubsystem);
19+
}
20+
21+
@Override
22+
public void initialize() {
23+
robotStateSubsystem.toReleaseGamepiece();
24+
}
25+
26+
@Override
27+
public boolean isFinished() {
28+
return handSubsystem.isFinished();
29+
}
30+
}

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

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -58,5 +58,10 @@ public boolean isFinished() {
5858
public void end(boolean interrupted) {
5959
robotStateSubsystem.endAutoPlace(interrupted);
6060
driveSubsystem.setAutoDriving(false);
61+
if (driveSubsystem.currDriveState != DriveStates.AUTO_DRIVE_FINISHED
62+
&& driveSubsystem.currDriveState != DriveStates.AUTO_DRIVE_FAILED) {
63+
logger.info("Drive: {} -> AUTO_DRIVE_FINISHED", driveSubsystem.currDriveState);
64+
driveSubsystem.currDriveState = DriveStates.AUTO_DRIVE_FINISHED;
65+
}
6166
}
6267
}

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

Lines changed: 25 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,25 @@
1+
package frc.robot.commands.robotState;
2+
3+
import edu.wpi.first.wpilibj2.command.InstantCommand;
4+
import frc.robot.subsystems.HandSubsystem;
5+
import frc.robot.subsystems.RobotStateSubsystem;
6+
import org.slf4j.Logger;
7+
import org.slf4j.LoggerFactory;
8+
9+
public class RecoverGamepieceCommand extends InstantCommand {
10+
private RobotStateSubsystem robotStateSubsystem;
11+
private HandSubsystem handSubsystem;
12+
private static final Logger logger = LoggerFactory.getLogger(RecoverGamepieceCommand.class);
13+
14+
public RecoverGamepieceCommand(
15+
RobotStateSubsystem robotStateSubsystem, HandSubsystem handSubsystem) {
16+
this.robotStateSubsystem = robotStateSubsystem;
17+
addRequirements(handSubsystem);
18+
}
19+
20+
@Override
21+
public void initialize() {
22+
logger.info("Going to Recover Gamepiece");
23+
robotStateSubsystem.toRecoverGamepiece();
24+
}
25+
}

‎src/main/java/frc/robot/subsystems/ArmSubsystem.java‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -61,6 +61,10 @@ public void setReinforceElevator(boolean doReinforceElevator) {
6161
logger.info("Turn Elevator Reinforce: {}", doReinforceElevator);
6262
}
6363

64+
public void unReinforceElevator() {
65+
elevatorSubsystem.unReinforceElevator();
66+
}
67+
6468
public void toStowPos() {
6569
toStowPos(ArmState.STOW);
6670
}

0 commit comments

Comments
 (0)