Skip to content

Commit 667851e

Browse files
dabeycorndabeycorn
committed
cooked
Signed-off-by: Dasun L. Abeykoon <[email protected]>
1 parent 1d00a73 commit 667851e

File tree

6 files changed

+44
-38
lines changed

6 files changed

+44
-38
lines changed

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

Lines changed: 11 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -194,24 +194,24 @@ public Robot() {
194194
@Override
195195
public void robotPeriodic() {
196196
CommandScheduler.getInstance().run();
197-
// if (Robot.isSimulation()) {
198-
// SimulatedArena.getInstance().simulationPeriodic();
199-
// }
197+
if (Robot.isSimulation()) {
198+
SimulatedArena.getInstance().simulationPeriodic();
199+
}
200200

201201
Monologue.updateAll();
202202
visualizer.periodic();
203203
}
204204

205205
@Override
206206
public void autonomousInit() {
207-
// Command auton =
208-
// autonChooser
209-
// .getSelected()
210-
// .withDeadline(Commands.waitUntil(() -> DriverStation.isEnabled()));
211-
212-
// if (auton != null) {
213-
// auton.schedule();
214-
// }
207+
Command auton =
208+
autonChooser
209+
.getSelected()
210+
.withDeadline(Commands.waitUntil(() -> DriverStation.isEnabled()));
211+
212+
if (auton != null) {
213+
auton.schedule();
214+
}
215215
}
216216

217217
@Override

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

Lines changed: 6 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,9 @@
88

99
import static wmironpatriots.Constants.DT_TIME;
1010

11+
import com.fasterxml.jackson.databind.util.Named;
1112
import com.pathplanner.lib.auto.AutoBuilder;
13+
import com.pathplanner.lib.auto.NamedCommands;
1214
import com.pathplanner.lib.config.ModuleConfig;
1315
import com.pathplanner.lib.config.PIDConstants;
1416
import com.pathplanner.lib.config.RobotConfig;
@@ -18,8 +20,11 @@
1820
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
1921
import edu.wpi.first.wpilibj2.command.Command;
2022
import edu.wpi.first.wpilibj2.command.Commands;
23+
import edu.wpi.first.wpilibj2.command.ScheduleCommand;
24+
import wmironpatriots.subsystems.elevator.Elevator;
2125
import wmironpatriots.subsystems.swerve.Swerve;
2226
import wmironpatriots.subsystems.swerve.module.Module;
27+
import wmironpatriots.subsystems.tail.Tail;
2328

2429
public class Autonomous {
2530
public static SendableChooser<Command> configureAutons(Swerve swerve) {
@@ -57,4 +62,4 @@ public static SendableChooser<Command> configureAutons(Swerve swerve) {
5762
choser.addOption("play dead", Commands.none());
5863
return choser;
5964
}
60-
}
65+
}

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

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

77
package wmironpatriots.subsystems;
88

9+
import edu.wpi.first.math.MathUtil;
910
import edu.wpi.first.wpilibj.Timer;
1011
import edu.wpi.first.wpilibj2.command.Command;
1112
import edu.wpi.first.wpilibj2.command.Commands;
@@ -168,6 +169,7 @@ public Superstructure(
168169
/** Checks to see if tail will hit top of carriage when stowed */
169170
public static boolean isTailSafe(Elevator elevator, Tail tail) {
170171
double SetpointDisplacement = elevator.getSetpointDisplacement();
172+
SetpointDisplacement = MathUtil.applyDeadband(SetpointDisplacement, 0.2);
171173

172174
if (SetpointDisplacement > 0 && elevator.getPose() < Elevator.POSE_MAX_CARRIAGE_STAGE_ONE) {
173175
return false;

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

Lines changed: 10 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -38,7 +38,7 @@ protected static TalonFXConfiguration getPivotConfig() {
3838
config.CurrentLimits.SupplyCurrentLimitEnable = true;
3939

4040
config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder;
41-
// config.Feedback.SensorToMechanismRatio = PIVOT_REDUCTION;
41+
config.Feedback.RotorToSensorRatio = PIVOT_REDUCTION;
4242
config.Slot0.kP = 20.0;
4343
config.Slot0.kI = 0.0;
4444
config.Slot0.kD = 0.68275;
@@ -60,7 +60,7 @@ protected static TalonFXConfiguration getDriveConfig() {
6060
config.CurrentLimits.StatorCurrentLimit = 120.0;
6161
config.CurrentLimits.StatorCurrentLimitEnable = true;
6262

63-
// config.Feedback.SensorToMechanismRatio = DRIVE_REDUCTION;
63+
config.Feedback.SensorToMechanismRatio = DRIVE_REDUCTION;
6464
// TODO TUNE VALUES
6565
config.Slot0.kA = 0.33291;
6666
config.Slot0.kS = 0.088468;
@@ -71,15 +71,16 @@ protected static TalonFXConfiguration getDriveConfig() {
7171
}
7272

7373
protected static CANcoderConfiguration getCANcoderConfig() {
74-
return new CANcoderConfiguration();
74+
CANcoderConfiguration config = new CANcoderConfiguration();
75+
return config;
7576
}
7677

7778
public static record ModuleConfig(
7879
int index,
7980
int pivotID,
8081
int driveID,
8182
int cancoderID,
82-
double cancoderOffsetRads,
83+
double cancoderOffsetRevs,
8384
boolean pivotInverted) {}
8485
;
8586

@@ -88,9 +89,9 @@ public static record ModuleConfig(
8889

8990
@Log public boolean driveOk = false;
9091

91-
@Log public double pivotABSPoseRads = 0.0;
92-
@Log public double pivotPoseRads = 0.0;
93-
@Log public double pivotVelRadsPerSec = 0.0;
92+
@Log public double pivotABSPoseRevs = 0.0;
93+
@Log public double pivotPoseRevs = 0.0;
94+
@Log public double pivotVelRevsPerSec = 0.0;
9495
@Log public double pivotAppliedVolts = 0.0;
9596
@Log public double pivotSupplyCurrent = 0.0;
9697
@Log public double pivotTorqueCurrent = 0.0;
@@ -136,7 +137,7 @@ public void moduleCoastingEnabled(boolean enabled) {
136137
}
137138

138139
public Rotation2d getCurrentAngle() {
139-
return Rotation2d.fromRotations(pivotPoseRads);
140+
return Rotation2d.fromRotations(pivotPoseRevs);
140141
}
141142

142143
public SwerveModulePosition getModulePose() {
@@ -150,7 +151,7 @@ public SwerveModuleState getModuleState() {
150151
/** HARDWARE METHODS */
151152
protected abstract void runPivotVolts(double volts);
152153

153-
protected abstract void runPivotPose(double poseRads);
154+
protected abstract void runPivotPose(double poseRevs);
154155

155156
protected void runDriveVolts(double volts) {
156157
runDriveVolts(volts, true);

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

Lines changed: 7 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -55,7 +55,7 @@ public ModuleIOComp(ModuleConfig config) {
5555
config.pivotInverted()
5656
? InvertedValue.Clockwise_Positive
5757
: InvertedValue.CounterClockwise_Positive;
58-
pivotConf.Feedback.FeedbackRotorOffset = config.cancoderOffsetRads() / (2 * Math.PI);
58+
pivotConf.Feedback.FeedbackRotorOffset = config.cancoderOffsetRevs();
5959
pivotConf.Feedback.FeedbackRemoteSensorID = config.cancoderID();
6060

6161
pivot.getConfigurator().apply(pivotConf);
@@ -120,15 +120,14 @@ public void periodic() {
120120
pivotOk = true;
121121
driveOk = true; // TODO add a debouncer
122122

123-
double rotsToRads = (2 * Math.PI);
124-
125-
pivotABSPoseRads = cancoderPose.getValueAsDouble();
126-
pivotPoseRads = pivotPose.getValueAsDouble();
127-
pivotVelRadsPerSec = pivotVel.getValueAsDouble() * rotsToRads;
123+
pivotABSPoseRevs = cancoderPose.getValueAsDouble();
124+
pivotPoseRevs = pivotPose.getValueAsDouble();
125+
pivotVelRevsPerSec = pivotVel.getValueAsDouble();
128126
pivotAppliedVolts = pivotVolts.getValueAsDouble();
129127
pivotSupplyCurrent = pivotSCurrent.getValueAsDouble();
130128
pivotTorqueCurrent = pivotTCurrent.getValueAsDouble();
131129

130+
double rotsToRads = (2 * Math.PI);
132131
drivePoseMeters = drivePose.getValueAsDouble() * rotsToRads * WHEEL_RADIUS_METERS;
133132
driveVelMPS = driveVel.getValueAsDouble() * rotsToRads * WHEEL_RADIUS_METERS;
134133
driveAppliedVolts = driveVolts.getValueAsDouble();
@@ -142,8 +141,8 @@ protected void runPivotVolts(double volts) {
142141
}
143142

144143
@Override
145-
protected void runPivotPose(double poseRads) {
146-
pivot.setControl(reqPivotFeedback.withPosition(poseRads / (2 * Math.PI)));
144+
protected void runPivotPose(double poseRevs) {
145+
pivot.setControl(reqPivotFeedback.withPosition(poseRevs));
147146
}
148147

149148
@Override

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

Lines changed: 8 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -61,7 +61,7 @@ public ModuleIOSim(ModuleConfig config, SwerveModuleSimulation simulation) {
6161
config.pivotInverted()
6262
? InvertedValue.Clockwise_Positive
6363
: InvertedValue.CounterClockwise_Positive;
64-
pivotConf.Feedback.FeedbackRotorOffset = config.cancoderOffsetRads() / (2 * Math.PI);
64+
pivotConf.Feedback.FeedbackRotorOffset = config.cancoderOffsetRevs();
6565

6666
pivot.getConfigurator().apply(pivotConf);
6767
drive.getConfigurator().apply(driveConf);
@@ -132,15 +132,14 @@ public void periodic() {
132132
pivotOk = true;
133133
driveOk = true; // TODO add a debouncer
134134

135-
double rotsToRads = (2 * Math.PI);
136-
137-
pivotABSPoseRads = cancoderPose.getValueAsDouble() * rotsToRads;
138-
pivotPoseRads = pivotPose.getValueAsDouble() * rotsToRads;
139-
pivotVelRadsPerSec = pivotVel.getValueAsDouble() * rotsToRads;
135+
pivotABSPoseRevs = cancoderPose.getValueAsDouble();
136+
pivotPoseRevs = pivotPose.getValueAsDouble();
137+
pivotVelRevsPerSec = pivotVel.getValueAsDouble();
140138
pivotAppliedVolts = pivotVolts.getValueAsDouble();
141139
pivotSupplyCurrent = pivotSCurrent.getValueAsDouble();
142140
pivotTorqueCurrent = pivotTCurrent.getValueAsDouble();
143141

142+
double rotsToRads = (2 * Math.PI);
144143
drivePoseMeters = drivePose.getValueAsDouble() * rotsToRads * WHEEL_RADIUS_METERS;
145144
driveVelMPS = driveVel.getValueAsDouble() * rotsToRads * WHEEL_RADIUS_METERS;
146145
driveAppliedVolts = driveVolts.getValueAsDouble();
@@ -154,9 +153,9 @@ protected void runPivotVolts(double volts) {
154153
}
155154

156155
@Override
157-
protected void runPivotPose(double poseRads) {
158-
System.out.println(poseRads);
159-
pivot.setControl(reqPivotFeedback.withPosition(poseRads / (2 * Math.PI)));
156+
protected void runPivotPose(double poseRevs) {
157+
System.out.println(poseRevs);
158+
pivot.setControl(reqPivotFeedback.withPosition(poseRevs));
160159
}
161160

162161
@Override

0 commit comments

Comments
 (0)