Skip to content

Commit d2a0df1

Browse files
timtogantimtogan
committed
made a getConfigAccessor method for MotorControllerFactory and made pid controller use correct config
1 parent b8896bb commit d2a0df1

3 files changed

Lines changed: 35 additions & 50 deletions

File tree

‎src/main/java/org/carlmontrobotics/lib199/MotorControllerFactory.java‎

Lines changed: 20 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -9,8 +9,7 @@
99

1010
import org.carlmontrobotics.lib199.sim.MockSparkFlex;
1111
import org.carlmontrobotics.lib199.sim.MockSparkMax;
12-
// import org.carlmontrobotics.lib199.sim.MockSparkFlex;
13-
// import org.carlmontrobotics.lib199.sim.MockSparkMax;
12+
1413
import org.carlmontrobotics.lib199.sim.MockTalonSRX;
1514

1615
import com.ctre.phoenix.motorcontrol.NeutralMode;
@@ -20,6 +19,7 @@
2019
import com.revrobotics.spark.SparkLowLevel;
2120
import com.revrobotics.spark.SparkMax;
2221
import com.revrobotics.spark.config.SparkBaseConfig;
22+
import com.revrobotics.spark.config.SparkBaseConfigAccessor;
2323
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
2424
import com.revrobotics.spark.config.SparkFlexConfig;
2525
import com.revrobotics.spark.config.SparkMaxConfig;
@@ -76,11 +76,11 @@ public static SparkMax createSparkMax(int id, MotorConfig motorConfig, SparkBase
7676
} else {
7777
spark = MockSparkMax.createMockSparkMax(id, SparkLowLevel.MotorType.kBrushless, MockSparkMax.NEOType.NEO);
7878
}
79-
spark.configure(
79+
MotorErrors.reportError(spark.configure(
8080
config,
8181
SparkBase.ResetMode.kResetSafeParameters,
8282
SparkBase.PersistMode.kNoPersistParameters
83-
);
83+
));
8484
MotorErrors.reportSparkTemp(spark, motorConfig.temperatureLimitCelsius);
8585
MotorErrors.checkSparkErrors(spark);
8686

@@ -101,19 +101,34 @@ public static SparkFlex createSparkFlex(int id) {
101101
* @param config the custom config to set
102102
*/
103103
public static SparkFlex createSparkFlex(int id, MotorConfig motorConfig, SparkBaseConfig config) {
104-
SparkFlex spark = null;
104+
SparkFlex spark;
105105
if (RobotBase.isReal()) {
106106
spark = new SparkFlex(id, SparkLowLevel.MotorType.kBrushless);
107107
} else {
108108
spark = MockSparkFlex.createMockSparkFlex(id, SparkLowLevel.MotorType.kBrushless);
109109
}
110+
MotorErrors.reportError(spark.configure(
111+
config,
112+
SparkBase.ResetMode.kResetSafeParameters,
113+
SparkBase.PersistMode.kNoPersistParameters
114+
));
110115

111116
MotorErrors.reportSparkTemp(spark, motorConfig.temperatureLimitCelsius);
112117
MotorErrors.checkSparkErrors(spark);
113118

114119
return spark;
115120
}
116121

122+
public static SparkBaseConfigAccessor getConfigAccessor(SparkBase motor){
123+
switch(getControllerType(motor)){
124+
case SPARK_MAX:
125+
return ((SparkMax)motor).configAccessor;
126+
case SPARK_FLEX:
127+
return ((SparkFlex)motor).configAccessor;
128+
default:
129+
return null;
130+
}
131+
}
117132
public static SparkBaseConfig createConfig(MotorControllerType type) {
118133
SparkBaseConfig config = null;
119134
switch(type){

‎src/main/java/org/carlmontrobotics/lib199/SparkVelocityPIDController.java‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -34,7 +34,7 @@ public SparkVelocityPIDController(String name, SparkBase spark, double defaultP,
3434
this.targetSpeed = targetSpeed;
3535
this.tolerance = tolerance;
3636

37-
spark.configure(new SparkMaxConfig().apply(
37+
spark.configure(MotorControllerFactory.sparkConfig(MotorConfig.NEO).apply(
3838
new ClosedLoopConfig().pid(
3939
this.currentP = defaultP,
4040
this.currentI = defaultI,
@@ -85,7 +85,7 @@ public double calculateFF(double velocity) {
8585
}
8686

8787
private void instantClosedLoopConfig(ClosedLoopConfig clConfig) {
88-
spark.configure(new SparkMaxConfig().apply(clConfig), ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters);
88+
spark.configure(MotorControllerFactory.sparkConfig(MotorConfig.NEO).apply(clConfig), ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters);
8989
}
9090

9191
@Override

‎src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java‎

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

77
import java.util.function.Supplier;
88

9+
import org.carlmontrobotics.lib199.MotorControllerFactory;
910
import org.carlmontrobotics.lib199.MotorControllerType;
1011
import org.mockito.internal.reporting.SmartPrinter;
1112

@@ -18,6 +19,7 @@
1819
import com.revrobotics.spark.SparkMax;
1920
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
2021
import com.revrobotics.spark.config.SparkBaseConfig;
22+
import com.revrobotics.spark.config.SparkBaseConfigAccessor;
2123
import com.revrobotics.spark.config.SparkMaxConfig;
2224
import com.revrobotics.spark.config.SparkFlexConfig;
2325

@@ -69,15 +71,15 @@ public enum ModuleType {FL, FR, BL, BR};
6971
private double turnSpeedCorrectionVolts, turnFFVolts, turnVolts;
7072
private double maxTurnVelocityWithoutTippingRps;
7173

72-
MotorControllerType driveMotorType;
73-
MotorControllerType turnMotorType;
74+
SparkBaseConfigAccessor driveConfigAccessor;
75+
SparkBaseConfigAccessor turnConfigAccessor;
7476

7577
public SwerveModule(SwerveConfig config, ModuleType type, SparkBase drive, SparkBase turn, CANcoder turnEncoder,
7678
int arrIndex, Supplier<Float> pitchDegSupplier, Supplier<Float> rollDegSupplier) {
77-
driveMotorType = MotorControllerType.getMotorControllerType(drive);
78-
turnMotorType = MotorControllerType.getMotorControllerType(turn);
79-
driveConfig = driveMotorType.createConfig();
80-
turnConfig = turnMotorType.createConfig();
79+
driveConfig = MotorControllerType.getMotorControllerType(drive).createConfig();
80+
turnConfig = MotorControllerType.getMotorControllerType(turn).createConfig();
81+
driveConfigAccessor = MotorControllerFactory.getConfigAccessor(drive);
82+
turnConfigAccessor = MotorControllerFactory.getConfigAccessor(turn);
8183
//SmartDashboard.putNumber("Target Angle (deg)", 0.0);
8284
String moduleString = type.toString();
8385
this.timer = new Timer();
@@ -121,18 +123,9 @@ public SwerveModule(SwerveConfig config, ModuleType type, SparkBase drive, Spark
121123
config.drivekD[arrIndex]);
122124

123125
/* offset for 1 relative encoder count */
124-
switch(MotorControllerType.getMotorControllerType(drive)) {
125-
case SPARK_MAX:
126-
drivetoleranceMPerS = (1.0
127-
/ (double)(((SparkMax)drive).configAccessor.encoder.getCountsPerRevolution()) * drivePositionFactor)
128-
/ Units.millisecondsToSeconds(((SparkMax)drive).configAccessor.signals.getPrimaryEncoderPositionPeriodMs() * ((SparkMax)drive).configAccessor.encoder.getQuadratureAverageDepth());
129-
break;
130-
case SPARK_FLEX:
131-
drivetoleranceMPerS = (1.0
132-
/ (double)(((SparkFlex)drive).configAccessor.encoder.getCountsPerRevolution()) * drivePositionFactor)
133-
/ Units.millisecondsToSeconds(((SparkFlex)drive).configAccessor.signals.getPrimaryEncoderPositionPeriodMs() * ((SparkFlex)drive).configAccessor.encoder.getQuadratureAverageDepth());
134-
break;
135-
}
126+
drivetoleranceMPerS = (1.0
127+
/ (double)(driveConfigAccessor.encoder.getCountsPerRevolution()) * drivePositionFactor)
128+
/ Units.millisecondsToSeconds(driveConfigAccessor.signals.getPrimaryEncoderPositionPeriodMs() * driveConfigAccessor.encoder.getQuadratureAverageDepth());
136129
drivePIDController.setTolerance(drivetoleranceMPerS);
137130

138131
//System.out.println("Velocity Constant: " + (positionConstant / 60));
@@ -448,31 +441,8 @@ public void updateSmartDashboard() {
448441
}
449442

450443
public void toggleMode() {
451-
switch(MotorControllerType.getMotorControllerType(drive)) {
452-
case SPARK_MAX:
453-
switch (MotorControllerType.getMotorControllerType(turn)) {
454-
case SPARK_MAX:
455-
if (((SparkMax)drive).configAccessor.getIdleMode() == IdleMode.kBrake && ((SparkMax)turn).configAccessor.getIdleMode() == IdleMode.kCoast) coast();
456-
else brake();
457-
break;
458-
case SPARK_FLEX:
459-
if (((SparkMax)drive).configAccessor.getIdleMode() == IdleMode.kBrake && ((SparkFlex)turn).configAccessor.getIdleMode() == IdleMode.kCoast) coast();
460-
else brake();
461-
break;
462-
}
463-
case SPARK_FLEX:
464-
switch (MotorControllerType.getMotorControllerType(turn)) {
465-
case SPARK_MAX:
466-
if (((SparkFlex)drive).configAccessor.getIdleMode() == IdleMode.kBrake && ((SparkMax)turn).configAccessor.getIdleMode() == IdleMode.kCoast) coast();
467-
else brake();
468-
break;
469-
case SPARK_FLEX:
470-
if (((SparkFlex)drive).configAccessor.getIdleMode() == IdleMode.kBrake && ((SparkFlex)turn).configAccessor.getIdleMode() == IdleMode.kCoast) coast();
471-
else brake();
472-
break;
473-
}
474-
break;
475-
}
444+
if (driveConfigAccessor.getIdleMode() == IdleMode.kBrake && turnConfigAccessor.getIdleMode() == IdleMode.kCoast) coast();
445+
else brake();
476446
}
477447

478448
public void brake() {

0 commit comments

Comments
 (0)