Skip to content

Commit 2572b6d

Browse files
Spikes-2212Spikes-2212
committed
sysID and other things
1 parent 7eb85ad commit 2572b6d

File tree

5 files changed

+54
-24
lines changed

5 files changed

+54
-24
lines changed

‎src/main/java/frc/robot/commands/district2/District2RotateStorage.java

Lines changed: 33 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -5,36 +5,62 @@
55
import com.spikes2212.control.FeedForwardSettings;
66
import com.spikes2212.control.PIDSettings;
77
import com.spikes2212.dashboard.RootNamespace;
8+
import com.spikes2212.dashboard.SpikesLogger;
9+
import frc.robot.subsystems.Storage;
810
import frc.robot.subsystems.district2.District2CoralJoint;
911

1012
import java.util.function.Supplier;
1113

1214
public class District2RotateStorage extends MoveGenericSubsystemWithPID {
1315

1416
private static final RootNamespace namespace = new RootNamespace("district 2 rotate storage");
15-
private static final PIDSettings intakePIDSettings = namespace.addPIDNamespace("rotate storage",
17+
private static final PIDSettings intakePIDSettings = namespace.addPIDNamespace("intake rotate storage",
1618
new PIDSettings(0.11, 0, 0.005, 0, 3, 0.5));
17-
private static final FeedForwardSettings intakeFeedForwardSettings = namespace.addFeedForwardNamespace("rotate storage",
18-
new FeedForwardSettings(0, 0, 0, 0.082, FeedForwardController.ControlMode.ANGULAR_POSITION));
19-
private static final PIDSettings outtakePIDSettings = namespace.addPIDNamespace("rotate storage",
20-
new PIDSettings(0.11, 0, 0.005, 0, 3, 0.5));
21-
private static final FeedForwardSettings outtakeFeedForwardSettings = namespace.addFeedForwardNamespace("rotate storage",
19+
private static final FeedForwardSettings intakeFeedForwardSettings = namespace.addFeedForwardNamespace("intake rotate storage",
2220
new FeedForwardSettings(0, 0, 0, 0.082, FeedForwardController.ControlMode.ANGULAR_POSITION));
21+
private static final PIDSettings outtakePIDSettings = namespace.addPIDNamespace("outtake rotate storage",
22+
new PIDSettings(0.15, 0, 0.012, 0, 3, 0.5));
23+
private static final FeedForwardSettings outtakeFeedForwardSettings = namespace.addFeedForwardNamespace("outtake rotate storage",
24+
new FeedForwardSettings(0, 0, 0, 0.12, FeedForwardController.ControlMode.ANGULAR_POSITION));
2325

2426
private final District2CoralJoint coralJoint;
27+
private SpikesLogger logger = new SpikesLogger();
2528

2629
public District2RotateStorage(District2CoralJoint coralJoint, Supplier<Double> setpoint) {
2730
super(coralJoint, () -> Math.toRadians(setpoint.get()), () -> Math.toRadians(coralJoint.getPosition()),
28-
intakePIDSettings, intakeFeedForwardSettings);
31+
Storage.getInstance().hasCoral() ? outtakePIDSettings : intakePIDSettings,
32+
Storage.getInstance().hasCoral() ? outtakeFeedForwardSettings : intakeFeedForwardSettings);
33+
pidSettings.setWaitTime(() -> 999.0);
2934
this.coralJoint = coralJoint;
35+
logger.log("m" + pidSettings.getWaitTime());
3036
}
3137

3238
public District2RotateStorage(District2CoralJoint coralJoint, District2CoralJoint.StoragePose pose) {
3339
this(coralJoint, () -> pose.neededPitch);
3440
}
3541

42+
@Override
43+
protected double calculatePIDAndFFValues() {
44+
if (Storage.getInstance().hasCoral()) {
45+
pidController.setTolerance(Math.toRadians(outtakePIDSettings.getTolerance()));
46+
pidController.setPID(outtakePIDSettings.getkP(), outtakePIDSettings.getkI(), outtakePIDSettings.getkD());
47+
pidController.setIZone(outtakePIDSettings.getIZone());
48+
feedForwardController.setGains(outtakeFeedForwardSettings);
49+
} else {
50+
pidController.setTolerance(Math.toRadians(intakePIDSettings.getTolerance()));
51+
pidController.setPID(intakePIDSettings.getkP(), intakePIDSettings.getkI(), intakePIDSettings.getkD());
52+
pidController.setIZone(intakePIDSettings.getIZone());
53+
feedForwardController.setGains(intakeFeedForwardSettings);
54+
}
55+
56+
double pidValue = pidController.calculate(source.get(), setpoint.get());
57+
double svagValue = feedForwardController.calculate(source.get(), setpoint.get());
58+
return pidValue + svagValue;
59+
}
60+
3661
@Override
3762
public boolean isFinished() {
63+
logger.log(source.get() + ", " + setpoint.get());
3864
return !(coralJoint.canMove(coralJoint.getSpeed())) || super.isFinished();
3965
}
4066
}

‎src/main/java/frc/robot/subsystems/Drivetrain.java

Lines changed: 18 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -7,23 +7,25 @@
77
import com.pathplanner.lib.config.RobotConfig;
88
import com.pathplanner.lib.controllers.PPHolonomicDriveController;
99
import com.spikes2212.command.DashboardedSubsystem;
10+
import com.spikes2212.control.FeedForwardController;
11+
import com.spikes2212.control.FeedForwardSettings;
1012
import com.studica.frc.AHRS;
13+
import edu.wpi.first.math.controller.PIDController;
1114
import edu.wpi.first.math.geometry.Pose2d;
1215
import edu.wpi.first.math.geometry.Rotation2d;
1316
import edu.wpi.first.math.geometry.Translation2d;
1417
import edu.wpi.first.math.kinematics.*;
1518
import edu.wpi.first.networktables.NetworkTableInstance;
1619
import edu.wpi.first.networktables.StructArrayPublisher;
20+
import edu.wpi.first.units.Units;
1721
import edu.wpi.first.units.measure.Voltage;
1822
import edu.wpi.first.wpilibj.DriverStation;
23+
import edu.wpi.first.wpilibj.RobotController;
1924
import edu.wpi.first.wpilibj2.command.InstantCommand;
2025
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
2126
import frc.robot.commands.Drive;
22-
import frc.robot.commands.IntakeAlgae;
2327
import frc.robot.util.VisionService;
2428

25-
import java.util.function.Supplier;
26-
2729
public class Drivetrain extends DashboardedSubsystem {
2830

2931
public static final double MAX_SPEED = 4;
@@ -142,7 +144,7 @@ private Drivetrain(SwerveModule frontLeft, SwerveModule frontRight, SwerveModule
142144
);
143145
sysIdRoutine = new SysIdRoutine(
144146
new SysIdRoutine.Config(null, null, null, state -> SignalLogger.writeString("state", state.toString())),
145-
new SysIdRoutine.Mechanism(this::voltageMove, null, this)
147+
new SysIdRoutine.Mechanism(this::sysID, null, this)
146148
);
147149
configureDashboard();
148150
}
@@ -191,11 +193,18 @@ public void drive(double xSpeed, double ySpeed, double rotationSpeed, boolean fi
191193
desiredStates.set(states);
192194
}
193195

194-
public void voltageMove(Voltage voltage) {
195-
frontLeft.sysID(voltage);
196-
frontRight.sysID(voltage);
197-
backLeft.sysID(voltage);
198-
backRight.sysID(voltage);
196+
private final PIDController turnPIDController = new PIDController(0, 0, 0);
197+
private final FeedForwardController turnFeedForwardController = new FeedForwardController(
198+
new FeedForwardSettings(0, 0, 0, FeedForwardController.ControlMode.LINEAR_POSITION)
199+
);
200+
public void sysID(Voltage voltage) {
201+
drive((voltage.in(Units.Volts) / RobotController.getBatteryVoltage()) * MAX_SPEED, 0,
202+
turnPIDController.calculate(getYaw(), 0) + turnFeedForwardController.calculate(getYaw(), 0),
203+
true, false, 0.02);
204+
}
205+
206+
public void sysIDNoAngle(Voltage voltage) {
207+
drive((voltage.in(Units.Volts) / RobotController.getBatteryVoltage()) * MAX_SPEED, 0, 0, true, false, 0.02);
199208
}
200209

201210
public void stop() {

‎src/main/java/frc/robot/subsystems/Storage.java

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -38,7 +38,6 @@ private Storage(String namespaceName, SparkWrapper motor, DigitalInput infrared)
3838
public void periodic() {
3939
super.periodic();
4040
SpikesLogger logger = new SpikesLogger();
41-
logger.log("a, " + isRunning());
4241
}
4342

4443
public boolean hasCoral() {

‎src/main/java/frc/robot/subsystems/SwerveModule.java

Lines changed: 0 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -109,11 +109,6 @@ public void setIdleMode(NeutralModeValue neutralMode) {
109109
driveMotor.setIdleMode(neutralMode);
110110
}
111111

112-
public void sysID(Voltage voltage) {
113-
setSpeed(voltage.in(Volts), false);
114-
setAngle(0);
115-
}
116-
117112
private void setAngle(double angle) {
118113
turnMotor.pidSet(UnifiedControlMode.POSITION, angle, turnPIDSettings, turnFeedForwardSettings, false);
119114
}

‎src/main/java/frc/robot/subsystems/district2/District2CoralJoint.java

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,7 @@
11
package frc.robot.subsystems.district2;
22

33
import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
4+
import com.ctre.phoenix6.configs.VoltageConfigs;
45
import com.ctre.phoenix6.signals.NeutralModeValue;
56
import com.spikes2212.command.genericsubsystem.commands.MoveGenericSubsystem;
67
import com.spikes2212.command.genericsubsystem.commands.MoveGenericSubsystemWithPID;
@@ -23,7 +24,7 @@ public class District2CoralJoint extends SmartMotorControllerGenericSubsystem {
2324

2425
public enum StoragePose {
2526

26-
INTAKE(0.06 * DEGREES_IN_ROTATIONS), L1(-0.075 * DEGREES_IN_ROTATIONS), L2(-0.018 * DEGREES_IN_ROTATIONS),
27+
INTAKE(-22), L1(-0.075 * DEGREES_IN_ROTATIONS), L2(-44),
2728
L3(0.13 * DEGREES_IN_ROTATIONS), RESTING(-0.2075 * DEGREES_IN_ROTATIONS);
2829
// RESTING(0.0);
2930

@@ -110,7 +111,7 @@ public void end(boolean True) {
110111
});
111112
namespace.putNumber("voltage", talonFX::get);
112113
namespace.putCommand("pid", new District2RotateStorage(this, setpoint));
113-
namespace.putCommand("dont kys", new MoveGenericSubsystem(this, 0.02));
114+
namespace.putCommand("dont kys", new MoveGenericSubsystem(this, 0.032));
114115
// namespace.putCommand("pid", new MoveGenericSubsystemWithPID(this, setpoint, talonFX::getPosition,
115116
// pidSettings, feedForwardSettings));
116117
}

0 commit comments

Comments
 (0)