Skip to content

Commit 5637e67

Browse files
Talon540-rootTalon540-root
committed
smellyush
1 parent abe6130 commit 5637e67

File tree

6 files changed

+68
-304
lines changed

6 files changed

+68
-304
lines changed

‎.wpilib/wpilib_preferences.json

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -2,5 +2,5 @@
22
"enableCppIntellisense": false,
33
"currentLanguage": "java",
44
"projectYear": "2025",
5-
"teamNumber": 6328
5+
"teamNumber": 540
66
}

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

Lines changed: 26 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -34,25 +34,30 @@ public class DriveConstants {
3434
};
3535

3636
// Zeroed rotation values for each module, see setup instructions
37-
public static final Rotation2d frontLeftZeroRotation = new Rotation2d(0.0);
38-
public static final Rotation2d frontRightZeroRotation = new Rotation2d(0.0);
39-
public static final Rotation2d backLeftZeroRotation = new Rotation2d(0.0);
40-
public static final Rotation2d backRightZeroRotation = new Rotation2d(0.0);
41-
42-
// Device CAN IDs
43-
public static final int pigeonCanId = 9;
37+
public static final Rotation2d frontLeftZeroRotation = new Rotation2d(0.1574540138244629);
38+
public static final Rotation2d frontRightZeroRotation = new Rotation2d(-0.16320089995861053);
39+
public static final Rotation2d backLeftZeroRotation = new Rotation2d(-2.942538261413574);
40+
public static final Rotation2d backRightZeroRotation = new Rotation2d(2.5917508602142334);
4441

42+
// Device CAN ID
4543
public static final int frontLeftTurnCanId = 2;
4644
public static final int frontLeftDriveCanId = 3;
4745

48-
public static final int frontRightTurnCanId = 5;
49-
public static final int frontRightDriveCanId = 6;
46+
public static final int frontRightTurnCanId = 4;
47+
public static final int frontRightDriveCanId = 5;
48+
49+
public static final int backLeftTurnCanId = 6;
50+
public static final int backLeftDriveCanId = 7;
51+
52+
public static final int backRightTurnCanId = 8;
53+
public static final int backRightDriveCanId = 9;
5054

51-
public static final int backLeftTurnCanId = 8;
52-
public static final int backLeftDriveCanId = 9;
55+
public static final int pigeonCanId = 10;
5356

54-
public static final int backRightTurnCanId = 11;
55-
public static final int backRightDriveCanId = 12;
57+
public static final int frontLeftEncoder = 2;
58+
public static final int frontRightEncoder = 3;
59+
public static final int backLeftEncoder = 1;
60+
public static final int backRightEncoder = 0;
5661

5762
// Drive motor configuration
5863
public static final int driveMotorCurrentLimit = 50;
@@ -70,7 +75,7 @@ public class DriveConstants {
7075
public static final double driveKp = 0.0;
7176
public static final double driveKd = 0.0;
7277
public static final double driveKs = 0.0;
73-
public static final double driveKv = 0.1;
78+
public static final double driveKv = 0.0;
7479
public static final double driveSimP = 0.05;
7580
public static final double driveSimD = 0.0;
7681
public static final double driveSimKs = 0.0;
@@ -79,17 +84,19 @@ public class DriveConstants {
7984
// Turn motor configuration
8085
public static final boolean turnInverted = false;
8186
public static final int turnMotorCurrentLimit = 20;
82-
public static final double turnMotorReduction = 12.8;
87+
public static final double turnMotorReduction = 150.0 / 7.0;
8388
public static final DCMotor turnGearbox = DCMotor.getNEO(1);
8489

8590
// Turn encoder configuration
8691
public static final boolean turnEncoderInverted = true;
87-
public static final double turnEncoderPositionFactor = 2 * Math.PI; // Rotations -> Radians
88-
public static final double turnEncoderVelocityFactor = (2 * Math.PI) / 60.0; // RPM -> Rad/Sec
92+
public static final double turnEncoderPositionFactor =
93+
2 * Math.PI / turnMotorReduction; // Rotations -> Radians
94+
public static final double turnEncoderVelocityFactor =
95+
(2 * Math.PI) / 60.0 / turnMotorReduction; // RPM -> Rad/Sec
8996

9097
// Turn PID configuration
91-
public static final double turnKp = 2.0;
92-
public static final double turnKd = 0.0;
98+
public static final double turnKp = 4000;
99+
public static final double turnKd = 50;
93100
public static final double turnSimP = 8.0;
94101
public static final double turnSimD = 0.0;
95102
public static final double turnPIDMinInput = 0; // Radians

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -26,7 +26,7 @@
2626

2727
/** IO implementation for Pigeon 2. */
2828
public class GyroIOPigeon2 implements GyroIO {
29-
private final Pigeon2 pigeon = new Pigeon2(14);
29+
private final Pigeon2 pigeon = new Pigeon2(DriveConstants.pigeonCanId);
3030
private final StatusSignal<Angle> yaw = pigeon.getYaw();
3131
private final Queue<Double> yawPositionQueue;
3232
private final Queue<Double> yawTimestampQueue;

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

Lines changed: 35 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -16,7 +16,6 @@
1616
import static frc.robot.subsystems.drive.DriveConstants.*;
1717
import static frc.robot.util.SparkUtil.*;
1818

19-
import com.revrobotics.AbsoluteEncoder;
2019
import com.revrobotics.RelativeEncoder;
2120
import com.revrobotics.spark.ClosedLoopSlot;
2221
import com.revrobotics.spark.SparkBase;
@@ -29,17 +28,17 @@
2928
import com.revrobotics.spark.SparkMax;
3029
import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
3130
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
32-
import com.revrobotics.spark.config.SparkFlexConfig;
3331
import com.revrobotics.spark.config.SparkMaxConfig;
3432
import edu.wpi.first.math.MathUtil;
3533
import edu.wpi.first.math.filter.Debouncer;
3634
import edu.wpi.first.math.geometry.Rotation2d;
35+
import edu.wpi.first.wpilibj.AnalogEncoder;
3736
import java.util.Queue;
3837
import java.util.function.DoubleSupplier;
3938

4039
/**
4140
* Module IO implementation for Spark Max drive motor controller, Spark Max turn motor controller,
42-
* and duty cycle absolute encoder.
41+
* and analog absolute encoder.
4342
*/
4443
public class ModuleIOSpark implements ModuleIO {
4544
private final Rotation2d zeroRotation;
@@ -48,7 +47,8 @@ public class ModuleIOSpark implements ModuleIO {
4847
private final SparkBase driveSpark;
4948
private final SparkBase turnSpark;
5049
private final RelativeEncoder driveEncoder;
51-
private final AbsoluteEncoder turnEncoder;
50+
private final RelativeEncoder turnEncoder;
51+
private final AnalogEncoder turnAbsoluteEncoder;
5252

5353
// Closed loop controllers
5454
private final SparkClosedLoopController driveController;
@@ -92,13 +92,24 @@ public ModuleIOSpark(int module) {
9292
default -> 0;
9393
},
9494
MotorType.kBrushless);
95+
turnAbsoluteEncoder =
96+
new AnalogEncoder(
97+
switch (module) {
98+
case 0 -> frontLeftEncoder;
99+
case 1 -> frontRightEncoder;
100+
case 2 -> backLeftEncoder;
101+
case 3 -> backRightEncoder;
102+
default -> 0;
103+
},
104+
2 * Math.PI,
105+
0);
95106
driveEncoder = driveSpark.getEncoder();
96-
turnEncoder = turnSpark.getAbsoluteEncoder();
107+
turnEncoder = turnSpark.getEncoder();
97108
driveController = driveSpark.getClosedLoopController();
98109
turnController = turnSpark.getClosedLoopController();
99110

100111
// Configure drive motor
101-
var driveConfig = new SparkFlexConfig();
112+
var driveConfig = new SparkMaxConfig();
102113
driveConfig
103114
.idleMode(IdleMode.kBrake)
104115
.smartCurrentLimit(driveMotorCurrentLimit)
@@ -134,39 +145,45 @@ public ModuleIOSpark(int module) {
134145

135146
// Configure turn motor
136147
var turnConfig = new SparkMaxConfig();
148+
137149
turnConfig
138150
.inverted(turnInverted)
139-
.idleMode(IdleMode.kBrake)
151+
.idleMode(IdleMode.kCoast)
140152
.smartCurrentLimit(turnMotorCurrentLimit)
141153
.voltageCompensation(12.0);
142154
turnConfig
143-
.absoluteEncoder
144-
.inverted(turnEncoderInverted)
155+
.encoder
145156
.positionConversionFactor(turnEncoderPositionFactor)
146157
.velocityConversionFactor(turnEncoderVelocityFactor)
147-
.averageDepth(2);
158+
.uvwMeasurementPeriod(10)
159+
.uvwAverageDepth(2);
148160
turnConfig
149161
.closedLoop
150-
.feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
162+
.feedbackSensor(FeedbackSensor.kPrimaryEncoder)
151163
.positionWrappingEnabled(true)
152164
.positionWrappingInputRange(turnPIDMinInput, turnPIDMaxInput)
153165
.pidf(turnKp, 0.0, turnKd, 0.0);
166+
154167
turnConfig
155168
.signals
156-
.absoluteEncoderPositionAlwaysOn(true)
157-
.absoluteEncoderPositionPeriodMs((int) (1000.0 / odometryFrequency))
158-
.absoluteEncoderVelocityAlwaysOn(true)
159-
.absoluteEncoderVelocityPeriodMs(20)
169+
.primaryEncoderPositionAlwaysOn(true)
170+
.primaryEncoderPositionPeriodMs((int) (1000.0 / odometryFrequency))
171+
.primaryEncoderVelocityAlwaysOn(true)
172+
.primaryEncoderVelocityPeriodMs(20)
160173
.appliedOutputPeriodMs(20)
161174
.busVoltagePeriodMs(20)
162175
.outputCurrentPeriodMs(20);
176+
163177
tryUntilOk(
164178
turnSpark,
165179
5,
166180
() ->
167181
turnSpark.configure(
168182
turnConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters));
169183

184+
turnEncoder.setPosition(
185+
Rotation2d.fromRadians(turnAbsoluteEncoder.get()).minus(zeroRotation).getRadians());
186+
170187
// Create odometry queues
171188
timestampQueue = SparkOdometryThread.getInstance().makeTimestampQueue();
172189
drivePositionQueue =
@@ -177,7 +194,6 @@ public ModuleIOSpark(int module) {
177194

178195
@Override
179196
public void updateInputs(ModuleIOInputs inputs) {
180-
System.out.println("TurnEncoder: " + turnEncoder.getPosition());
181197
// Update drive inputs
182198
sparkStickyFault = false;
183199
ifOk(driveSpark, driveEncoder::getPosition, (value) -> inputs.drivePositionRad = value);
@@ -194,7 +210,7 @@ public void updateInputs(ModuleIOInputs inputs) {
194210
ifOk(
195211
turnSpark,
196212
turnEncoder::getPosition,
197-
(value) -> inputs.turnPosition = new Rotation2d(value).minus(zeroRotation));
213+
(value) -> inputs.turnPosition = new Rotation2d(value));
198214
ifOk(turnSpark, turnEncoder::getVelocity, (value) -> inputs.turnVelocityRadPerSec = value);
199215
ifOk(
200216
turnSpark,
@@ -210,7 +226,7 @@ public void updateInputs(ModuleIOInputs inputs) {
210226
drivePositionQueue.stream().mapToDouble((Double value) -> value).toArray();
211227
inputs.odometryTurnPositions =
212228
turnPositionQueue.stream()
213-
.map((Double value) -> new Rotation2d(value).minus(zeroRotation))
229+
.map((Double value) -> new Rotation2d(value))
214230
.toArray(Rotation2d[]::new);
215231
timestampQueue.clear();
216232
drivePositionQueue.clear();
@@ -241,8 +257,7 @@ public void setDriveVelocity(double velocityRadPerSec) {
241257
@Override
242258
public void setTurnPosition(Rotation2d rotation) {
243259
double setpoint =
244-
MathUtil.inputModulus(
245-
rotation.plus(zeroRotation).getRadians(), turnPIDMinInput, turnPIDMaxInput);
260+
MathUtil.inputModulus(rotation.plus(zeroRotation).getRadians(), -Math.PI, Math.PI);
246261
turnController.setReference(setpoint, ControlType.kPosition);
247262
}
248263
}

0 commit comments

Comments
 (0)