Skip to content

Commit 4acc1d7

Browse files
bobopop787bobopop787
committed
small fixes
- backward funnel unstuck - auto dealgae - prep x controller preset pos
1 parent 87df4ba commit 4acc1d7

File tree

3 files changed

+53
-15
lines changed

3 files changed

+53
-15
lines changed

‎src/main/java/frc/robot/subsystems/elevator/Elevator.java‎

Lines changed: 34 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -21,12 +21,19 @@ public class Elevator extends SubsystemBase {
2121
private boolean intaking;
2222
private boolean bouncing;
2323

24+
private double desiredFunnelVolts;
25+
private double lastUpperPhotosensorTrigger;
26+
27+
private boolean dealgaeRunning;
28+
2429
public Elevator(ElevatorIO io) {
2530
this.elevatorIO = io;
2631
elevatorIOInputs = new ElevatorIOInputsAutoLogged();
2732
openLoop = false;
2833
intaking = false;
2934
bouncing = true;
35+
dealgaeRunning = false;
36+
desiredFunnelVolts = 0;
3037
Preferences.initDouble("ele/leftvolts", 0);
3138
Preferences.initDouble("ele/rightvolts", 0);
3239
Preferences.initDouble("ele/elevatorvolts", 0);
@@ -75,7 +82,8 @@ public Command runToElevatorPosition(ElevatorPosition position) {
7582
openLoop = false;
7683
if(intaking) {
7784
elevatorIO.setEffectorVolts(0, 0);
78-
elevatorIO.setFunnelMotorVolts(0);
85+
// elevatorIO.setFunnelMotorVolts(0);
86+
desiredFunnelVolts = 0;
7987
intaking = false;
8088
}
8189
elevatorIO.setElevatorPosition(position);
@@ -110,7 +118,8 @@ public Command runaEffectorPreferences() {
110118
}
111119

112120
public Command runSetFunnelVolts(double volts) {
113-
return runOnce(() -> elevatorIO.setFunnelMotorVolts(volts));
121+
// return runOnce(() -> elevatorIO.setFunnelMotorVolts(volts));
122+
return runOnce(() -> desiredFunnelVolts = volts);
114123
}
115124

116125
public Command runSetDealgaeVolts(double volts) {
@@ -155,10 +164,28 @@ public void periodic() {
155164
}
156165
if(!openLoop) {
157166
if(elevatorIOInputs.desiredPosition.equals(ElevatorPosition.HOME) && !hasCoral() && bouncing) {
158-
elevatorIO.setElevatorPosition(ElevatorPosition.HOME.height + (0.003 * Math.sin(Timer.getFPGATimestamp() * 12)));
167+
elevatorIO.setElevatorPosition(ElevatorPosition.HOME.height + (0.005 * Math.sin(Timer.getFPGATimestamp() * 12)));
159168
} else elevatorIO.setElevatorPosition(elevatorIOInputs.desiredHeight);
160169
// elevatorIO.setElevatorPosition(elevatorIOInputs.desiredPosition);
161170
}
171+
172+
if(elevatorIOInputs.upperPhotosensor) {
173+
lastUpperPhotosensorTrigger = Timer.getFPGATimestamp();
174+
}
175+
if(Timer.getFPGATimestamp() - lastUpperPhotosensorTrigger < 1.5 && !elevatorIOInputs.lowerPhotosensor) {
176+
elevatorIO.setFunnelMotorVolts(Timer.getFPGATimestamp() % 6 > 1.5 && Timer.getFPGATimestamp() % 1.5 > 0.75 ? -desiredFunnelVolts : desiredFunnelVolts);
177+
} else {
178+
elevatorIO.setFunnelMotorVolts(desiredFunnelVolts);
179+
}
180+
181+
if(!dealgaeRunning && elevatorIOInputs.elevatorHeight > 0.05) {
182+
dealgaeRunning = true;
183+
elevatorIO.setDealgaeMotorVolts(6);
184+
}
185+
if(dealgaeRunning && elevatorIOInputs.elevatorHeight < 0.05) {
186+
dealgaeRunning = false;
187+
elevatorIO.setDealgaeMotorVolts(0);
188+
}
162189
}
163190

164191
/**
@@ -204,11 +231,13 @@ public Command runIntakeEffector(double effectorVolts, double funnelVolts, Boole
204231
if(elevatorIOInputs.desiredPosition.equals(ElevatorPosition.HOME) && intaking) {
205232
intaking = false;
206233
elevatorIO.setEffectorVolts(0, 0);
207-
elevatorIO.setFunnelMotorVolts(0);
234+
// elevatorIO.setFunnelMotorVolts(0);
235+
desiredFunnelVolts = 0;
208236
} else {
209237
if(elevatorIOInputs.desiredPosition.equals(ElevatorPosition.HOME)) {
210238
intaking = true;
211-
elevatorIO.setFunnelMotorVolts(funnelVolts);
239+
// elevatorIO.setFunnelMotorVolts(funnelVolts);
240+
desiredFunnelVolts = funnelVolts;
212241
elevatorIO.setEffectorVolts(-effectorVolts, effectorVolts);
213242
} else if(isAlignedSupplier.getAsBoolean()) {
214243
if(elevatorIOInputs.desiredPosition.equals(ElevatorPosition.L1)) {

‎src/main/java/frc/robot/subsystems/elevator/ElevatorPosition.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
package frc.robot.subsystems.elevator;
22

33
public enum ElevatorPosition {
4-
HOME("Home", 0.007),
4+
HOME("Home", 0.005),
55
L1("L1", 0.327),
66
L2("L2", 0.452),
77
L3("L3", 0.609),

‎src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java‎

Lines changed: 18 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -14,6 +14,7 @@
1414
import edu.wpi.first.math.MathUtil;
1515
import edu.wpi.first.math.Matrix;
1616
import edu.wpi.first.math.controller.PIDController;
17+
import edu.wpi.first.math.controller.ProfiledPIDController;
1718
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
1819
import edu.wpi.first.math.geometry.Pose2d;
1920
import edu.wpi.first.math.geometry.Rotation2d;
@@ -26,6 +27,7 @@
2627
import edu.wpi.first.math.kinematics.SwerveModuleState;
2728
import edu.wpi.first.math.numbers.N1;
2829
import edu.wpi.first.math.numbers.N3;
30+
import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
2931
import edu.wpi.first.math.util.Units;
3032
import edu.wpi.first.wpilibj.Alert;
3133
import edu.wpi.first.wpilibj.DriverStation;
@@ -65,7 +67,8 @@ public class SwerveDrive extends SubsystemBase {
6567
private PIDController trajHeadingController;
6668

6769
private PIDController presetRotController;
68-
private PIDController presetPosController;
70+
private ProfiledPIDController presetXController;
71+
private PIDController presetYController;
6972

7073
private FieldZones fieldZone;
7174

@@ -129,12 +132,18 @@ private void initMathModels() {
129132
);
130133
poseEstimator = new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, modulePositions, (Constants.isRed() ? new Pose2d(17.548, 8.052, Rotation2d.kPi) : new Pose2d()));
131134

132-
presetPosController = new PIDController(
135+
presetXController = new ProfiledPIDController(
136+
SwerveConstants.kPresetPosControlConstants.kP(),
137+
SwerveConstants.kPresetPosControlConstants.kI(),
138+
SwerveConstants.kPresetPosControlConstants.kD(),
139+
new Constraints(SwerveConstants.kMagVelLimit, SwerveConstants.kMagVelLimit * 2)
140+
);
141+
presetYController = new PIDController(
133142
SwerveConstants.kPresetPosControlConstants.kP(),
134143
SwerveConstants.kPresetPosControlConstants.kI(),
135144
SwerveConstants.kPresetPosControlConstants.kD()
136145
);
137-
presetPosController.setTolerance(0.01);
146+
presetYController.setTolerance(0.01);
138147

139148
trajVXController = new PIDController(10, 0, 0);
140149
trajVYController = new PIDController(10, 0, 0);
@@ -321,7 +330,7 @@ public void injectPresetPosition(ChassisSpeeds chassisSpeeds, boolean fieldRelat
321330
errorPose = errorPose.rotateAround(Translation2d.kZero, desiredPose.getRotation().unaryMinus());
322331
Logger.recordOutput("Swerve/desiredPose", desiredPose);
323332
Logger.recordOutput("Swerve/errorPose", errorPose);
324-
double vyReefRelative = presetPosController.calculate(errorPose.getY(), 0);
333+
double vyReefRelative = presetYController.calculate(errorPose.getY(), 0);
325334
if(vyReefRelative > 0.4) vyReefRelative *= elevatorSpeedFactor; // TODO double check when higher kP
326335
Logger.recordOutput("Swerve/vyReefRelative", vyReefRelative);
327336

@@ -337,8 +346,8 @@ public void injectPresetPosition(ChassisSpeeds chassisSpeeds, boolean fieldRelat
337346
break;
338347
case PROCESSOR:
339348
double vx = switch(Constants.kFieldType.getSelected()) {
340-
case ANDYMARK -> presetPosController.calculate(getPose().getX(), Constants.isRed() ? FieldConstants.fieldWidth - 6.27 : 6.27);
341-
case WELDED -> presetPosController.calculate(getPose().getX(), Constants.isRed() ? FieldConstants.fieldWidth - 6.18 : 6.18);
349+
case ANDYMARK -> presetYController.calculate(getPose().getX(), Constants.isRed() ? FieldConstants.fieldWidth - 6.27 : 6.27);
350+
case WELDED -> presetYController.calculate(getPose().getX(), Constants.isRed() ? FieldConstants.fieldWidth - 6.18 : 6.18);
342351
default -> 0;
343352
} * elevatorSpeedFactor;
344353
fieldRelativeSpeeds = fieldRelative ?
@@ -502,9 +511,9 @@ public Command runUpdateControlConstants() {
502511
for(SDSSwerveModule module : modules) {
503512
module.updateControlConstants();
504513
}
505-
presetPosController.setP(SwerveConstants.kPresetRotControlConstants.kP());
506-
presetPosController.setI(SwerveConstants.kPresetRotControlConstants.kI());
507-
presetPosController.setD(SwerveConstants.kPresetRotControlConstants.kD());
514+
presetYController.setP(SwerveConstants.kPresetRotControlConstants.kP());
515+
presetYController.setI(SwerveConstants.kPresetRotControlConstants.kI());
516+
presetYController.setD(SwerveConstants.kPresetRotControlConstants.kD());
508517
System.out.println("Swerve control constants updated");
509518
});
510519
}

0 commit comments

Comments
 (0)