Skip to content

Commit dd53e55

Browse files
gavinskycastlegavinskycastle
authored
Merge pull request #52 from 4201VitruvianBots/test-mode
Merge Indexer Test Mode with main
2 parents b9f55a5 + a4e848d commit dd53e55

4 files changed

Lines changed: 76 additions & 23 deletions

File tree

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

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -307,8 +307,8 @@ public class INDEXER {
307307
// TODO: change values
308308
public static final double kP = 1.0;
309309
public static final double gearRatio = 1.0;
310-
public static final double peakForwardOutput = 0.8;
311-
public static final double peakReverseOutput = -0.5;
310+
public static final double peakForwardOutput = 0.9;
311+
public static final double peakReverseOutput = -0.9;
312312
public static final double kInertia = 0.005;
313313

314314
public static final DCMotor gearbox = DCMotor.getKrakenX60(3);

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

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -208,7 +208,7 @@ private void configureBindings() {
208208
.whileTrue(
209209
new ParallelCommandGroup(
210210
new RunUptake(m_uptake),
211-
new Index(m_indexer, INDEXER_SPEED.INDEXING),
211+
new Index(m_indexer, INDEXER_SPEED.INDEXING, INDEXER_SPEED.INDEXING),
212212
new RunIntake(m_intake, INTAKE_SPEED.INTAKING)));
213213
}
214214

@@ -260,11 +260,13 @@ public void testInit() {
260260
if (m_flywheel != null) m_flywheel.testInit();
261261
if (m_vision != null) m_vision.testInit();
262262
if (m_uptake != null) m_uptake.testInit();
263+
if (m_indexer != null) m_indexer.testInit();
263264
}
264265

265266
public void testPeriodic() {
266267
if (m_flywheel != null) m_flywheel.testPeriodic();
267268
if (m_uptake != null) m_uptake.testPeriodic();
269+
if (m_indexer != null) m_indexer.testPeriodic();
268270
}
269271

270272
/**

‎src/main/java/frc/robot/commands/Index.java‎

Lines changed: 7 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -8,20 +8,22 @@ public class Index extends Command {
88

99
private final Indexer m_indexer;
1010

11-
private final INDEXER_SPEED m_speed;
11+
private final INDEXER_SPEED m_speed1;
12+
private final INDEXER_SPEED m_speed2;
1213

1314
/** Creates a new Index. */
14-
public Index(Indexer indexer, INDEXER_SPEED speed) {
15+
public Index(Indexer indexer, INDEXER_SPEED speed1, INDEXER_SPEED speed2) {
1516
m_indexer = indexer;
16-
m_speed = speed;
17+
m_speed1 = speed1;
18+
m_speed2 = speed2;
1719

1820
addRequirements(indexer);
1921
}
2022

2123
// Called when the command is initially scheduled.
2224
@Override
2325
public void initialize() {
24-
m_indexer.setSpeed(m_speed.get());
26+
m_indexer.setSpeeds(m_speed1.get(), m_speed2.get());
2527
}
2628

2729
// Called every time the scheduler runs while the command is scheduled.
@@ -31,7 +33,7 @@ public void execute() {}
3133
// Called once the command ends or is interrupted.
3234
@Override
3335
public void end(boolean interrupted) {
34-
m_indexer.setSpeed(0.0);
36+
m_indexer.setSpeeds(0.0, 0.0);
3537
}
3638

3739
// Returns true when the command should end.

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

Lines changed: 64 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -8,15 +8,16 @@
88
import static edu.wpi.first.units.Units.Rotations;
99

1010
import com.ctre.phoenix6.configs.TalonFXConfiguration;
11-
import com.ctre.phoenix6.controls.Follower;
1211
import com.ctre.phoenix6.hardware.TalonFX;
1312
import com.ctre.phoenix6.signals.InvertedValue;
14-
import com.ctre.phoenix6.signals.MotorAlignmentValue;
1513
import com.ctre.phoenix6.signals.NeutralModeValue;
1614
import com.ctre.phoenix6.sim.TalonFXSimState;
1715
import edu.wpi.first.epilogue.Logged;
1816
import edu.wpi.first.epilogue.Logged.Importance;
1917
import edu.wpi.first.math.system.plant.LinearSystemId;
18+
import edu.wpi.first.networktables.DoublePublisher;
19+
import edu.wpi.first.networktables.DoubleSubscriber;
20+
import edu.wpi.first.networktables.NetworkTableInstance;
2021
import edu.wpi.first.wpilibj.RobotController;
2122
import edu.wpi.first.wpilibj.simulation.DCMotorSim;
2223
import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -29,17 +30,27 @@ public class Indexer extends SubsystemBase {
2930
@Logged(name = "Indexer Motor 1", importance = Importance.INFO)
3031
private final TalonFX m_indexerMotor1 = new TalonFX(CAN.kIndexerMotor1, CAN.driveBaseCanbus);
3132

32-
@Logged(name = "Indexer Motor 2", importance = Importance.DEBUG)
33+
@Logged(name = "Indexer Motor 2", importance = Importance.INFO)
3334
private final TalonFX m_indexerMotor2 = new TalonFX(CAN.kIndexerMotor2, CAN.driveBaseCanbus);
3435

3536
// @Logged(name = "Indexer Motor 3", importance = Importance.DEBUG)
3637
// private final TalonFX m_indexerMotor3 = new TalonFX(CAN.kIndexerMotor3);
3738

39+
private DoubleSubscriber m_speedSubscriber1;
40+
private DoublePublisher m_speedPublisher1;
41+
private DoubleSubscriber m_speedSubscriber2;
42+
private DoublePublisher m_speedPublisher2;
43+
3844
private final DCMotorSim m_indexerMotor1Sim =
3945
new DCMotorSim(
4046
LinearSystemId.createDCMotorSystem(INDEXER.gearbox, INDEXER.kInertia, INDEXER.gearRatio),
4147
INDEXER.gearbox);
42-
private final TalonFXSimState m_simState;
48+
private final DCMotorSim m_indexerMotor2Sim =
49+
new DCMotorSim(
50+
LinearSystemId.createDCMotorSystem(INDEXER.gearbox, INDEXER.kInertia, INDEXER.gearRatio),
51+
INDEXER.gearbox);
52+
private final TalonFXSimState m_simState1;
53+
private final TalonFXSimState m_simState2;
4354

4455
/** Creates a new Indexer. */
4556
public Indexer() {
@@ -54,42 +65,80 @@ public Indexer() {
5465
config.CurrentLimits.StatorCurrentLimit = 60;
5566
config.CurrentLimits.StatorCurrentLimitEnable = true;
5667
CtreUtils.configureTalonFx(m_indexerMotor1, config);
68+
config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
5769
CtreUtils.configureTalonFx(m_indexerMotor2, config);
5870

59-
m_indexerMotor2.setControl(
60-
new Follower(m_indexerMotor1.getDeviceID(), MotorAlignmentValue.Opposed));
71+
// m_indexerMotor2.setControl(
72+
// new Follower(m_indexerMotor1.getDeviceID(), MotorAlignmentValue.Opposed));
6173
// m_indexerMotor3.setControl(
6274
// new Follower(m_indexerMotor1.getDeviceID(), MotorAlignmentValue.Aligned));
6375

64-
m_simState = m_indexerMotor1.getSimState();
76+
m_simState1 = m_indexerMotor1.getSimState();
77+
m_simState2 = m_indexerMotor2.getSimState();
6578
}
6679

67-
public void setSpeed(double speed) {
68-
m_indexerMotor1.set(speed);
80+
public void setSpeeds(double speed1, double speed2) {
81+
m_indexerMotor1.set(speed1);
82+
m_indexerMotor2.set(speed2);
6983
}
7084

7185
public boolean isConnected() {
72-
return m_indexerMotor1.isConnected();
86+
return m_indexerMotor1.isConnected() && m_indexerMotor2.isConnected();
7387
}
7488

75-
@Logged(name = "Motor Output", importance = Logged.Importance.INFO)
89+
@Logged(name = "Motor Output 1", importance = Logged.Importance.INFO)
7690
public double getPercentOutput() {
7791
return m_indexerMotor1.get();
7892
}
7993

94+
@Logged(name = "Motor Output 2", importance = Logged.Importance.INFO)
95+
public double getPercentOutput2() {
96+
return m_indexerMotor2.get();
97+
}
98+
8099
@Override
81100
public void periodic() {}
82101

83102
@Override
84103
public void simulationPeriodic() {
85-
m_simState.setSupplyVoltage(RobotController.getBatteryVoltage());
86-
m_indexerMotor1Sim.setInputVoltage(m_simState.getMotorVoltage());
104+
m_simState1.setSupplyVoltage(RobotController.getBatteryVoltage());
105+
m_indexerMotor1Sim.setInputVoltage(m_simState1.getMotorVoltage());
87106

88107
m_indexerMotor1Sim.update(0.02);
89108

90-
m_simState.setRawRotorPosition(
109+
m_simState1.setRawRotorPosition(
91110
Rotations.of(m_indexerMotor1Sim.getAngularPositionRotations()).times(INDEXER.gearRatio));
92-
m_simState.setRotorVelocity(
111+
m_simState1.setRotorVelocity(
93112
RPM.of(m_indexerMotor1Sim.getAngularVelocityRPM()).times(INDEXER.gearRatio));
113+
m_simState2.setSupplyVoltage(RobotController.getBatteryVoltage());
114+
m_indexerMotor2Sim.setInputVoltage(m_simState2.getMotorVoltage());
115+
116+
m_indexerMotor2Sim.update(0.02);
117+
118+
m_simState2.setRawRotorPosition(
119+
Rotations.of(m_indexerMotor2Sim.getAngularPositionRotations()).times(INDEXER.gearRatio));
120+
m_simState2.setRotorVelocity(
121+
RPM.of(m_indexerMotor2Sim.getAngularVelocityRPM()).times(INDEXER.gearRatio));
122+
}
123+
124+
public void testInit() {
125+
var topic =
126+
NetworkTableInstance.getDefault()
127+
.getTable("SmartDashboard")
128+
.getDoubleTopic("Indexer Roller Speed Setpoint 1");
129+
m_speedSubscriber1 = topic.subscribe(0.0);
130+
m_speedPublisher1 = topic.publish();
131+
var topic2 =
132+
NetworkTableInstance.getDefault()
133+
.getTable("SmartDashboard")
134+
.getDoubleTopic("Indexer Roller Speed Setpoint 2");
135+
m_speedSubscriber2 = topic2.subscribe(0.0);
136+
m_speedPublisher2 = topic2.publish();
137+
m_speedPublisher1.set(0.0);
138+
m_speedPublisher2.set(0.0);
139+
}
140+
141+
public void testPeriodic() {
142+
setSpeeds(m_speedSubscriber1.get(), m_speedSubscriber2.get());
94143
}
95144
}

0 commit comments

Comments
 (0)