Skip to content

Commit de46fd2

Browse files
BloodhatamBloodhatam
committed
[FIXED, NEEDS TEST] Swerve drive final config
1 parent 4c23912 commit de46fd2

File tree

3 files changed

+50
-47
lines changed

3 files changed

+50
-47
lines changed

‎src/main/java/frc/robot/Robot.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -47,7 +47,7 @@ public Robot() {
4747
switch (Constants.currentMode) {
4848
case REAL:
4949
// Running on a real robot, log to a USB stick ("/U/logs")
50-
Logger.addDataReceiver(new WPILOGWriter());
50+
// Logger.addDataReceiver(new WPILOGWriter());
5151
Logger.addDataReceiver(new NT4Publisher());
5252
break;
5353

‎src/main/java/frc/robot/subsystems/drive/DriveConstants.java‎

Lines changed: 17 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -31,10 +31,10 @@ public class DriveConstants {
3131
};
3232

3333
// Zeroed rotation values for each module, see setup instructions
34-
public static final Rotation2d frontLeftZeroRotation = new Rotation2d(0.0);
35-
public static final Rotation2d frontRightZeroRotation = new Rotation2d(0.0);
36-
public static final Rotation2d backLeftZeroRotation = new Rotation2d(0.0);
37-
public static final Rotation2d backRightZeroRotation = new Rotation2d(0.0);
34+
public static final Rotation2d frontLeftZeroRotation = new Rotation2d(-0.532);
35+
public static final Rotation2d frontRightZeroRotation = new Rotation2d(1.275);
36+
public static final Rotation2d backLeftZeroRotation = new Rotation2d(-0.451);
37+
public static final Rotation2d backRightZeroRotation = new Rotation2d(1.968);
3838

3939
// Device CAN IDs
4040
public static final int pigeonCanId = 11;
@@ -49,6 +49,11 @@ public class DriveConstants {
4949
public static final int frontRightTurnCanId = 5;
5050
public static final int backRightTurnCanId = 7;
5151

52+
public static final int frontLeftAbsoluteEncoderChannel = 0;
53+
public static final int frontRightAbsoluteEncoderChannel = 1;
54+
public static final int backLeftAbsoluteEncoderChannel = 3;
55+
public static final int backRightAbsoluteEncoderChannel = 2;
56+
5257
// Drive motor configuration
5358
public static final int driveMotorCurrentLimit = 40;
5459
public static final double wheelRadiusMeters = Meter.convertFrom(5, Centimeter);
@@ -77,19 +82,22 @@ public class DriveConstants {
7782
public static final double driveSimKv = 0.0789;
7883

7984
// Turn motor configuration
80-
public static final boolean turnInverted = false;
85+
public static final boolean turnInverted = true;
8186
public static final int turnMotorCurrentLimit = 20;
8287
public static final double turnMotorReduction = /*9424.0 / 203.0*/ 21.42;
8388
public static final DCMotor turnGearbox = DCMotor.getNEO(1);
8489

8590
// Turn encoder configuration
86-
public static final boolean turnEncoderInverted = true;
91+
public static final boolean turnEncoderInverted = false;
8792
public static final double turnEncoderPositionFactor =
88-
2 * Math.PI * 360; // Rotations (0-1) * 360deg to convert 0-1 range to 0-360 -> Radians
89-
public static final double turnEncoderVelocityFactor = (2 * Math.PI) / 60.0; // RPM -> Rad/Sec
93+
2
94+
* Math.PI
95+
/ turnMotorReduction; // Rotations (0-1) * 360deg to convert 0-1 range to 0-360 -> Radians
96+
public static final double turnEncoderVelocityFactor =
97+
(2 * Math.PI) / 60.0 / turnMotorReduction; // RPM -> Rad/Sec
9098

9199
// Turn PID configuration
92-
public static final double turnKp = 0.005;
100+
public static final double turnKp = 0.2;
93101
public static final double turnKd = 0.0;
94102
public static final double turnSimP = 8.0;
95103
public static final double turnSimD = 0.0;

‎src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java‎

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

88
package frc.robot.subsystems.drive;
99

10+
import static edu.wpi.first.units.Units.Degree;
11+
import static edu.wpi.first.units.Units.Radians;
1012
import static frc.robot.subsystems.drive.DriveConstants.*;
1113
import static frc.robot.util.SparkUtil.*;
1214

@@ -19,11 +21,9 @@
1921
import com.revrobotics.spark.SparkBase.ControlType;
2022
import com.revrobotics.spark.SparkClosedLoopController;
2123
import com.revrobotics.spark.SparkClosedLoopController.ArbFFUnits;
22-
import com.revrobotics.spark.SparkFlex;
2324
import com.revrobotics.spark.SparkLowLevel.MotorType;
2425
import com.revrobotics.spark.SparkMax;
2526
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
26-
import com.revrobotics.spark.config.SparkFlexConfig;
2727
import com.revrobotics.spark.config.SparkMaxConfig;
2828
import edu.wpi.first.math.MathUtil;
2929
import edu.wpi.first.math.filter.Debouncer;
@@ -43,7 +43,9 @@ public class ModuleIOSpark implements ModuleIO {
4343
private final SparkBase driveSpark;
4444
private final SparkBase turnSpark;
4545
private final RelativeEncoder driveEncoder;
46-
private final AnalogEncoder turnEncoder;
46+
private final RelativeEncoder turnEncoder;
47+
48+
private final AnalogEncoder absoluteEncoder; // MODIFIED
4749

4850
// Closed loop controllers
4951
private final SparkClosedLoopController driveController;
@@ -70,7 +72,7 @@ public ModuleIOSpark(int module) {
7072
default -> Rotation2d.kZero;
7173
};
7274
driveSpark =
73-
new SparkFlex(
75+
new SparkMax(
7476
switch (module) {
7577
case 0 -> frontLeftDriveCanId;
7678
case 1 -> frontRightDriveCanId;
@@ -90,33 +92,23 @@ public ModuleIOSpark(int module) {
9092
},
9193
MotorType.kBrushless);
9294
driveEncoder = driveSpark.getEncoder();
93-
// turnEncoder = turnSpark.getAbsoluteEncoder();
95+
turnEncoder = turnSpark.getEncoder();
9496

95-
// # BEGIN CUSTOMIZED CODE:
96-
switch (module) {
97-
case 0:
98-
turnEncoder = new AnalogEncoder(0);
99-
break;
100-
case 1:
101-
turnEncoder = new AnalogEncoder(1);
102-
break;
103-
case 2:
104-
turnEncoder = new AnalogEncoder(3);
105-
break;
106-
case 3:
107-
turnEncoder = new AnalogEncoder(2);
108-
break;
109-
default:
110-
turnEncoder = new AnalogEncoder(5);
111-
}
112-
113-
// # END CUSTOMIZED CODE.
97+
absoluteEncoder =
98+
new AnalogEncoder(
99+
switch (module) {
100+
case 0 -> frontLeftAbsoluteEncoderChannel;
101+
case 1 -> frontRightAbsoluteEncoderChannel;
102+
case 2 -> backLeftAbsoluteEncoderChannel;
103+
case 3 -> backRightAbsoluteEncoderChannel;
104+
default -> 0;
105+
});
114106

115107
driveController = driveSpark.getClosedLoopController();
116108
turnController = turnSpark.getClosedLoopController();
117109

118110
// Configure drive motor
119-
var driveConfig = new SparkFlexConfig();
111+
var driveConfig = new SparkMaxConfig();
120112
driveConfig
121113
.idleMode(IdleMode.kBrake)
122114
.smartCurrentLimit(driveMotorCurrentLimit)
@@ -156,23 +148,23 @@ public ModuleIOSpark(int module) {
156148
.smartCurrentLimit(turnMotorCurrentLimit)
157149
.voltageCompensation(12.0);
158150
turnConfig
159-
.absoluteEncoder
160-
.inverted(turnEncoderInverted)
151+
.encoder
152+
// .inverted(turnEncoderInverted)
161153
.positionConversionFactor(turnEncoderPositionFactor)
162154
.velocityConversionFactor(turnEncoderVelocityFactor)
163-
.averageDepth(2);
155+
.uvwAverageDepth(2);
164156
turnConfig
165157
.closedLoop
166-
.feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
158+
.feedbackSensor(FeedbackSensor.kPrimaryEncoder)
167159
.positionWrappingEnabled(true)
168160
.positionWrappingInputRange(turnPIDMinInput, turnPIDMaxInput)
169161
.pid(turnKp, 0.0, turnKd);
170162
turnConfig
171163
.signals
172-
.absoluteEncoderPositionAlwaysOn(true)
173-
.absoluteEncoderPositionPeriodMs((int) (1000.0 / odometryFrequency))
174-
.absoluteEncoderVelocityAlwaysOn(true)
175-
.absoluteEncoderVelocityPeriodMs(20)
164+
.primaryEncoderPositionAlwaysOn(true)
165+
.primaryEncoderPositionPeriodMs((int) (1000.0 / odometryFrequency))
166+
.primaryEncoderVelocityAlwaysOn(true)
167+
.primaryEncoderVelocityPeriodMs(20)
176168
.appliedOutputPeriodMs(20)
177169
.busVoltagePeriodMs(20)
178170
.outputCurrentPeriodMs(20);
@@ -183,12 +175,16 @@ public ModuleIOSpark(int module) {
183175
turnSpark.configure(
184176
turnConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters));
185177

178+
tryUntilOk(
179+
turnSpark,
180+
5,
181+
() -> turnEncoder.setPosition(Radians.convertFrom(absoluteEncoder.get() * 360, Degree)));
186182
// Create odometry queues
187183
timestampQueue = SparkOdometryThread.getInstance().makeTimestampQueue();
188184
drivePositionQueue =
189185
SparkOdometryThread.getInstance().registerSignal(driveSpark, driveEncoder::getPosition);
190186
turnPositionQueue =
191-
SparkOdometryThread.getInstance().registerSignal(turnSpark, turnEncoder::get);
187+
SparkOdometryThread.getInstance().registerSignal(turnSpark, turnEncoder::getPosition);
192188
//// MODIFIED
193189
}
194190

@@ -209,10 +205,9 @@ public void updateInputs(ModuleIOInputs inputs) {
209205
sparkStickyFault = false;
210206
ifOk(
211207
turnSpark,
212-
turnEncoder::get, // MODIFIED
208+
turnEncoder::getPosition,
213209
(value) -> inputs.turnPosition = new Rotation2d(value).minus(zeroRotation));
214-
// ifOk(turnSpark, turnEncoder::get, (value) -> inputs.turnVelocityRadPerSec = value); REMOVED
215-
// DUE TO NON-EXISTANT METHOD
210+
ifOk(turnSpark, turnEncoder::getVelocity, (value) -> inputs.turnVelocityRadPerSec = value);
216211
ifOk(
217212
turnSpark,
218213
new DoubleSupplier[] {turnSpark::getAppliedOutput, turnSpark::getBusVoltage},

0 commit comments

Comments
 (0)