Skip to content

Commit 42d4f47

Browse files
dabeycorndabeycorn
committed
I love joon - Aditya Kumar 2025
Signed-off-by: Dasun L. Abeykoon <[email protected]>
1 parent 40c40e6 commit 42d4f47

File tree

5 files changed

+66
-78
lines changed

5 files changed

+66
-78
lines changed

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

Lines changed: 48 additions & 51 deletions
Original file line numberDiff line numberDiff line change
@@ -37,11 +37,9 @@
3737
import wmironpatriots.subsystems.elevator.Elevator;
3838
import wmironpatriots.subsystems.elevator.ElevatorIOComp;
3939
import wmironpatriots.subsystems.elevator.ElevatorIOSim;
40-
import wmironpatriots.subsystems.swerve.Swerve;
4140
import wmironpatriots.subsystems.tail.Tail;
4241
import wmironpatriots.subsystems.tail.TailIOComp;
4342
import wmironpatriots.subsystems.tail.TailIOSim;
44-
import wmironpatriots.util.deviceUtil.InputStream;
4543
import wmironpatriots.util.deviceUtil.OperatorController;
4644

4745
public class Robot extends TimedRobot implements Logged {
@@ -50,13 +48,13 @@ public class Robot extends TimedRobot implements Logged {
5048
private final CommandXboxController driveController;
5149
private final OperatorController operatorController;
5250

53-
private final Swerve swerve;
51+
// private final Swerve swerve;
5452
private final Tail tail;
5553
private final Elevator elevator;
5654
private final Chute chute;
5755

5856
private final RobotVisualizer visualizer;
59-
private final SendableChooser<Command> autonChooser;
57+
// private final SendableChooser<Command> autonChooser;
6058

6159
public Robot() {
6260
// * MONOLOGUE SETUP
@@ -104,14 +102,14 @@ public Robot() {
104102
driveController = new CommandXboxController(0);
105103
operatorController = new OperatorController(1);
106104

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();
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();
113111

114-
swerve = new Swerve(swerveSim);
112+
// swerve = new Swerve(swerveSim);
115113
if (Robot.isReal()) {
116114
tail = new TailIOComp();
117115
elevator = new ElevatorIOComp();
@@ -123,50 +121,50 @@ public Robot() {
123121
}
124122

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

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

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);
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);
139137

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

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)));
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)));
146144

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);
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);
153151

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

156154
elevator.setDefaultCommand(
157155
Commands.sequence(
158156
elevator.runPoseZeroingCmmd().onlyIf(() -> !elevator.isZeroed()),
159157
elevator.setTargetPoseCmmd(Elevator.IDLE).until(() -> elevator.inSetpointRange()),
160158
elevator.stopMotorInputCmmd()));
161159

162-
tail.setDefaultCommand(
163-
Commands.sequence(
164-
tail.runPoseZeroingCmmd()
165-
.onlyIf(() -> !tail.isZeroed() && Superstructure.isTailSafe(elevator, tail)),
166-
tail.setTargetPoseCmmd(Tail.POSE_OUT_RADS)
167-
.until(() -> Superstructure.isTailSafe(elevator, tail)),
168-
tail.setTargetPoseCmmd(Tail.POSE_IN_RADS).until(() -> tail.inSetpointRange()),
169-
tail.stopMotorInputCmmd()));
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()));
170168

171169
// * SUPERSTRUCTURE INIT
172170
Map<Requests, Trigger> triggerMap = new HashMap<Superstructure.Requests, Trigger>();
@@ -175,11 +173,10 @@ public Robot() {
175173
visualizer = new RobotVisualizer(elevator, tail);
176174

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

181179
new Superstructure(
182-
swerve,
183180
elevator,
184181
tail,
185182
chute,
@@ -198,14 +195,14 @@ public void robotPeriodic() {
198195

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

211208
@Override

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

Lines changed: 7 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -56,7 +56,6 @@ public static enum State {
5656
private boolean hasCoral = false;
5757

5858
public Superstructure(
59-
Swerve swerve,
6059
Elevator elevator,
6160
Tail tail,
6261
Chute chute,
@@ -112,7 +111,7 @@ public Superstructure(
112111
.whileTrue(
113112
tail.setTargetPoseCmmd(Tail.POSE_OUT_RADS)
114113
.until(() -> isTailSafe(elevator, tail))
115-
.andThen(tail.setTargetPoseCmmd(Tail.POSE_IN_RADS)))
114+
.andThen(tail.setTargetPoseCmmd(Tail.POSE_MIN_REVS)))
116115
.whileTrue(elevator.setTargetPoseCmmd(Elevator.POSE_INTAKING))
117116
.and(() -> elevator.inSetpointRange())
118117
.whileTrue(tail.setRollerSpeedCmmd(Tail.INTAKING_SPEEDS))
@@ -126,7 +125,7 @@ public Superstructure(
126125
.whileTrue(
127126
tail.setTargetPoseCmmd(Tail.POSE_OUT_RADS)
128127
.until(() -> isTailSafe(elevator, tail))
129-
.andThen(tail.setTargetPoseCmmd(Tail.POSE_IN_RADS)))
128+
.andThen(tail.setTargetPoseCmmd(Tail.POSE_MIN_REVS)))
130129
.whileTrue(elevator.setTargetPoseCmmd(Elevator.POSE_L1))
131130
.and(() -> elevator.inSetpointRange())
132131
.onTrue(setCurrentStateCommand(State.REEF_SCORE));
@@ -136,7 +135,7 @@ public Superstructure(
136135
.whileTrue(
137136
tail.setTargetPoseCmmd(Tail.POSE_OUT_RADS)
138137
.until(() -> isTailSafe(elevator, tail))
139-
.andThen(tail.setTargetPoseCmmd(Tail.POSE_IN_RADS)))
138+
.andThen(tail.setTargetPoseCmmd(Tail.POSE_MIN_REVS)))
140139
.whileTrue(elevator.setTargetPoseCmmd(Elevator.POSE_L2))
141140
.and(() -> elevator.inSetpointRange())
142141
.onTrue(setCurrentStateCommand(State.REEF_SCORE));
@@ -146,7 +145,7 @@ public Superstructure(
146145
.whileTrue(
147146
tail.setTargetPoseCmmd(Tail.POSE_OUT_RADS)
148147
.until(() -> isTailSafe(elevator, tail))
149-
.andThen(tail.setTargetPoseCmmd(Tail.POSE_IN_RADS)))
148+
.andThen(tail.setTargetPoseCmmd(Tail.POSE_MIN_REVS)))
150149
.whileTrue(elevator.setTargetPoseCmmd(Elevator.POSE_L3))
151150
.and(() -> elevator.inSetpointRange())
152151
.onTrue(setCurrentStateCommand(State.REEF_SCORE));
@@ -170,12 +169,11 @@ public Superstructure(
170169

171170
/** Checks to see if tail will hit top of carriage when stowed */
172171
public static boolean isTailSafe(Elevator elevator, Tail tail) {
173-
double vel = elevator.getVelocity();
174-
vel = MathUtil.applyDeadband(vel, 0.2); // deadbands speed
172+
double SetpointDisplacement = elevator.getSetpointDisplacement();
175173

176-
if (vel > 0 && elevator.getPose() < Elevator.POSE_MAX_CARRIAGE_STAGE_ONE) {
174+
if (SetpointDisplacement > 0 && elevator.getPose() < Elevator.POSE_MAX_CARRIAGE_STAGE_ONE) {
177175
return false;
178-
} else if (vel < 0 && elevator.getPose() > Elevator.POSE_MAX_CARRIAGE_STAGE_ONE) {
176+
} else if (SetpointDisplacement < 0 && elevator.getPose() > Elevator.POSE_MAX_CARRIAGE_STAGE_ONE) {
179177
return false;
180178
}
181179
return true;

‎src/main/java/wmironpatriots/subsystems/elevator/Elevator.java‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -117,6 +117,11 @@ public double getVelocity() {
117117
return velRPM;
118118
}
119119

120+
/** Gets displacement from setpoint pose */
121+
public double getSetpointDisplacement() {
122+
return setpointPoseRots - poseRevs;
123+
}
124+
120125
/** Checks if elevator has been zeroed */
121126
public boolean isZeroed() {
122127
return isZeroed;

‎src/main/java/wmironpatriots/subsystems/tail/Tail.java‎

Lines changed: 3 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,6 @@
99
import edu.wpi.first.wpilibj2.command.Command;
1010
import edu.wpi.first.wpilibj2.command.SubsystemBase;
1111
import monologue.Annotations.Log;
12-
import wmironpatriots.util.ntUtils.TunableDouble;
1312

1413
public abstract class Tail extends SubsystemBase {
1514
/** CONSTANTS */
@@ -23,22 +22,17 @@ public abstract class Tail extends SubsystemBase {
2322
public static final double CURRENT_LIMIT = 40.0;
2423

2524
// Poses
26-
public static final double POSE_IN_RADS = Math.PI / 2;
27-
public static final double POSE_OUT_RADS = Math.PI / 6;
28-
public static final double POSE_LNONFOUR_RADS = POSE_IN_RADS;
29-
public static final double POSE_L4_RADS = POSE_OUT_RADS;
25+
public static final double POSE_MIN_REVS = -7.357;
26+
public static final double POSE_OUT_RADS = 0;
3027

3128
// Roller speeds
3229
public static final double INTAKING_SPEEDS = 5;
3330
public static final double OUTTAKING_SPEEDS = 5;
3431

35-
public static final double PIVOT_P = 0.0;
32+
public static final double PIVOT_P = 0.3;
3633
public static final double PIVOT_I = 0.0;
3734
public static final double PIVOT_D = 0.0;
3835

39-
public static final double PIVOT_VELOCITY = 0.0;
40-
public static final double PIVOT_ACCELERATION = 0.0;
41-
4236
/** LOGGED VALUES */
4337
@Log protected boolean isZeroed = false;
4438

‎src/main/java/wmironpatriots/subsystems/tail/TailIOComp.java‎

Lines changed: 3 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -38,24 +38,18 @@ public TailIOComp() {
3838

3939
pivotConf
4040
.softLimit
41-
.forwardSoftLimit(POSE_IN_RADS)
41+
.forwardSoftLimit(POSE_MIN_REVS)
4242
.reverseSoftLimit(POSE_OUT_RADS)
4343
.forwardSoftLimitEnabled(true)
4444
.reverseSoftLimitEnabled(true);
4545

4646
pivotConf
4747
.closedLoop
4848
.pid(PIVOT_P, PIVOT_I, PIVOT_D)
49-
.feedbackSensor(FeedbackSensor.kPrimaryEncoder)
50-
.maxMotion
51-
.maxVelocity(PIVOT_VELOCITY)
52-
.maxAcceleration(PIVOT_ACCELERATION)
53-
.allowedClosedLoopError(0.1);
49+
.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
5450

5551
pivotConf
5652
.encoder
57-
.positionConversionFactor(2 * Math.PI * REDUCTION)
58-
.velocityConversionFactor(2 * Math.PI * REDUCTION)
5953
.uvwAverageDepth(16)
6054
.uvwMeasurementPeriod(32);
6155

@@ -108,7 +102,7 @@ protected void runRollerSpeed(double speed) {
108102

109103
@Override
110104
protected void setEncoderPose(double poseRads) {
111-
pivot.getEncoder().setPosition(poseRads / (2 * Math.PI));
105+
pivot.getEncoder().setPosition(poseRads);
112106
}
113107

114108
@Override

0 commit comments

Comments
 (0)