Skip to content

Commit 6f6dde3

Browse files
dabeycorndabeycorn
committed
dirty commit
Signed-off-by: Dasun L. Abeykoon <[email protected]>
1 parent 42d4f47 commit 6f6dde3

File tree

14 files changed

+334
-166
lines changed

14 files changed

+334
-166
lines changed

‎src/main/java/wmironpatriots/Constants.java‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -55,10 +55,10 @@ public class Constants {
5555
: Optional.empty();
5656

5757
public static enum LevelTarget {
58-
L1(Elevator.POSE_L1, Tail.POSE_LNONFOUR_RADS),
59-
L2(Elevator.POSE_L2, Tail.POSE_LNONFOUR_RADS),
60-
L3(Elevator.POSE_L3, Tail.POSE_LNONFOUR_RADS),
61-
L4(Elevator.POSE_L4, Tail.POSE_L4_RADS);
58+
L1(Elevator.POSE_L1, Tail.POSE_MIN_REVS),
59+
L2(Elevator.POSE_L2, Tail.POSE_MIN_REVS),
60+
L3(Elevator.POSE_L3, Tail.POSE_MIN_REVS),
61+
L4(Elevator.POSE_L4, Tail.POSE_MAX_REVS);
6262
public final double elevatorPoseRevs, tailPoseRads;
6363

6464
private LevelTarget(double elevatorPoseRevs, double tailPoseRads) {

‎src/main/java/wmironpatriots/Robot.java‎

Lines changed: 53 additions & 48 deletions
Original file line numberDiff line numberDiff line change
@@ -37,9 +37,11 @@
3737
import wmironpatriots.subsystems.elevator.Elevator;
3838
import wmironpatriots.subsystems.elevator.ElevatorIOComp;
3939
import wmironpatriots.subsystems.elevator.ElevatorIOSim;
40+
import wmironpatriots.subsystems.swerve.Swerve;
4041
import wmironpatriots.subsystems.tail.Tail;
4142
import wmironpatriots.subsystems.tail.TailIOComp;
4243
import wmironpatriots.subsystems.tail.TailIOSim;
44+
import wmironpatriots.util.deviceUtil.InputStream;
4345
import wmironpatriots.util.deviceUtil.OperatorController;
4446

4547
public class Robot extends TimedRobot implements Logged {
@@ -48,13 +50,13 @@ public class Robot extends TimedRobot implements Logged {
4850
private final CommandXboxController driveController;
4951
private final OperatorController operatorController;
5052

51-
// private final Swerve swerve;
53+
private final Swerve swerve;
5254
private final Tail tail;
5355
private final Elevator elevator;
5456
private final Chute chute;
5557

5658
private final RobotVisualizer visualizer;
57-
// private final SendableChooser<Command> autonChooser;
59+
private final SendableChooser<Command> autonChooser;
5860

5961
public Robot() {
6062
// * MONOLOGUE SETUP
@@ -102,14 +104,14 @@ public Robot() {
102104
driveController = new CommandXboxController(0);
103105
operatorController = new OperatorController(1);
104106

105-
// Optional<SwerveDriveSimulation> swerveSim =
106-
// Robot.isSimulation()
107-
// ? Optional.of(
108-
// new SwerveDriveSimulation(
109-
// SWERVE_SIM_CONFIG.get(), new Pose2d(3, 3, new Rotation2d())))
110-
// : Optional.empty();
107+
Optional<SwerveDriveSimulation> swerveSim =
108+
Robot.isSimulation()
109+
? Optional.of(
110+
new SwerveDriveSimulation(
111+
SWERVE_SIM_CONFIG.get(), new Pose2d(3, 3, new Rotation2d())))
112+
: Optional.empty();
111113

112-
// swerve = new Swerve(swerveSim);
114+
swerve = new Swerve(swerveSim);
113115
if (Robot.isReal()) {
114116
tail = new TailIOComp();
115117
elevator = new ElevatorIOComp();
@@ -121,50 +123,50 @@ public Robot() {
121123
}
122124

123125
// Setup simulated arena if simulated
124-
// if (Robot.isSimulation()) {
125-
// SimulatedArena.getInstance().addDriveTrainSimulation(swerveSim.orElse(null));
126-
// swerve.resetOdo(swerveSim.get().getSimulatedDriveTrainPose());
127-
// }
126+
if (Robot.isSimulation()) {
127+
SimulatedArena.getInstance().addDriveTrainSimulation(swerveSim.orElse(null));
128+
swerve.resetOdo(swerveSim.get().getSimulatedDriveTrainPose());
129+
}
128130

129131
// * DEFAULT COMMANDS
130132
// Set up driver input streams
131-
// double maxSpeed = swerve.MAX_LINEAR_SPEED_MPS;
132-
// double maxAngularSpeed = swerve.MAX_ANGULAR_SPEED_RADS_PER_SEC;
133+
double maxSpeed = swerve.MAX_LINEAR_SPEED_MPS;
134+
double maxAngularSpeed = swerve.MAX_ANGULAR_SPEED_RADS_PER_SEC;
133135

134-
// InputStream x = InputStream.of(driveController::getLeftY);
135-
// InputStream y = InputStream.of(driveController::getLeftX);
136-
// InputStream speed = InputStream.of(driveController::getRightTriggerAxis).deadband(0.1, 1.0);
136+
InputStream x = InputStream.of(driveController::getLeftY);
137+
InputStream y = InputStream.of(driveController::getLeftX);
138+
InputStream speed = InputStream.of(driveController::getRightTriggerAxis).deadband(0.1, 1.0);
137139

138-
// InputStream hypot =
139-
// InputStream.hypot(y, x).clamp(1).deadband(0.05, 1.0).signedPow(2).scale(maxSpeed);
140+
InputStream hypot =
141+
InputStream.hypot(y, x).clamp(1).deadband(0.05, 1.0).signedPow(2).scale(maxSpeed);
140142

141-
// InputStream theta = InputStream.arcTan(y, x);
142-
// x = hypot.scale(hypot.scale(theta.map(Math::cos)));
143-
// y = hypot.scale(hypot.scale(theta.map(Math::sin)));
143+
InputStream theta = InputStream.arcTan(y, x);
144+
x = hypot.scale(hypot.scale(theta.map(Math::cos)));
145+
y = hypot.scale(hypot.scale(theta.map(Math::sin)));
144146

145-
// InputStream omega =
146-
// InputStream.of(driveController::getRightX)
147-
// .clamp(1.0)
148-
// .deadband(0.05, 1.0)
149-
// .signedPow(2.0)
150-
// .scale(maxAngularSpeed);
147+
InputStream omega =
148+
InputStream.of(driveController::getRightX)
149+
.clamp(1.0)
150+
.deadband(0.05, 1.0)
151+
.signedPow(2.0)
152+
.scale(maxAngularSpeed);
151153

152-
// swerve.setDefaultCommand(swerve.teleopSwerveCmmd(x, y, omega));
154+
swerve.setDefaultCommand(swerve.teleopSwerveCmmd(x, y, omega));
153155

154156
elevator.setDefaultCommand(
155157
Commands.sequence(
156158
elevator.runPoseZeroingCmmd().onlyIf(() -> !elevator.isZeroed()),
157159
elevator.setTargetPoseCmmd(Elevator.IDLE).until(() -> elevator.inSetpointRange()),
158160
elevator.stopMotorInputCmmd()));
159161

160-
// tail.setDefaultCommand(
161-
// Commands.sequence(
162-
// tail.runPoseZeroingCmmd()
163-
// .onlyIf(() -> !tail.isZeroed() && Superstructure.isTailSafe(elevator, tail)),
164-
// tail.setTargetPoseCmmd(Tail.POSE_OUT_RADS)
165-
// .until(() -> Superstructure.isTailSafe(elevator, tail)),
166-
// tail.setTargetPoseCmmd(Tail.POSE_MIN_REVS).until(() -> tail.inSetpointRange()),
167-
// tail.stopMotorInputCmmd()));
162+
tail.setDefaultCommand(
163+
Commands.sequence(
164+
tail.runPoseZeroingCmmd()
165+
.onlyIf(() -> !tail.isZeroed() && Superstructure.isTailSafe(elevator, tail)),
166+
tail.setTargetPoseCmmd(Tail.POSE_MAX_REVS)
167+
.until(() -> Superstructure.isTailSafe(elevator, tail)),
168+
tail.setTargetPoseCmmd(Tail.POSE_MIN_REVS).until(() -> tail.inSetpointRange()),
169+
tail.stopMotorInputCmmd()));
168170

169171
// * SUPERSTRUCTURE INIT
170172
Map<Requests, Trigger> triggerMap = new HashMap<Superstructure.Requests, Trigger>();
@@ -173,8 +175,8 @@ public Robot() {
173175
visualizer = new RobotVisualizer(elevator, tail);
174176

175177
// * AUTON INIT
176-
// autonChooser = Autonomous.configureAutons(swerve);
177-
// SmartDashboard.putData("Select Auton", autonChooser);
178+
autonChooser = Autonomous.configureAutons(swerve);
179+
SmartDashboard.putData("Select Auton", autonChooser);
178180

179181
new Superstructure(
180182
elevator,
@@ -188,21 +190,24 @@ public Robot() {
188190
@Override
189191
public void robotPeriodic() {
190192
CommandScheduler.getInstance().run();
193+
if (Robot.isSimulation()) {
194+
SimulatedArena.getInstance().simulationPeriodic();
195+
}
191196

192197
Monologue.updateAll();
193198
visualizer.periodic();
194199
}
195200

196201
@Override
197202
public void autonomousInit() {
198-
// Command auton =
199-
// autonChooser
200-
// .getSelected()
201-
// .withDeadline(Commands.waitUntil(() -> DriverStation.isEnabled()));
202-
203-
// if (auton != null) {
204-
// auton.schedule();
205-
// }
203+
Command auton =
204+
autonChooser
205+
.getSelected()
206+
.withDeadline(Commands.waitUntil(() -> DriverStation.isEnabled()));
207+
208+
if (auton != null) {
209+
auton.schedule();
210+
}
206211
}
207212

208213
@Override

‎src/main/java/wmironpatriots/commands/RunToPoseCmmd.java‎

Lines changed: 18 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -1,24 +1,29 @@
1-
package wmironpatriots.commands;
1+
// Copyright (c) 2025 FRC 6423 - Ward Melville Iron Patriots
2+
// https://github.com/FIRSTTeam6423
3+
//
4+
// Open Source Software; you can modify and/or share it under the terms of
5+
// MIT license file in the root directory of this project
26

3-
import java.util.function.Supplier;
7+
package wmironpatriots.commands;
48

59
import edu.wpi.first.math.geometry.Pose2d;
610
import edu.wpi.first.math.kinematics.ChassisSpeeds;
711
import edu.wpi.first.wpilibj2.command.Command;
12+
import java.util.function.Supplier;
813
import wmironpatriots.subsystems.swerve.Swerve;
914

1015
public class RunToPoseCmmd extends Command {
11-
private final Swerve swerve;
12-
private final Supplier<Pose2d> desiredPose, currentPose;
16+
private final Swerve swerve;
17+
private final Supplier<Pose2d> desiredPose, currentPose;
1318

14-
public RunToPoseCmmd(Swerve swerve, Supplier<Pose2d> desiredPose, Supplier<Pose2d> currentPose) {
15-
this.swerve = swerve;
16-
this.desiredPose = desiredPose;
17-
this.currentPose = currentPose;
18-
}
19+
public RunToPoseCmmd(Swerve swerve, Supplier<Pose2d> desiredPose, Supplier<Pose2d> currentPose) {
20+
this.swerve = swerve;
21+
this.desiredPose = desiredPose;
22+
this.currentPose = currentPose;
23+
}
1924

20-
@Override
21-
public void initialize() {
22-
ChassisSpeeds currentFieldRelativeVelocties = swerve.getFieldRelativeVelocities();
23-
}
25+
@Override
26+
public void initialize() {
27+
ChassisSpeeds currentFieldRelativeVelocties = swerve.getFieldRelativeVelocities();
28+
}
2429
}

‎src/main/java/wmironpatriots/subsystems/Superstructure.java‎

Lines changed: 7 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -6,7 +6,6 @@
66

77
package wmironpatriots.subsystems;
88

9-
import edu.wpi.first.math.MathUtil;
109
import edu.wpi.first.wpilibj.Timer;
1110
import edu.wpi.first.wpilibj2.command.Command;
1211
import edu.wpi.first.wpilibj2.command.Commands;
@@ -18,7 +17,6 @@
1817
import wmironpatriots.Constants.LevelTarget;
1918
import wmironpatriots.subsystems.chute.Chute;
2019
import wmironpatriots.subsystems.elevator.Elevator;
21-
import wmironpatriots.subsystems.swerve.Swerve;
2220
import wmironpatriots.subsystems.tail.Tail;
2321

2422
/** Cursed superstructure ahh frfr */
@@ -109,7 +107,7 @@ public Superstructure(
109107
stateMap
110108
.get(State.INTAKING_CHUTE)
111109
.whileTrue(
112-
tail.setTargetPoseCmmd(Tail.POSE_OUT_RADS)
110+
tail.setTargetPoseCmmd(Tail.POSE_MAX_REVS)
113111
.until(() -> isTailSafe(elevator, tail))
114112
.andThen(tail.setTargetPoseCmmd(Tail.POSE_MIN_REVS)))
115113
.whileTrue(elevator.setTargetPoseCmmd(Elevator.POSE_INTAKING))
@@ -123,7 +121,7 @@ public Superstructure(
123121
stateMap
124122
.get(State.L1_SETUP)
125123
.whileTrue(
126-
tail.setTargetPoseCmmd(Tail.POSE_OUT_RADS)
124+
tail.setTargetPoseCmmd(Tail.POSE_MAX_REVS)
127125
.until(() -> isTailSafe(elevator, tail))
128126
.andThen(tail.setTargetPoseCmmd(Tail.POSE_MIN_REVS)))
129127
.whileTrue(elevator.setTargetPoseCmmd(Elevator.POSE_L1))
@@ -133,7 +131,7 @@ public Superstructure(
133131
stateMap
134132
.get(State.L2_SETUP)
135133
.whileTrue(
136-
tail.setTargetPoseCmmd(Tail.POSE_OUT_RADS)
134+
tail.setTargetPoseCmmd(Tail.POSE_MAX_REVS)
137135
.until(() -> isTailSafe(elevator, tail))
138136
.andThen(tail.setTargetPoseCmmd(Tail.POSE_MIN_REVS)))
139137
.whileTrue(elevator.setTargetPoseCmmd(Elevator.POSE_L2))
@@ -143,7 +141,7 @@ public Superstructure(
143141
stateMap
144142
.get(State.L3_SETUP)
145143
.whileTrue(
146-
tail.setTargetPoseCmmd(Tail.POSE_OUT_RADS)
144+
tail.setTargetPoseCmmd(Tail.POSE_MAX_REVS)
147145
.until(() -> isTailSafe(elevator, tail))
148146
.andThen(tail.setTargetPoseCmmd(Tail.POSE_MIN_REVS)))
149147
.whileTrue(elevator.setTargetPoseCmmd(Elevator.POSE_L3))
@@ -152,7 +150,7 @@ public Superstructure(
152150

153151
stateMap
154152
.get(State.L4_SETUP)
155-
.whileTrue(tail.setTargetPoseCmmd(Tail.POSE_OUT_RADS))
153+
.whileTrue(tail.setTargetPoseCmmd(Tail.POSE_MAX_REVS))
156154
.whileTrue(elevator.setTargetPoseCmmd(Elevator.POSE_L4))
157155
.and(() -> elevator.inSetpointRange())
158156
.onTrue(setCurrentStateCommand(State.REEF_SCORE));
@@ -173,7 +171,8 @@ public static boolean isTailSafe(Elevator elevator, Tail tail) {
173171

174172
if (SetpointDisplacement > 0 && elevator.getPose() < Elevator.POSE_MAX_CARRIAGE_STAGE_ONE) {
175173
return false;
176-
} else if (SetpointDisplacement < 0 && elevator.getPose() > Elevator.POSE_MAX_CARRIAGE_STAGE_ONE) {
174+
} else if (SetpointDisplacement < 0
175+
&& elevator.getPose() > Elevator.POSE_MAX_CARRIAGE_STAGE_ONE) {
177176
return false;
178177
}
179178
return true;

‎src/main/java/wmironpatriots/subsystems/swerve/Swerve.java‎

Lines changed: 5 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,7 @@
1818
import edu.wpi.first.math.kinematics.SwerveModulePosition;
1919
import edu.wpi.first.math.kinematics.SwerveModuleState;
2020
import edu.wpi.first.math.system.plant.DCMotor;
21+
import edu.wpi.first.math.util.Units;
2122
import edu.wpi.first.networktables.NetworkTableInstance;
2223
import edu.wpi.first.networktables.StructArrayPublisher;
2324
import edu.wpi.first.wpilibj.DriverStation;
@@ -77,8 +78,7 @@ public class Swerve implements IronSubsystem {
7778

7879
// TODO check with
7980
// https://www.chiefdelphi.com/t/how-to-calculate-the-max-free-speed-of-a-swerve/400741/3
80-
public static double MAX_LINEAR_SPEED_MPS =
81-
((6000 / 60) * (2 * Math.PI)) / (Module.DRIVE_REDUCTION * Module.WHEEL_RADIUS_METERS);
81+
public static double MAX_LINEAR_SPEED_MPS = Units.feetToMeters(3);
8282
public static double MAX_LINEAR_ACCEL_MPS_SQRD = 2;
8383
public static double MAX_ANGULAR_SPEED_RADS_PER_SEC =
8484
MAX_LINEAR_SPEED_MPS / (Math.hypot(TRACK_WIDTH_METERS / 2.0, TRACK_WIDTH_METERS / 2.0));
@@ -215,7 +215,9 @@ public Rotation2d getHeading() {
215215
}
216216

217217
public Pose2d getPose() {
218-
return odo.getEstimatedPosition();
218+
return Robot.isReal()
219+
? odo.getEstimatedPosition()
220+
: simulation.get().getSimulatedDriveTrainPose();
219221
}
220222

221223
public ChassisSpeeds getRobotRelativeVelocities() {

‎src/main/java/wmironpatriots/subsystems/swerve/module/Module.java‎

Lines changed: 0 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -39,15 +39,12 @@ protected static TalonFXConfiguration getPivotConfig() {
3939

4040
config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder;
4141
config.Feedback.SensorToMechanismRatio = PIVOT_REDUCTION;
42-
config.Feedback.SensorToMechanismRatio = 1.0;
4342
config.Slot0.kP = 20.0;
4443
config.Slot0.kI = 0.0;
4544
config.Slot0.kD = 0.68275;
4645
config.Slot0.kV = 0.42962962963;
4746
config.Slot0.kA = 0.031543;
4847

49-
config.MotionMagic.MotionMagicCruiseVelocity = (5500 / 60) / PIVOT_REDUCTION;
50-
config.MotionMagic.MotionMagicAcceleration = (5500 / 60) / (PIVOT_REDUCTION * 0.005);
5148
config.ClosedLoopGeneral.ContinuousWrap = true;
5249

5350
return config;

‎src/main/java/wmironpatriots/subsystems/swerve/module/ModuleIOComp.java‎

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,8 @@
1212
import com.ctre.phoenix6.configs.CANcoderConfiguration;
1313
import com.ctre.phoenix6.configs.TalonFXConfiguration;
1414
import com.ctre.phoenix6.controls.MotionMagicVoltage;
15+
import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC;
16+
import com.ctre.phoenix6.controls.PositionVoltage;
1517
import com.ctre.phoenix6.controls.TorqueCurrentFOC;
1618
import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC;
1719
import com.ctre.phoenix6.controls.VoltageOut;
@@ -29,7 +31,7 @@ public class ModuleIOComp extends Module {
2931

3032
private final VoltageOut reqPivotVolts, reqDriveVolts;
3133
private final VelocityTorqueCurrentFOC reqDriveVel;
32-
private final MotionMagicVoltage reqPivotFeedback;
34+
private final PositionVoltage reqPivotFeedback;
3335
private final TorqueCurrentFOC reqDriveCurrent;
3436

3537
private final BaseStatusSignal cancoderPose,
@@ -65,7 +67,7 @@ public ModuleIOComp(ModuleConfig config) {
6567
reqPivotVolts = new VoltageOut(0.0).withEnableFOC(true);
6668
reqDriveVolts = new VoltageOut(0.0).withEnableFOC(true);
6769
reqDriveVel = new VelocityTorqueCurrentFOC(0.0).withSlot(0);
68-
reqPivotFeedback = new MotionMagicVoltage(0.0).withEnableFOC(true);
70+
reqPivotFeedback = new PositionVoltage(0.0).withEnableFOC(true);
6971
reqDriveCurrent = new TorqueCurrentFOC(0.0);
7072

7173
cancoderPose = cancoder.getAbsolutePosition();

‎src/main/java/wmironpatriots/subsystems/swerve/module/ModuleIOSim.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -155,6 +155,7 @@ protected void runPivotVolts(double volts) {
155155

156156
@Override
157157
protected void runPivotPose(double poseRads) {
158+
System.out.println(poseRads);
158159
pivot.setControl(reqPivotFeedback.withPosition(poseRads / (2 * Math.PI)));
159160
}
160161

@@ -165,7 +166,6 @@ protected void runDriveVolts(double volts, boolean focEnabled) {
165166

166167
@Override
167168
protected void runDriveVel(double velMPS, double ff) {
168-
System.out.println(velMPS);
169169
drive.setControl(
170170
reqDriveVel
171171
.withVelocity(velMPS / (2 * Math.PI * WHEEL_RADIUS_METERS))

0 commit comments

Comments
 (0)