Skip to content

Commit 1370bfc

Browse files
nlaverdureBenGamer3claude
authored
Switch intake rollers to Spark/NEO Vortex (#188)
* added .java files * deleated old feeder and changed to intake * made it so the files on my computer could be read * Switch intake rollers to Spark/NEO Vortex, move gains into IO classes - Robot.java: use RollerIOSpark instead of RollerIOTalonFX for both rollers - IntakeConstants: remove Kraken/TalonFX-specific gains and DCMotor gearbox; add numMotors constant - RollerIOSpark/SimSpark: make PID gains and maxTangentialVelocity static; fix RollerIOSimSpark to accept RollerConfig and use intake constants instead of kicker constants - RollerIOTalonFX/SimTalonFX: move Slot0/Slot1 gain configs into each class Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com> * use spark in sim * spotless --------- Co-authored-by: BenGamer3 <benbenw2020@gmail.com> Co-authored-by: Claude Sonnet 4.6 <noreply@anthropic.com>
1 parent a68f68f commit 1370bfc

7 files changed

Lines changed: 230 additions & 20 deletions

File tree

‎build.gradle‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -155,6 +155,8 @@ wpi.java.configureTestTasks(test)
155155
// Configure string concat to always inline compile
156156
tasks.withType(JavaCompile) {
157157
options.compilerArgs.add '-XDstringConcat=inline'
158+
// Ensure Java compiler treats source files as UTF-8 so Unicode comments/characters compile
159+
options.encoding = 'UTF-8'
158160
}
159161

160162
// Create version file

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

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -72,8 +72,8 @@
7272
import frc.robot.subsystems.intake.IntakeConstants;
7373
import frc.robot.subsystems.intake.IntakeConstants.RollerConstants;
7474
import frc.robot.subsystems.intake.RollerIO;
75-
import frc.robot.subsystems.intake.RollerIOSimTalonFX;
76-
import frc.robot.subsystems.intake.RollerIOTalonFX;
75+
import frc.robot.subsystems.intake.RollerIOSimSpark;
76+
import frc.robot.subsystems.intake.RollerIOSpark;
7777
import frc.robot.subsystems.launcher.FlywheelIO;
7878
import frc.robot.subsystems.launcher.FlywheelIOSimTalonFX;
7979
import frc.robot.subsystems.launcher.FlywheelIOTalonFX;
@@ -198,8 +198,8 @@ public Robot() {
198198
if (FeatureFlags.kHopperEnabled) hopper = new Hopper(new HopperIOReal());
199199
intake =
200200
new Intake(
201-
new RollerIOTalonFX(RollerConstants.upperRollerConfig),
202-
new RollerIOTalonFX(RollerConstants.lowerRollerConfig),
201+
new RollerIOSpark(RollerConstants.upperRollerConfig),
202+
new RollerIOSpark(RollerConstants.lowerRollerConfig),
203203
new IntakeArmIOReal());
204204
feeder = new Feeder(new SpindexerIOSpark(), new KickerIOSpark());
205205
compressor = new LoggedCompressor(PneumaticsModuleType.REVPH, "Compressor");
@@ -245,8 +245,8 @@ public Robot() {
245245
var intakeArmIOSim = new IntakeArmIOSim();
246246
intake =
247247
new Intake(
248-
new RollerIOSimTalonFX(RollerConstants.upperRollerConfig),
249-
new RollerIOSimTalonFX(RollerConstants.lowerRollerConfig),
248+
new RollerIOSimSpark(RollerConstants.upperRollerConfig),
249+
new RollerIOSimSpark(RollerConstants.lowerRollerConfig),
250250
intakeArmIOSim);
251251
pneumaticsSimulator =
252252
new PneumaticsSimulator(intakeArmIOSim.intakeArmPneumatic, new REVPHSim(1));

‎src/main/java/frc/robot/subsystems/intake/IntakeConstants.java‎

Lines changed: 4 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -3,13 +3,8 @@
33
import static edu.wpi.first.units.Units.*;
44

55
import com.ctre.phoenix6.CANBus;
6-
import com.ctre.phoenix6.configs.Slot0Configs;
7-
import com.ctre.phoenix6.configs.Slot1Configs;
8-
import edu.wpi.first.math.system.plant.DCMotor;
9-
import edu.wpi.first.units.measure.AngularVelocity;
106
import edu.wpi.first.units.measure.Distance;
117
import frc.robot.Constants.CANBusPorts.CAN2;
12-
import frc.robot.Constants.MotorConstants.KrakenX60Constants;
138

149
public class IntakeConstants {
1510
/** Time (seconds) to wait after resolving an intake/hopper interlock before proceeding. */
@@ -20,18 +15,13 @@ public static class RollerConstants {
2015

2116
// motor controller
2217
public static final double motorReduction = 1.0;
23-
public static final AngularVelocity maxAngularVelocity =
24-
KrakenX60Constants.kFreeSpeed.div(motorReduction);
25-
public static final Slot0Configs velocityVoltageGains =
26-
new Slot0Configs().withKP(0.11).withKI(0.0).withKD(0.0).withKS(0.1).withKV(0.12);
27-
public static final Slot1Configs velocityTorqueCurrentGains =
28-
new Slot1Configs().withKP(5).withKI(0.0).withKD(0.0).withKS(2.5);
29-
18+
public static final int numMotors = 1;
3019
public static final double maxAcceleration = 4000.0;
3120
public static final double maxJerk = 40000.0;
3221

33-
// simulation
34-
public static final DCMotor gearbox = DCMotor.getKrakenX60(2);
22+
// roller constants
23+
public static final double encoderPositionFactor = 2.0 * Math.PI / motorReduction; // Meters
24+
public static final double encoderVelocityFactor = encoderPositionFactor / 60.0; // Meters/sec
3525

3626
// configs
3727
public static final RollerConfig upperRollerConfig =

‎src/main/java/frc/robot/subsystems/intake/RollerIOSimSpark.java‎

Lines changed: 103 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,103 @@
1+
package frc.robot.subsystems.intake;
2+
3+
import static edu.wpi.first.units.Units.*;
4+
import static frc.robot.subsystems.intake.IntakeConstants.RollerConstants.*;
5+
6+
import com.revrobotics.PersistMode;
7+
import com.revrobotics.ResetMode;
8+
import com.revrobotics.sim.SparkFlexSim;
9+
import com.revrobotics.spark.ClosedLoopSlot;
10+
import com.revrobotics.spark.FeedbackSensor;
11+
import com.revrobotics.spark.SparkBase.ControlType;
12+
import com.revrobotics.spark.SparkClosedLoopController;
13+
import com.revrobotics.spark.SparkFlex;
14+
import com.revrobotics.spark.SparkLowLevel.MotorType;
15+
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
16+
import com.revrobotics.spark.config.SparkFlexConfig;
17+
import edu.wpi.first.math.system.plant.DCMotor;
18+
import edu.wpi.first.math.system.plant.LinearSystemId;
19+
import edu.wpi.first.units.measure.LinearVelocity;
20+
import edu.wpi.first.units.measure.Voltage;
21+
import edu.wpi.first.wpilibj.simulation.DCMotorSim;
22+
import edu.wpi.first.wpilibj.simulation.RoboRioSim;
23+
import frc.robot.Constants.MotorConstants.NEOVortexConstants;
24+
import frc.robot.Constants.RobotConstants;
25+
import frc.robot.Robot;
26+
import frc.robot.subsystems.intake.IntakeConstants.RollerConfig;
27+
28+
public class RollerIOSimSpark implements RollerIO {
29+
private static final double KICKER_MOI_KG_M2 = 0.00052;
30+
private static final double KP = 0.11;
31+
private static final double KD = 0.0;
32+
private static final LinearVelocity maxTangentialVelocity =
33+
MetersPerSecond.of(
34+
NEOVortexConstants.kFreeSpeed.in(RadiansPerSecond)
35+
* rollerRadius.in(Meters)
36+
/ motorReduction);
37+
private static final DCMotor gearbox = DCMotor.getNeoVortex(numMotors);
38+
39+
private final DCMotorSim rollerSim;
40+
41+
private final SparkFlex flex;
42+
private final SparkClosedLoopController controller;
43+
private final SparkFlexSim flexSim;
44+
45+
public RollerIOSimSpark(RollerConfig rollerConfig) {
46+
flex = new SparkFlex(rollerConfig.port, MotorType.kBrushless);
47+
controller = flex.getClosedLoopController();
48+
49+
var config = new SparkFlexConfig();
50+
config
51+
.inverted(rollerConfig.inverted)
52+
.idleMode(IdleMode.kBrake)
53+
.smartCurrentLimit(NEOVortexConstants.kDefaultSupplyCurrentLimit)
54+
.voltageCompensation(RobotConstants.kNominalVoltage);
55+
56+
config
57+
.encoder
58+
.positionConversionFactor(encoderPositionFactor)
59+
.velocityConversionFactor(encoderVelocityFactor);
60+
61+
config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(KP, 0.0, KD);
62+
63+
flex.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
64+
flexSim = new SparkFlexSim(flex, gearbox);
65+
66+
rollerSim =
67+
new DCMotorSim(
68+
LinearSystemId.createDCMotorSystem(gearbox, KICKER_MOI_KG_M2, motorReduction), gearbox);
69+
}
70+
71+
@Override
72+
public void updateInputs(RollerIOInputs inputs) {
73+
// Update simulation state
74+
double busVoltage = RoboRioSim.getVInVoltage();
75+
rollerSim.setInput(flexSim.getAppliedOutput() * busVoltage);
76+
rollerSim.update(Robot.defaultPeriodSecs);
77+
flexSim.iterate(rollerSim.getAngularVelocityRadPerSec(), busVoltage, Robot.defaultPeriodSecs);
78+
79+
// Update inputs
80+
inputs.connected = true;
81+
inputs.velocityMetersPerSec = flexSim.getVelocity() * rollerRadius.in(Meters);
82+
inputs.appliedVolts = flexSim.getAppliedOutput() * flexSim.getBusVoltage();
83+
inputs.currentAmps = Math.abs(flexSim.getMotorCurrent());
84+
}
85+
86+
@Override
87+
public void setOpenLoop(Voltage volts) {
88+
flexSim.setAppliedOutput(volts.in(Volts) / RobotConstants.kNominalVoltage);
89+
}
90+
91+
@Override
92+
public void setVelocity(LinearVelocity tangentialVelocity) {
93+
double feedforwardVolts =
94+
RobotConstants.kNominalVoltage
95+
* tangentialVelocity.in(MetersPerSecond)
96+
/ maxTangentialVelocity.in(MetersPerSecond);
97+
controller.setSetpoint(
98+
tangentialVelocity.in(MetersPerSecond) / rollerRadius.in(Meters),
99+
ControlType.kVelocity,
100+
ClosedLoopSlot.kSlot0,
101+
feedforwardVolts);
102+
}
103+
}

‎src/main/java/frc/robot/subsystems/intake/RollerIOSimTalonFX.java‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,8 @@
66

77
import com.ctre.phoenix6.BaseStatusSignal;
88
import com.ctre.phoenix6.StatusSignal;
9+
import com.ctre.phoenix6.configs.Slot0Configs;
10+
import com.ctre.phoenix6.configs.Slot1Configs;
911
import com.ctre.phoenix6.configs.TalonFXConfiguration;
1012
import com.ctre.phoenix6.controls.NeutralOut;
1113
import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC;
@@ -14,6 +16,7 @@
1416
import com.ctre.phoenix6.signals.InvertedValue;
1517
import com.ctre.phoenix6.signals.NeutralModeValue;
1618
import edu.wpi.first.math.filter.Debouncer;
19+
import edu.wpi.first.math.system.plant.DCMotor;
1720
import edu.wpi.first.math.system.plant.LinearSystemId;
1821
import edu.wpi.first.units.measure.AngularAcceleration;
1922
import edu.wpi.first.units.measure.AngularVelocity;
@@ -26,6 +29,13 @@
2629
import frc.robot.subsystems.intake.IntakeConstants.RollerConfig;
2730

2831
public class RollerIOSimTalonFX implements RollerIO {
32+
private static final double kP = 0.11;
33+
private static final double kD = 0.0;
34+
private static final Slot0Configs velocityVoltageGains =
35+
new Slot0Configs().withKP(kP).withKI(0.0).withKD(kD).withKS(0.1).withKV(0.12);
36+
private static final Slot1Configs velocityTorqueCurrentGains =
37+
new Slot1Configs().withKP(kP).withKI(0.0).withKD(kD).withKS(2.5);
38+
private static final DCMotor gearbox = DCMotor.getKrakenX60(numMotors);
2939

3040
private final DCMotorSim rollerSim;
3141

‎src/main/java/frc/robot/subsystems/intake/RollerIOSpark.java‎

Lines changed: 96 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,96 @@
1+
package frc.robot.subsystems.intake;
2+
3+
import static edu.wpi.first.units.Units.*;
4+
import static frc.robot.subsystems.intake.IntakeConstants.RollerConstants.*;
5+
import static frc.robot.util.SparkUtil.*;
6+
7+
import com.revrobotics.PersistMode;
8+
import com.revrobotics.RelativeEncoder;
9+
import com.revrobotics.ResetMode;
10+
import com.revrobotics.spark.ClosedLoopSlot;
11+
import com.revrobotics.spark.FeedbackSensor;
12+
import com.revrobotics.spark.SparkBase.ControlType;
13+
import com.revrobotics.spark.SparkClosedLoopController;
14+
import com.revrobotics.spark.SparkFlex;
15+
import com.revrobotics.spark.SparkLowLevel.MotorType;
16+
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
17+
import com.revrobotics.spark.config.SparkFlexConfig;
18+
import edu.wpi.first.units.measure.LinearVelocity;
19+
import edu.wpi.first.units.measure.Voltage;
20+
import frc.robot.Constants.MotorConstants.NEOVortexConstants;
21+
import frc.robot.Constants.RobotConstants;
22+
import frc.robot.subsystems.intake.IntakeConstants.RollerConfig;
23+
import frc.robot.util.SparkOdometryThread;
24+
import frc.robot.util.SparkOdometryThread.SparkInputs;
25+
26+
public class RollerIOSpark implements RollerIO {
27+
private static final double KP = 0.001;
28+
private static final double KD = 0.0;
29+
private static final LinearVelocity maxTangentialVelocity =
30+
MetersPerSecond.of(
31+
NEOVortexConstants.kFreeSpeed.in(RadiansPerSecond)
32+
* rollerRadius.in(Meters)
33+
/ motorReduction);
34+
35+
private final SparkFlex flex;
36+
private final RelativeEncoder encoder;
37+
private final SparkClosedLoopController controller;
38+
private final SparkInputs sparkInputs;
39+
40+
public RollerIOSpark(RollerConfig rollerConfig) {
41+
flex = new SparkFlex(rollerConfig.port, MotorType.kBrushless);
42+
encoder = flex.getEncoder();
43+
controller = flex.getClosedLoopController();
44+
45+
var config = new SparkFlexConfig();
46+
config
47+
.inverted(rollerConfig.inverted)
48+
.idleMode(IdleMode.kBrake)
49+
.smartCurrentLimit(NEOVortexConstants.kDefaultSupplyCurrentLimit)
50+
.voltageCompensation(RobotConstants.kNominalVoltage);
51+
52+
config
53+
.encoder
54+
.positionConversionFactor(encoderPositionFactor)
55+
.velocityConversionFactor(encoderVelocityFactor)
56+
.uvwAverageDepth(2)
57+
.uvwMeasurementPeriod(8);
58+
59+
config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(KP, 0.0, KD);
60+
61+
tryUntilOk(
62+
flex,
63+
5,
64+
() ->
65+
flex.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters));
66+
67+
sparkInputs = SparkOdometryThread.getInstance().registerSpark(flex, encoder);
68+
}
69+
70+
@Override
71+
public void updateInputs(RollerIOInputs inputs) {
72+
inputs.connected = sparkInputs.isConnected();
73+
inputs.velocityMetersPerSec = sparkInputs.getVelocity() * rollerRadius.in(Meters);
74+
inputs.appliedVolts = sparkInputs.getAppliedVolts();
75+
inputs.currentAmps = sparkInputs.getOutputCurrent();
76+
}
77+
78+
@Override
79+
public void setOpenLoop(Voltage volts) {
80+
flex.setVoltage(volts);
81+
;
82+
}
83+
84+
@Override
85+
public void setVelocity(LinearVelocity tangentialVelocity) {
86+
double feedforwardVolts =
87+
RobotConstants.kNominalVoltage
88+
* tangentialVelocity.in(MetersPerSecond)
89+
/ maxTangentialVelocity.in(MetersPerSecond);
90+
controller.setSetpoint(
91+
tangentialVelocity.in(MetersPerSecond) / rollerRadius.in(Meters),
92+
ControlType.kVelocity,
93+
ClosedLoopSlot.kSlot0,
94+
feedforwardVolts);
95+
}
96+
}

‎src/main/java/frc/robot/subsystems/intake/RollerIOTalonFX.java‎

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,8 @@
66

77
import com.ctre.phoenix6.BaseStatusSignal;
88
import com.ctre.phoenix6.StatusSignal;
9+
import com.ctre.phoenix6.configs.Slot0Configs;
10+
import com.ctre.phoenix6.configs.Slot1Configs;
911
import com.ctre.phoenix6.configs.TalonFXConfiguration;
1012
import com.ctre.phoenix6.controls.NeutralOut;
1113
import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC;
@@ -23,6 +25,13 @@
2325
import frc.robot.subsystems.intake.IntakeConstants.RollerConfig;
2426

2527
public class RollerIOTalonFX implements RollerIO {
28+
private static final double kP = 0.11;
29+
private static final double kD = 0.0;
30+
private static final Slot0Configs velocityVoltageGains =
31+
new Slot0Configs().withKP(kP).withKI(0.0).withKD(kD).withKS(0.1).withKV(0.12);
32+
private static final Slot1Configs velocityTorqueCurrentGains =
33+
new Slot1Configs().withKP(kP).withKI(0.0).withKD(kD).withKS(2.5);
34+
2635
private final TalonFX motor;
2736
private final TalonFXConfiguration config;
2837
private final Debouncer connectedDebounce = new Debouncer(0.5, Debouncer.DebounceType.kFalling);

0 commit comments

Comments
 (0)