Skip to content

Commit b8a90e9

Browse files
bobopop787bobopop787
committed
temp backup
1 parent b2d1e3b commit b8a90e9

File tree

11 files changed

+409
-125
lines changed

11 files changed

+409
-125
lines changed

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

Lines changed: 10 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -198,7 +198,7 @@ public static class SwerveModuleConstants {
198198
}
199199

200200
public static class ElevatorConstants { // CAN ID range 11-19
201-
public static final int kElevatorLeaderCANID = 11;
201+
public static final int kElevatorLeaderCANID = 19;
202202
public static final int kElevatorFollowerCANID = 12;
203203
public static final int kLeftEffectorCANID = 13;
204204
public static final int kRightEffectorCANID = 14;
@@ -212,13 +212,13 @@ public static class ElevatorConstants { // CAN ID range 11-19
212212

213213
public static final double kSprocketPitchDiameter = Units.inchesToMeters(1.7567); // meters
214214
public static final double kElevatorPositionConversionFactor = kSprocketPitchDiameter * Math.PI;
215-
public static final double kElevatorPositionConversionFactorInternal = kSprocketPitchDiameter * Math.PI / 27;
215+
public static final double kElevatorPositionConversionFactorInternal = kSprocketPitchDiameter * Math.PI / 20;
216216

217217
public static final ControlConstants elevatorControlConstants = new ControlConstants(
218218
"elevator",
219-
30, // to test
220-
0,
219+
20, // to test
221220
0,
221+
1,
222222
0,
223223
0.22,
224224
0
@@ -236,7 +236,6 @@ public static class ElevatorConstants { // CAN ID range 11-19
236236
.inverted(true)
237237
.closedLoopRampRate(0.1)
238238
; elevatorLeaderConfig.closedLoop
239-
// .feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
240239
.p(elevatorControlConstants.kP(), ClosedLoopSlot.kSlot0)
241240
.i(elevatorControlConstants.kI(), ClosedLoopSlot.kSlot0)
242241
.d(elevatorControlConstants.kD(), ClosedLoopSlot.kSlot0)
@@ -368,7 +367,7 @@ public static class OperatorConstants {
368367

369368
public static class FieldConstants {
370369
public static final double fieldWidth = 17.548; // m
371-
public static final double fieldHeight = 8.042; // m - ANDYMARK FIELD NOT WELDED BC FIRST IS ANNOYING :)
370+
public static final double fieldHeight = 8.042; // m - ANDYMARK FIELD NOT WELDED BC FIRST IS ANNOYING :) 8.052
372371

373372
public static final Translation2d reefCenterBlue = new Translation2d(4.489323, fieldHeight / 2);
374373
public static final Translation2d reefCenterRed = new Translation2d(13.058902, fieldHeight / 2);
@@ -379,5 +378,10 @@ public static class FieldConstants {
379378
public static final Pose2d blueCloseRightReef = new Pose2d(3.2512, fieldHeight / 2 - 0.164338, Rotation2d.kZero);
380379
public static final Pose2d redFarLeftReef = blueCloseLeftReef.plus(betweenReefsTransform);
381380
public static final Pose2d redFarRightReef = blueCloseRightReef.plus(betweenReefsTransform);
381+
382+
public static final Pose2d blueCoralY = new Pose2d(1.59336668626, 7.44378938214, Rotation2d.fromDegrees(306));
383+
public static final Pose2d blueCoralZ = new Pose2d(1.59336668626, fieldHeight - 7.44378938214, Rotation2d.fromDegrees(54));
384+
public static final Pose2d redCoralY = blueCoralY.rotateAround(fieldCenter, Rotation2d.k180deg);
385+
public static final Pose2d redCoralZ = blueCoralZ.rotateAround(fieldCenter, Rotation2d.k180deg);
382386
}
383387
}

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

Lines changed: 16 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,7 @@
1111
import org.littletonrobotics.junction.wpilog.WPILOGReader;
1212
import org.littletonrobotics.junction.wpilog.WPILOGWriter;
1313

14+
import edu.wpi.first.net.PortForwarder;
1415
import edu.wpi.first.wpilibj.PowerDistribution;
1516
import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
1617
import edu.wpi.first.wpilibj2.command.Command;
@@ -19,7 +20,7 @@
1920
public class Robot extends LoggedRobot { // Use TimedRobot if not using AdvantageKit
2021
private Command m_autonomousCommand;
2122

22-
private final RobotContainer m_robotContainer;
23+
private final RobotContainer robotContainer;
2324

2425
public Robot() {
2526
Logger.recordMetadata("ProjectName", "MyProject"); // Set a metadata value
@@ -38,7 +39,19 @@ public Robot() {
3839

3940
Logger.start(); // Start logging! No more data receivers, replay sources, or metadata values may be added.
4041

41-
m_robotContainer = new RobotContainer();
42+
robotContainer = new RobotContainer();
43+
// TODO check port forwarding
44+
for(int light = 0; light < 3; light++) {
45+
String remoteHost = switch(light) {
46+
case 0 -> "limelight-front.local";
47+
case 1 -> "limelight-high.local";
48+
case 2 -> "limelight-side.local";
49+
default -> "limelight.local";
50+
};
51+
for(int port = 5800; port <= 5807; port++) {
52+
PortForwarder.add(port, remoteHost, port + light * 10);
53+
}
54+
}
4255
}
4356

4457
@Override
@@ -57,7 +70,7 @@ public void disabledExit() {}
5770

5871
@Override
5972
public void autonomousInit() {
60-
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
73+
m_autonomousCommand = robotContainer.getAutonomousCommand();
6174

6275
if (m_autonomousCommand != null) {
6376
m_autonomousCommand.schedule();

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

Lines changed: 43 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,7 @@
55
package frc.robot;
66

77
import edu.wpi.first.wpilibj.Preferences;
8+
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
89
import edu.wpi.first.wpilibj2.command.Command;
910
import edu.wpi.first.wpilibj2.command.Commands;
1011
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
@@ -24,6 +25,7 @@
2425
import frc.robot.subsystems.swerve.SDSModuleIO;
2526
import frc.robot.subsystems.swerve.SDSModuleIOSpark;
2627
import frc.robot.subsystems.swerve.SwerveDrive;
28+
import frc.robot.subsystems.swerve.SwerveDrive.PresetPositionType;
2729
import frc.robot.subsystems.vision.LimelightLocations;
2830
import frc.robot.subsystems.vision.Vision;
2931
import frc.robot.subsystems.vision.VisionIO;
@@ -60,10 +62,10 @@ public RobotContainer() {
6062
elevator = new Elevator(new ElevatorIOSpark());
6163
// manipulator = new Manipulator(new ManipulatorIOSpark());
6264
vision = new Vision(
63-
swerve::addVisionMeasurement,
64-
new VisionIOLimelight(LimelightLocations.FRONT3, () -> swerve.getPose().getRotation()),
65-
new VisionIOLimelight(LimelightLocations.HIGH3, () -> swerve.getPose().getRotation()),
66-
new VisionIOLimelight(LimelightLocations.SIDE2, () -> swerve.getPose().getRotation())
65+
swerve::addVisionMeasurement
66+
, new VisionIOLimelight(LimelightLocations.FRONT3, () -> swerve.getPose().getRotation())
67+
, new VisionIOLimelight(LimelightLocations.HIGH3, () -> swerve.getPose().getRotation())
68+
// , new VisionIOLimelight(LimelightLocations.SIDE2, () -> swerve.getPose().getRotation())
6769
);
6870
break;
6971
default:
@@ -85,14 +87,13 @@ public RobotContainer() {
8587

8688
private void configureBindings() {
8789
swerve.setToAimSuppliers(
88-
driverController.x()::getAsBoolean, // aim reef
90+
driverController.leftTrigger()::getAsBoolean, // aim reef
8991
driverController.b()::getAsBoolean, // aim processor
9092
driverController.a()::getAsBoolean // aim station
9193
);
92-
swerve.setToPosSuppliers(
93-
driverController.povLeft()::getAsBoolean, // left reef
94-
driverController.povRight()::getAsBoolean, // right reef
95-
driverController.povDown()::getAsBoolean // processor
94+
swerve.setReefChooserSuppliers(
95+
driverController::getRightX,
96+
driverController::getRightY
9697
);
9798
swerve.setElevatorHeightSupplier(elevator::getElevatorHeight);
9899
swerve.setDefaultCommand(swerve.runDriveInputs(
@@ -108,6 +109,9 @@ private void configureBindings() {
108109
driverController.back().onTrue(swerve.runToggleToXPosition(true));
109110
driverController.b().onTrue(swerve.runUpdateControlConstants().andThen(elevator.runUpdateControlConstants()));
110111
driverController.start().onTrue(swerve.runSetTempPose());
112+
driverController.povLeft().onTrue(swerve.runTogglePresetPosition(PresetPositionType.LEFTREEF));
113+
driverController.povRight().onTrue(swerve.runTogglePresetPosition(PresetPositionType.RIGHTREEF));
114+
driverController.povUp().onTrue(swerve.runTogglePresetPosition(PresetPositionType.PROCESSOR));
111115

112116
// if(Constants.simMode.equals(RobotMode.SIM)) {
113117
// swerve.setDefaultCommand(swerve.runSimOdometryMoveBy(
@@ -132,28 +136,49 @@ private void configureBindings() {
132136
// driverController.povRight().onTrue(elevator.runaElevatorOpenLoopPreferences());
133137
// driverController.povRight().onFalse(elevator.runElevatorOpenLoop(0));
134138

135-
// auxController.a().onTrue(manipulator.runSetManipulatorPosition(ManipulatorPosition.HOME));
136-
// auxController.b().onTrue(manipulator.runSetManipulatorPosition(ManipulatorPosition.INTAKE));
137-
// auxController.x().onTrue(manipulator.runSetManipulatorPosition(ManipulatorPosition.CARRY));
139+
Combo alignedOverrideCombo = new Combo("isAligned Override Combo", 0.5,
140+
auxController.leftTrigger(),
141+
auxController.leftTrigger().negate(),
142+
auxController.leftTrigger()
143+
);
144+
alignedOverrideCombo.getTrigger().onTrue(Commands.run(() -> {}));
145+
Shuffleboard.getTab("Teleoperated")
146+
.addBoolean("Aligned Override", alignedOverrideCombo.getTrigger()).
147+
withPosition(4, 2)
148+
.withSize(1, 1);
138149

139150
auxController.b().onTrue(elevator.runToElevatorPosition(ElevatorPosition.HOME));
140151
auxController.a().onTrue(elevator.runToElevatorPosition(ElevatorPosition.L1));
141152
auxController.x().onTrue(elevator.runToElevatorPosition(ElevatorPosition.L2));
142153
auxController.y().onTrue(elevator.runToElevatorPosition(ElevatorPosition.L3));
143-
auxController.leftBumper().onTrue(elevator.runIntakeEffector(-4, 4));
154+
auxController.leftBumper().onTrue(elevator.runIntakeEffector(
155+
5, // effector volts
156+
-2, // funnel volts
157+
() -> swerve.isAligned() || alignedOverrideCombo.getTrigger().getAsBoolean() // is aligned supplier
158+
// () -> true
159+
));
144160
auxController.leftBumper().onFalse(elevator.runStopIntakeEffector());
145-
auxController.leftTrigger().onTrue(elevator.runEffector(-6, 3));
146-
auxController.leftTrigger().onFalse(elevator.runEffector(0, 0));
147-
auxController.rightBumper().onTrue(elevator.runIntakeFunnel(-2));
148-
auxController.rightBumper().onFalse(elevator.runSetFunnelVolts(0));
161+
162+
// auxController.leftBumper().onTrue(elevator.runEffector(-5, 5));
163+
// auxController.leftBumper().onFalse(elevator.runEffector(0, 0));
164+
// should auto adjust speed for L1
165+
// auxController.leftTrigger().onTrue(elevator.runEffector(-6, 3));
166+
// auxController.leftTrigger().onFalse(elevator.runEffector(0, 0));
167+
auxController.leftTrigger().onTrue(elevator.runUpdateControlConstants());
168+
// auxController.rightBumper().onTrue(elevator.runIntakeFunnel(-2));
169+
// auxController.rightBumper().onFalse(elevator.runSetFunnelVolts(0));
170+
149171
auxController.leftStick().onTrue(elevator.runSetDealgaeVolts(5));
150172
auxController.leftStick().onFalse(elevator.runSetDealgaeVolts(0));
151173
auxController.rightStick().onTrue(elevator.runSetDealgaeVolts(-8));
152174
auxController.rightStick().onFalse(elevator.runSetDealgaeVolts(0));
153-
154175
auxController.povUp().whileTrue(elevator.runJogElevatorPosition(0.001));
155176
auxController.povDown().whileTrue(elevator.runJogElevatorPosition(-0.001));
156177

178+
// auxController.a().onTrue(manipulator.runSetManipulatorPosition(ManipulatorPosition.HOME));
179+
// auxController.b().onTrue(manipulator.runSetManipulatorPosition(ManipulatorPosition.INTAKE));
180+
// auxController.x().onTrue(manipulator.runSetManipulatorPosition(ManipulatorPosition.CARRY));
181+
157182
// Combo testCombo = new Combo("test combo", 1,
158183
// driverController.a(),
159184
// driverController.a().negate(),

‎src/main/java/frc/robot/commands/Autos.java‎

Lines changed: 28 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,8 @@
2121
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
2222
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
2323
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
24+
import edu.wpi.first.wpilibj.shuffleboard.SimpleWidget;
25+
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
2426
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
2527
import edu.wpi.first.wpilibj2.command.Command;
2628
import edu.wpi.first.wpilibj2.command.Commands;
@@ -35,8 +37,9 @@
3537
* Utility class for generating autonomous routines.
3638
*/
3739
public class Autos {
38-
private static final SendableChooser<StartingPositions> startingPositionChooser = new SendableChooser<>();
40+
public static final SendableChooser<StartingPositions> startingPositionChooser = new SendableChooser<>();
3941
private static GenericEntry autoStringFormEntry;
42+
public static final Field2d startingPositionVisualizerField = new Field2d();
4043

4144
private static final double preemptiveElevatorInterval = 0.2; // seconds before reaching target for raising elevator (currently no delay implemented before preemptive intake)
4245
private static final double elevatorAllowMovementTolerance = 0.002; // meters above home tolerated before allowing movement
@@ -93,10 +96,10 @@ private enum AutoPositions {
9396

9497
static {
9598
ShuffleboardTab tab = Shuffleboard.getTab("Autonomous ");
96-
autoStringFormEntry = tab.add("Auto String", "")
97-
.withPosition(0, 1)
98-
.withSize(2, 1)
99-
.getEntry();
99+
SimpleWidget autoStringWidget = tab.add("Auto String", "")
100+
.withPosition(7, 0)
101+
.withSize(3, 1);
102+
autoStringFormEntry = autoStringWidget.getEntry();
100103

101104
for(StartingPositions position : StartingPositions.values()) {
102105
if(position.equals(StartingPositions.CENTER)) {
@@ -109,6 +112,12 @@ private enum AutoPositions {
109112
.withWidget(BuiltInWidgets.kSplitButtonChooser)
110113
.withPosition(0, 0)
111114
.withSize(7, 1);
115+
tab.add("Starting Position Field", startingPositionVisualizerField)
116+
.withPosition(0, 1)
117+
.withSize(3, 2);
118+
tab.add("Generate Path", Commands.print("test"))
119+
.withPosition(0, 3)
120+
.withSize(2, 1);
112121

113122
loadTrajectories();
114123
}
@@ -205,13 +214,7 @@ private static void loadTrajectories() {
205214
System.out.println("Done loading trajectories");
206215
}
207216

208-
/**
209-
* Selects correct trajectories using responses from Shuffleboard, generates Commands for each leg of auto, and combines them in a SequenceCommandGroup.
210-
* @param swerve Robot's swerve drive subystem instance.
211-
* @param elevator Robot's elevator subsystem instance.
212-
* @return SequenceCommandGroup representing autonomous generated based on Shuffleboard selections.
213-
*/
214-
public static Command getAutoCommand(SwerveDrive swerve, Elevator elevator) {
217+
public static Pose2d getStartingPose() {
215218
StartingPositions startingPosition = Constants.isRed() ? switch(startingPositionChooser.getSelected()) {
216219
case ALLYCAGELEFT -> StartingPositions.OPPOSINGCAGERIGHT;
217220
case ALLYCAGECENTER -> StartingPositions.OPPOSINGCAGECENTER;
@@ -221,11 +224,21 @@ public static Command getAutoCommand(SwerveDrive swerve, Elevator elevator) {
221224
case OPPOSINGCAGECENTER -> StartingPositions.ALLYCAGECENTER;
222225
case OPPOSINGCAGERIGHT -> StartingPositions.ALLYCAGELEFT;
223226
} : startingPositionChooser.getSelected();
224-
Command resetPoseCommand = Commands.runOnce(() -> swerve.setPose(new Pose2d(
227+
return new Pose2d(
225228
Constants.isRed() ? StartingPositions.redX : StartingPositions.blueX,
226229
startingPosition.y,
227-
Constants.isRed() ? Rotation2d.kZero : Rotation2d.k180deg)
228-
), swerve);
230+
Constants.isRed() ? Rotation2d.kZero : Rotation2d.k180deg
231+
);
232+
}
233+
234+
/**
235+
* Selects correct trajectories using responses from Shuffleboard, generates Commands for each leg of auto, and combines them in a SequenceCommandGroup.
236+
* @param swerve Robot's swerve drive subystem instance.
237+
* @param elevator Robot's elevator subsystem instance.
238+
* @return SequenceCommandGroup representing autonomous generated based on Shuffleboard selections.
239+
*/
240+
public static Command getAutoCommand(SwerveDrive swerve, Elevator elevator) {
241+
Command resetPoseCommand = Commands.runOnce(() -> swerve.setPose(getStartingPose()), swerve);
229242

230243
String rawAutoString = autoStringFormEntry.getString("");
231244
System.out.println("Trying to load auto string \"" + rawAutoString + "\"");

0 commit comments

Comments
 (0)