Skip to content

Commit 0a1322d

Browse files
mwitcpalekmwitcpalek
authored
Merge pull request #111 from strykeforce/Quicker-Cube-Pickup-Auto
Auto Routine Speed-Up
2 parents 68f6dab + 3e3ec37 commit 0a1322d

37 files changed

+472
-144
lines changed

‎src/main/deploy/paths/pieceOneDeliverBumpCubeTwo.toml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -11,7 +11,7 @@ target_yaw = 0.0 #degrees
1111
angle = 180.0
1212

1313
[end_pose]
14-
x = 2.22 #2.32
14+
x = 2.22 #2.32->2.22->2.12
1515
y = 1.27 #1.4
1616
angle = 180.0
1717

‎src/main/deploy/paths/pieceOneFetchBumpCubeOneFallback.toml‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
max_velocity = 2.5 #m/s
1+
max_velocity = 2.75 #m/s
22
max_acceleration = 2.75 #m/s
33
start_velocity = 0.0 #m/s
44
end_velocity = 0.0 #m/s
@@ -11,7 +11,7 @@ target_yaw = 0.0 #degrees
1111
angle = 0.0
1212

1313
[end_pose]
14-
x = 7.07
14+
x = 7.27
1515
y = 1.07 #0.92->1.02
1616
angle = 0.0
1717

‎src/main/deploy/paths/pieceOneFetchBumpCubeTwo.toml‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
max_velocity = 3.25 #m/s 2.75
22
max_acceleration = 3.0 #m/s 2.75
33
start_velocity = 0.0 #m/s
4-
end_velocity = 0.6 #m/s
4+
end_velocity = 0.75 #m/s 0.6
55
is_reversed = false
66
target_yaw = 0.0 #degrees
77

@@ -12,7 +12,7 @@ target_yaw = 0.0 #degrees
1212

1313
[end_pose]
1414
x = 6.77 #7.00
15-
y = 2.34 #2.3
15+
y = 2.64 #2.3->2.34->2.44->2.54
1616
angle = 0.0
1717

1818
[[internal_points]]

‎src/main/deploy/paths/pieceOneFetchPath.toml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,7 @@ target_yaw = 0.0 #degrees
1212

1313
[end_pose]
1414
x = 7.0 #7
15-
y = 4.68 #4.58 ->4.48
15+
y = 4.58 #4.58 ->4.48
1616
angle = 0.0
1717

1818
[[internal_points]]

‎src/main/deploy/paths/pieceTwoFetchBumpCubeOne.toml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
max_velocity = 3.25 #m/s
22
max_acceleration = 3.25 #m/s
33
start_velocity = 0.0 #m/s
4-
end_velocity = 0.6 #m/s
4+
end_velocity = 0.75 #m/s 0.6
55
is_reversed = false
66
target_yaw = 0.0 #degrees
77

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

Lines changed: 19 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -82,9 +82,9 @@ public static final class RobotStateConstants {
8282
public static final Pose2d kCubeThreeAutoPickup =
8383
new Pose2d(new Translation2d(7.07, 3.36), new Rotation2d());
8484
public static final Pose2d kCubeTwoAutoPickup =
85-
new Pose2d(new Translation2d(6.9, 2.18), new Rotation2d());
85+
new Pose2d(new Translation2d(7.0, 2.18), new Rotation2d());
8686
public static final Pose2d kCubeOneAutoPickup =
87-
new Pose2d(new Translation2d(6.9, 0.92), new Rotation2d());
87+
new Pose2d(new Translation2d(7.0, 0.92), new Rotation2d());
8888

8989
public static final double kPolePlaceOffset = 0.56;
9090
public static final double kShelfOffset = 0.75;
@@ -242,6 +242,9 @@ public static Translation2d[] getWheelLocationMeters() {
242242
public static final double kHoldSpeed = 0.1;
243243
public static final double kSettleTime = 1.0;
244244

245+
public static final double kAutonBumpHighCubeYawOffsetDeg = -5.0;
246+
public static final double kAutonSmoothHighCubeYawOffsetDeg = 5.0;
247+
245248
public static TalonSRXConfiguration getAzimuthTalonConfig() {
246249
// constructor sets encoder to Quad/CTRE_MagEncoder_Relative
247250
TalonSRXConfiguration azimuthConfig = new TalonSRXConfiguration();
@@ -310,12 +313,18 @@ public static TalonFXConfiguration getDriveTalonConfig() {
310313
public static final double kPAutoDrive = 3; // 3
311314
public static final double kIAutoDrive = 0.0000;
312315
public static final double kDAutoDrive = 0.00; // kPHolonomic/100
316+
public static final double kAutoDriveMaxAccelOmega = 5.0;
317+
318+
public static final double kPAutoDriveOmega = 4.5;
319+
public static final double kIAutoDriveOmega = 0.0;
320+
public static final double kDAutoDriveOmega = 0.0;
313321

314322
public static final double kAutoDriveMaxVelocity = 2; //
315323
public static final double kAutoDriveMaxAccel = 2;
316324

317325
// Auto PickUp Constants
318-
public static final double kPAutoPickup = 3;
326+
public static final double kPAutoPickupY = 3.25; // 3.25
327+
public static final double kPAutoPickup = 2.75; // 3
319328
public static final double kIAutoPickup = 0.0;
320329
public static final double kDAutoPickup = 0.0;
321330

@@ -536,7 +545,7 @@ public static final class ElbowConstants { // FIXME needs real tick values
536545
public static final double kFloorElbowSweep = 16_876;
537546
public static final double kAutoLevelThreeCubeElbow = 65_497; // 64_297
538547
public static final double kAutoLevelTwoConeElbow = 60_097;
539-
public static final double kAutoLevelTwoCubeElbow = 36_500;
548+
public static final double kAutoLevelTwoCubeElbow = 37_700;
540549
public static final double kAutoLevelThreeConeElbow = 84_553;
541550

542551
public static final double kRetrieveGamepiecePercentOutput = 0.2;
@@ -551,8 +560,8 @@ public static final class ElbowConstants { // FIXME needs real tick values
551560
public static final double kStowToIntakeStageParallelAllowed = -10_000;
552561
public static final double kIntakeStageToIntakeParallelAllowed = kIntakeElbow;
553562

554-
public static final double kElbowTeleMotionCruiseVelocity = 12_000.0; // 13_000
555-
public static final double kElbowTeleMotionAcceleration = 35_000.0; // 38_000
563+
public static final double kElbowTeleMotionCruiseVelocity = 13_000.0; // 12_000
564+
public static final double kElbowTeleMotionAcceleration = 50_000.0; // 35_000
556565
public static final double kElbowAutoMotionCruiseVelocity = 13_000.0;
557566
public static final double kElbowAutoMotionAcceleration = 50_000.0;
558567

@@ -784,6 +793,7 @@ public static class HandConstants {
784793
public static final int kZeroStableCounts = 1592;
785794
public static final int kHasConeStableCounts = 2;
786795
public static final int kHasCubeStableCounts = 2;
796+
public static final int kHasCubeStableCountsAuto = 1;
787797
public static final int kHandVelStable = 50;
788798

789799
public static final double kHandHoldingPercent = 0.1; // FIXME
@@ -857,7 +867,7 @@ public static SupplyCurrentLimitConfiguration getRollerSupplyLimitConfig() {
857867

858868
public static class CompConstants {
859869
// Drive
860-
public static final double kWheelDiameterInches = 3.0 * (496 / 500.0);
870+
public static final double kWheelDiameterInches = 3.0 * (506.5 / 500.0);
861871

862872
// Elbow
863873
public static final int kElbowZeroTicks = 705; // 700
@@ -867,12 +877,12 @@ public static class CompConstants {
867877
public static final double kShoulderFollowerZeroTicks = 2057;
868878

869879
// Intake
870-
public static final int kIntakeZeroTicks = 3_600; // 2440 ->2540
880+
public static final int kIntakeZeroTicks = 2_700; // 2440 ->2540
871881
public static final double kExtendPosTicks = -2_100;
872882

873883
// Hand
874884
// 273 for a tooth on the hand
875-
public static final double kHandZeroTicks = 684; // 686 ->976
885+
public static final double kHandZeroTicks = 950; // 684 (hand 1)
876886
}
877887

878888
public static class ProtoConstants {

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

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -110,6 +110,7 @@ public void teleopInit() {
110110
if (m_autonomousCommand != null) {
111111
m_autonomousCommand.cancel();
112112
}
113+
m_robotContainer.autoStowTele();
113114
m_robotContainer.raiseServo();
114115
m_robotContainer.zeroElevator();
115116
m_robotContainer.setAuto(false); // commented out for now - to allow testing in Tele

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

Lines changed: 26 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -20,11 +20,11 @@
2020
import frc.robot.Constants.IntakeConstants;
2121
import frc.robot.commands.auto.AutoCommandInterface;
2222
import frc.robot.commands.auto.TestBalanceCommand;
23+
import frc.robot.commands.drive.DriveAutonCommand;
2324
import frc.robot.commands.drive.DriveTeleopCommand;
2425
import frc.robot.commands.drive.InterruptDriveCommand;
2526
import frc.robot.commands.drive.LockZeroCommand;
2627
import frc.robot.commands.drive.ZeroGyroCommand;
27-
import frc.robot.commands.drive.xLockCommand;
2828
import frc.robot.commands.elbow.ElbowHoldPosCommand;
2929
import frc.robot.commands.elbow.JogElbowCommand;
3030
import frc.robot.commands.elevator.AdjustElevatorCommand;
@@ -42,7 +42,6 @@
4242
import frc.robot.commands.intake.IntakeExtendCommand;
4343
import frc.robot.commands.intake.IntakeOpenLoopCommand;
4444
import frc.robot.commands.robotState.AutoPlaceCommand;
45-
import frc.robot.commands.robotState.AutoPlaceCommandGroup;
4645
import frc.robot.commands.robotState.FloorPickupCommand;
4746
import frc.robot.commands.robotState.ManualScoreCommand;
4847
import frc.robot.commands.robotState.RecoverGamepieceCommand;
@@ -104,10 +103,12 @@ public class RobotContainer {
104103
private SuppliedValueWidget<Boolean> allianceColor;
105104
private Alliance alliance = Alliance.Invalid;
106105
private SuppliedValueWidget<Boolean> currGamePiece;
106+
private boolean isEvent = false;
107107

108108
// Paths
109109
// private GrabCubeBalanceCommand testpath;
110110
private TestBalanceCommand balancepath;
111+
private DriveAutonCommand fiveMeterTest;
111112
// private CommunityToDockCommandGroup communityToDockCommandGroup;
112113
// private TwoPieceWithDockAutoCommandGroup twoPieceWithDockAutoCommandGroup;
113114
// private TwoPieceAutoPlacePathCommandGroup twoPieceAutoPlacePathCommandGroup;
@@ -120,6 +121,7 @@ public class RobotContainer {
120121
public RobotContainer() {
121122
DigitalInput eventFlag = new DigitalInput(10);
122123
boolean isEvent = eventFlag.get();
124+
this.isEvent = isEvent;
123125
if (isEvent && Constants.isCompBot) {
124126
// must be set before the first call to LoggerFactory.getLogger();
125127
System.setProperty(ContextInitializer.CONFIG_FILE_PROPERTY, "logback-event.xml");
@@ -192,6 +194,12 @@ public void setVisionEnabled(boolean isEnabled) {
192194
driveSubsystem.visionUpdates = isEnabled;
193195
}
194196

197+
public void autoStowTele() {
198+
if (elevatorSubsystem.hasZeroed() && isEvent) {
199+
// robotStateSubsystem.toStow();
200+
}
201+
}
202+
195203
public void setDisabled(boolean isDisabled) {
196204
robotStateSubsystem.setDisabled(isDisabled);
197205
}
@@ -250,6 +258,7 @@ private void configurePaths() {
250258
// elevatorSubsystem,
251259
// "TestAutoDrivePathOne",
252260
// "TestAutoDrivePathTwo");
261+
fiveMeterTest = new DriveAutonCommand(driveSubsystem, "straightPathX", true, true);
253262
balancepath =
254263
new TestBalanceCommand(
255264
driveSubsystem,
@@ -329,22 +338,28 @@ private void configureDriverButtonBindings() {
329338
false,
330339
robotStateSubsystem.getTargetCol(),
331340
true));*/
341+
// new JoystickButton(driveJoystick, Trim.RIGHT_X_POS.id) // 3578
342+
// .onTrue(
343+
// new AutoPlaceCommandGroup(
344+
// driveSubsystem, robotStateSubsystem, armSubsystem, handSubsystem));
332345
new JoystickButton(driveJoystick, Trim.RIGHT_X_POS.id) // 3578
333346
.onTrue(
334-
new AutoPlaceCommandGroup(
335-
driveSubsystem, robotStateSubsystem, armSubsystem, handSubsystem));
336-
new JoystickButton(driveJoystick, Trim.RIGHT_X_POS.id) // 3578
337-
.onTrue(
338-
new AutoPlaceCommand(driveSubsystem, robotStateSubsystem, armSubsystem, handSubsystem))
347+
new AutoPlaceCommand(
348+
driveSubsystem, robotStateSubsystem, armSubsystem, handSubsystem, false, 0.0))
339349
.onFalse(new InterruptDriveCommand(driveSubsystem));
340350
// TESTING AT
341351
// LAKEVIEW PRACTICE FIELD
342352

343353
// new JoystickButton(driveJoystick, InterlinkButton.X.id)
344354
// .onTrue(new ZeroElbowCommand(elbowSubsystem));
345355

346-
new JoystickButton(driveJoystick, InterlinkButton.X.id)
347-
.onTrue(new xLockCommand(driveSubsystem));
356+
new JoystickButton(driveJoystick, InterlinkButton.X.id).onTrue(fiveMeterTest);
357+
358+
// new JoystickButton(driveJoystick, InterlinkButton.X.id)
359+
// .onTrue(new xLockCommand(driveSubsystem));
360+
361+
// new JoystickButton(driveJoystick, InterlinkButton.X.id)
362+
// .onTrue(new AutonZeroElevatorCommand(elevatorSubsystem));
348363

349364
// Hand
350365
/*new JoystickButton(driveJoystick, Shoulder.LEFT_DOWN.id)
@@ -701,7 +716,7 @@ private void configurePitImportantDashboard() {
701716
ShuffleboardTab pitImportantTab = Shuffleboard.getTab("PitImportant");
702717
pitImportantTab
703718
.add(
704-
"HealthCheck Drive",
719+
"HealthCheck",
705720
new ShuffleBoardHealthCheckCommandGroup(
706721
elbowSubsystem,
707722
shoulderSubsystem,
@@ -763,7 +778,7 @@ public void setAllianceColor(Alliance alliance) {
763778
Map.of(
764779
"colorWhenTrue", alliance == Alliance.Red ? "red" : "blue", "colorWhenFalse", "black"));
765780
robotStateSubsystem.setAllianceColor(alliance);
766-
// testpath.generateTrajectory();
781+
fiveMeterTest.generateTrajectory();
767782
balancepath.generateTrajectory();
768783
// communityToDockCommandGroup.generateTrajectory();
769784
// twoPieceWithDockAutoCommandGroup.generateTrajectory();

‎src/main/java/frc/robot/commands/auto/AutoPlaceAutonCommand.java‎

Lines changed: 8 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -17,14 +17,20 @@ public class AutoPlaceAutonCommand extends CommandBase {
1717
private boolean isShelf;
1818
private TargetCol targetCol;
1919
private boolean isBlue;
20+
private boolean doYawAdjust;
21+
private double yawAdjustBy;
2022

2123
public AutoPlaceAutonCommand(
2224
DriveSubsystem driveSubsystem,
2325
RobotStateSubsystem robotStateSubsystem,
2426
ArmSubsystem armSubsystem,
25-
HandSubsystem handSubsystem) {
27+
HandSubsystem handSubsystem,
28+
boolean doYawAdjust,
29+
double yawAdjustBy) {
2630
addRequirements(driveSubsystem, armSubsystem, handSubsystem);
31+
this.doYawAdjust = doYawAdjust;
2732
this.driveSubsystem = driveSubsystem;
33+
this.yawAdjustBy = yawAdjustBy;
2834
this.robotStateSubsystem = robotStateSubsystem;
2935
}
3036

@@ -35,6 +41,7 @@ public void initialize() {
3541
"Starting Autoplace in Auton: Level: {}, Position(Col): {}",
3642
robotStateSubsystem.getTargetLevel().name(),
3743
robotStateSubsystem.getTargetCol().name());
44+
driveSubsystem.setDoYawAdjust(doYawAdjust, yawAdjustBy);
3845
robotStateSubsystem.toAutoScore();
3946
}
4047

‎src/main/java/frc/robot/commands/auto/DoNothingAutonCommand.java‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -4,7 +4,7 @@
44
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
55
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
66
import frc.robot.commands.drive.ZeroGyroCommand;
7-
import frc.robot.commands.elevator.ZeroElevatorCommand;
7+
import frc.robot.commands.elevator.AutonZeroElevatorCommand;
88
import frc.robot.commands.robotState.ManualScoreCommand;
99
import frc.robot.commands.robotState.ReleaseGamepieceCommand;
1010
import frc.robot.commands.robotState.SetGamePieceCommand;
@@ -38,7 +38,7 @@ public DoNothingAutonCommand(
3838
new ZeroGyroCommand(driveSubsystem),
3939
new SetGamePieceCommand(robotStateSubsystem, GamePiece.CONE),
4040
new SetTargetLevelCommand(robotStateSubsystem, TargetLevel.HIGH),
41-
new ZeroElevatorCommand(elevatorSubsystem),
41+
new AutonZeroElevatorCommand(elevatorSubsystem),
4242
new AutoGrabConeCommand(handSubsystem),
4343
new SetVisionUpdateCommand(driveSubsystem, false)),
4444
new ManualScoreCommand(robotStateSubsystem, armSubsystem, handSubsystem),

0 commit comments

Comments
 (0)