Skip to content

Commit 4723a73

Browse files
quakeqquakeq
committed
finished canrange classes, need to figure out good way to sim it
1 parent 3fe6b53 commit 4723a73

File tree

13 files changed

+115
-875
lines changed

13 files changed

+115
-875
lines changed

‎src/main/java/frc/lib/subsystems/canDevice/CanRangeIOHardware.java‎

Lines changed: 6 additions & 36 deletions
Original file line numberDiff line numberDiff line change
@@ -14,69 +14,39 @@ public class CanRangeIOHardware implements CanRangeIO {
1414
protected CanRangeConfig config;
1515
private final BaseStatusSignal[] signals;
1616

17-
private StatusSignal<Boolean> trippedSignal;
18-
private StatusSignal<Distance> distanceSignal;
1917
private StatusSignal<Boolean> trippedSignal;
2018
private StatusSignal<Distance> distanceSignal;
2119

2220
public CanRangeIOHardware(CanRangeConfig config) {
21+
canRange = new CANrange(config.CANID.getDeviceNumber(), config.CANID.getBusName());
2322
this.config = config;
24-
public CanRangeIOHardware(CanRangeConfig config) {
25-
this.config = config;
26-
27-
canrange = new CANrange(config.CANID.getDeviceNumber(), config.CANID.getBusName());
28-
canrange = new CANrange(config.CANID.getDeviceNumber(), config.CANID.getBusName());
2923

30-
CTREUtil.applyConfiguration(canrange, this.config.config);
31-
trippedSignal = canrange.getIsDetected();
32-
distanceSignal = canrange.getDistance();
33-
CTREUtil.applyConfiguration(canrange, this.config.config);
34-
trippedSignal = canrange.getIsDetected();
35-
distanceSignal = canrange.getDistance();
24+
CTREUtil.applyConfiguration(canRange, this.config.config);
25+
trippedSignal = canRange.getIsDetected();
26+
distanceSignal = canRange.getDistance();
3627

37-
signals = new BaseStatusSignal[] {trippedSignal, distanceSignal};
3828
signals = new BaseStatusSignal[] {trippedSignal, distanceSignal};
3929

40-
BaseStatusSignal.setUpdateFrequencyForAll(100.0, signals);
4130
BaseStatusSignal.setUpdateFrequencyForAll(100.0, signals);
4231

4332
CANStatusLogger.getInstance()
4433
.registerCANrange(
4534
"CANrange_ID" + config.CANID.getDeviceNumber(),
46-
canrange,
47-
config.CANID.getDeviceNumber(),
48-
config.CANID.getBusName());
49-
}
50-
CANStatusLogger.getInstance()
51-
.registerCANrange(
52-
"CANrange_ID" + config.CANID.getDeviceNumber(),
53-
canrange,
35+
canRange,
5436
config.CANID.getDeviceNumber(),
5537
config.CANID.getBusName());
5638
}
5739

5840
@Override
5941
public void readInputs(CanRangeInputs inputs) {
6042
BaseStatusSignal.refreshAll(signals);
61-
@Override
62-
public void readInputs(CanRangeInputs inputs) {
63-
BaseStatusSignal.refreshAll(signals);
64-
65-
inputs.isTripped = trippedSignal.getValue();
66-
inputs.distanceMeters = distanceSignal.getValue().in(Meters);
67-
inputs.isConnected = canrange.isConnected();
68-
}
6943
inputs.isTripped = trippedSignal.getValue();
7044
inputs.distanceMeters = distanceSignal.getValue().in(Meters);
71-
inputs.isConnected = canrange.isConnected();
45+
inputs.isConnected = canRange.isConnected();
7246
}
7347

7448
@Override
7549
public void updateFrequency(double hz) {
7650
BaseStatusSignal.setUpdateFrequencyForAll(hz, signals);
7751
}
78-
@Override
79-
public void updateFrequency(double hz) {
80-
BaseStatusSignal.setUpdateFrequencyForAll(hz, signals);
81-
}
8252
}

‎src/main/java/frc/lib/subsystems/simulation/SimCanRangeIO.java‎

Lines changed: 25 additions & 31 deletions
Original file line numberDiff line numberDiff line change
@@ -1,40 +1,34 @@
11
package frc.lib.subsystems.simulation;
22

3-
import java.util.function.Supplier;
4-
5-
import com.ctre.phoenix6.signals.SensorDirectionValue;
63
import com.ctre.phoenix6.sim.CANrangeSimState;
7-
84
import frc.lib.subsystems.canDevice.CanRangeConfig;
95
import frc.lib.subsystems.canDevice.CanRangeIOHardware;
106
import frc.lib.subsystems.canDevice.CanRangeInputs;
11-
7+
import java.util.function.Supplier;
128

139
public class SimCanRangeIO extends CanRangeIOHardware {
14-
public static class SimCanRangeState{
15-
public boolean isTripped;
16-
public double distanceMeters;
17-
}
18-
19-
20-
protected CANrangeSimState simState;
21-
protected Supplier<SimCanRangeState> supplier;
22-
23-
public SimCanRangeIO(CanRangeConfig config, Supplier<SimCanRangeState> supplier) {
24-
super(config);
25-
simState = this.canRange.getSimState();
26-
this.supplier = supplier;
27-
// this.simState.setDistance(0);
28-
this.simState.setSupplyVoltage(0);
29-
}
30-
31-
@Override
32-
public void readInputs(CanRangeInputs inputs) {
33-
var suppliedState = supplier.get();
34-
this.simState.setDistance(suppliedState.distanceMeters);
35-
this.simState.setSupplyVoltage(suppliedState.isTripped ? 1000000.0 : 0.0);
36-
37-
super.readInputs(inputs);
38-
}
39-
10+
public static class SimCanRangeState {
11+
public boolean isTripped;
12+
public double distanceMeters;
13+
}
14+
15+
protected CANrangeSimState simState;
16+
protected Supplier<SimCanRangeState> supplier;
17+
18+
public SimCanRangeIO(CanRangeConfig config, Supplier<SimCanRangeState> supplier) {
19+
super(config);
20+
simState = this.canRange.getSimState();
21+
this.supplier = supplier;
22+
// this.simState.setDistance(0);
23+
this.simState.setSupplyVoltage(0);
24+
}
25+
26+
@Override
27+
public void readInputs(CanRangeInputs inputs) {
28+
var suppliedState = supplier.get();
29+
this.simState.setDistance(suppliedState.distanceMeters);
30+
this.simState.setSupplyVoltage(suppliedState.isTripped ? 1000000.0 : 0.0);
31+
32+
super.readInputs(inputs);
33+
}
4034
}

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

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -35,6 +35,8 @@
3535
import edu.wpi.first.wpilibj.RobotBase;
3636
import frc.lib.drivers.CANDeviceId;
3737
import frc.lib.subsystems.canDevice.CanCoderConfig;
38+
import frc.lib.subsystems.canDevice.CanRangeConfig;
39+
import frc.lib.subsystems.real.ServoMotorSubsystemConfig;
3840
import frc.lib.subsystems.real.ServoMotorSubsystemWithCanCoderConfig;
3941
import frc.lib.subsystems.real.ServoMotorSubsystemWithFollowersConfig;
4042
import frc.lib.subsystems.simulation.SimElevator;
@@ -638,6 +640,26 @@ public static class EndEffectorConstants2 {
638640
public static final ServoMotorSubsystemWithCanCoderConfig kEndEffectorConfig =
639641
new ServoMotorSubsystemWithCanCoderConfig();
640642

643+
public static final ServoMotorSubsystemConfig kClawConfig = new ServoMotorSubsystemConfig();
644+
645+
public static final CanRangeConfig kCanRangeConfig = new CanRangeConfig();
646+
public static final CanRangeConfig kCanRangeConfig2 = new CanRangeConfig();
647+
648+
static {
649+
kClawConfig.name = "Claw";
650+
kClawConfig.talonCANID = new CANDeviceId(EndEffectorConstants.rollerID, MISC_CANIVORE);
651+
kClawConfig.unitToRotorRatio = 1 / EndEffectorConstants.CORAL_GEAR_RATIO;
652+
653+
kClawConfig.fxConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
654+
kClawConfig.fxConfig.CurrentLimits.SupplyCurrentLimitEnable = true;
655+
kClawConfig.fxConfig.CurrentLimits.SupplyCurrentLimit = ROLLER_CURRENT_LIMIT_AMPS;
656+
657+
kCanRangeConfig.CANID =
658+
new CANDeviceId(EndEffectorConstants.FIRST_CORAL_CANRANGE_ID, Constants.MISC_CANIVORE);
659+
kCanRangeConfig2.CANID =
660+
new CANDeviceId(EndEffectorConstants.SECOND_CORAL_CANRANGE_ID, Constants.MISC_CANIVORE);
661+
}
662+
641663
static {
642664
kEndEffectorConfig.name = "EndEffector";
643665
kEndEffectorConfig.talonCANID =

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -309,7 +309,7 @@ public void testInit() {
309309
public void testPeriodic() {
310310
LoopTimingLogger.startTiming("TestPeriodic");
311311
// Add any test-specific code here if needed
312-
Commands.run(() -> robotContainer.getClaw().setRollerVoltage(1))
312+
Commands.run(() -> robotContainer.getClaw().setVoltage(() -> 1))
313313
.withInterruptBehavior(InterruptionBehavior.kCancelIncoming)
314314
.schedule();
315315
Commands.run(() -> robotContainer.getIntake().setRollerVoltage(1))

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

Lines changed: 37 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -18,7 +18,10 @@
1818
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
1919
import frc.lib.subsystems.TalonFXIO;
2020
import frc.lib.subsystems.canDevice.CanCoderIOHardware;
21+
import frc.lib.subsystems.canDevice.CanRangeIOHardware;
2122
import frc.lib.subsystems.simulation.SimCanCoderIO;
23+
import frc.lib.subsystems.simulation.SimCanRangeIO;
24+
import frc.lib.subsystems.simulation.SimCanRangeIO.SimCanRangeState;
2225
import frc.lib.subsystems.simulation.SimElevator;
2326
import frc.lib.subsystems.simulation.SimTalonFXWithCancoder;
2427
import frc.robot.Constants.ElevatorConstants.Elevator2Constants;
@@ -44,10 +47,6 @@
4447
import frc.robot.subsystems.drive.DriveSubsystem;
4548
import frc.robot.subsystems.elevator.Elevator;
4649
import frc.robot.subsystems.end_effector.Claw;
47-
import frc.robot.subsystems.end_effector.ClawIO;
48-
import frc.robot.subsystems.end_effector.ClawIOReal;
49-
import frc.robot.subsystems.end_effector.ClawIOSim;
50-
import frc.robot.subsystems.end_effector.ClawOld;
5150
import frc.robot.subsystems.end_effector.EndEffector;
5251
import frc.robot.subsystems.feeder.Feeder;
5352
import frc.robot.subsystems.feeder.FeederIO;
@@ -110,6 +109,9 @@ public void accept(VisionFieldPoseEstimate estimate) {
110109
// private NamedCommandsSetup namedCommands;
111110
private AutoChooserSetup autoChooserSetup;
112111

112+
SimCanRangeState clawSimRange1 = new SimCanRangeState();
113+
SimCanRangeState clawSimRange2 = new SimCanRangeState();
114+
113115
/** The container for the robot. Contains subsystems, OI devices, and commands. */
114116
public RobotContainer() {
115117
elevator = buildElevator2();
@@ -128,7 +130,13 @@ public RobotContainer() {
128130
new TalonFXIO(EndEffectorConstants2.kEndEffectorConfig),
129131
new CanCoderIOHardware(EndEffectorConstants2.kEndEffectorConfig.canCoderConfig),
130132
robotState);
131-
claw = new Claw(new ClawIOReal() {});
133+
claw =
134+
new Claw(
135+
EndEffectorConstants2.kClawConfig,
136+
new TalonFXIO(EndEffectorConstants2.kClawConfig),
137+
robotState,
138+
new CanRangeIOHardware(EndEffectorConstants2.kCanRangeConfig),
139+
new CanRangeIOHardware(EndEffectorConstants2.kCanRangeConfig2));
132140
// elevator = new ElevatorOld(new ElevatorIOReal());
133141
superstructure = new Superstructure(elevator, endEffector, this);
134142
climber = new Climber(new ClimberIOReal());
@@ -158,7 +166,13 @@ public RobotContainer() {
158166
EndEffectorConstants2.kEndEffectorConfig)),
159167
robotState);
160168
// elevator = new ElevatorOld(new ElevatorIOSim());
161-
claw = new Claw(new ClawIOSim() {});
169+
claw =
170+
new Claw(
171+
EndEffectorConstants2.kClawConfig,
172+
simulatedEndEffectorMotor,
173+
robotState,
174+
new SimCanRangeIO(EndEffectorConstants2.kCanRangeConfig, () -> clawSimRange1),
175+
new SimCanRangeIO(EndEffectorConstants2.kCanRangeConfig2, () -> clawSimRange2));
162176
superstructure = new Superstructure(elevator, endEffector, this);
163177
climber = new Climber(new ClimberIOSim());
164178
climbRoller = new ClimbRoller(new ClimbRollerIOSim());
@@ -186,7 +200,13 @@ public RobotContainer() {
186200
simulatedEndEffectorMotor.getSupplierForCancoder(
187201
EndEffectorConstants2.kEndEffectorConfig)),
188202
robotState);
189-
claw = new Claw(new ClawIO() {});
203+
claw =
204+
new Claw(
205+
EndEffectorConstants2.kClawConfig,
206+
new TalonFXIO(EndEffectorConstants2.kClawConfig),
207+
robotState,
208+
new CanRangeIOHardware(EndEffectorConstants2.kCanRangeConfig),
209+
new CanRangeIOHardware(EndEffectorConstants2.kCanRangeConfig2));
190210
// elevator = new ElevatorOld(new ElevatorIO() {});
191211
superstructure = new Superstructure(elevator, endEffector, this);
192212
climber = new Climber(new ClimberIO() {});
@@ -313,4 +333,14 @@ public Led getLed() {
313333
public RobotState getRobotState() {
314334
return robotState;
315335
}
336+
337+
public void setSimCanRange1(boolean tripped, double distanceMeters) {
338+
clawSimRange1.isTripped = tripped;
339+
clawSimRange1.distanceMeters = distanceMeters;
340+
}
341+
342+
public void setSimCanRange2(boolean tripped, double distanceMeters) {
343+
clawSimRange2.isTripped = tripped;
344+
clawSimRange2.distanceMeters = distanceMeters;
345+
}
316346
}

‎src/main/java/frc/robot/commands/test/CleaningTest.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -10,7 +10,7 @@ public class CleaningTest extends ParallelCommandGroup {
1010

1111
public CleaningTest(Intake intake, Claw claw, Feeder feeder) {
1212
addCommands(
13-
new InstantCommand(() -> claw.setRollerVoltage(1)),
13+
new InstantCommand(() -> claw.setVoltage(() -> 1)),
1414
new InstantCommand(() -> intake.setRollerVoltage(1)),
1515
new InstantCommand(() -> feeder.setRollerVoltage(1)));
1616
}

‎src/main/java/frc/robot/humanControls/ElasticTabs.java‎

Lines changed: 14 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -127,12 +127,12 @@ private void buildEndEffectorTab() {
127127
elasticTabMap.put(key, tab);
128128

129129
// Configure the while-held behavior
130-
tab.addButton("Roller Forward (While Held)")
131-
.setupWhileHeldCommand(claw.rollerFWD(), claw.rollerSTOP());
132-
tab.addButton("Roller Reverse (While Held)")
133-
.setupWhileHeldCommand(claw.rollerRVS(), claw.rollerSTOP());
134-
tab.addButton("Roller Hold (While Held)")
135-
.setupWhileHeldCommand(claw.holdAlgae(), claw.holdAlgae());
130+
// tab.addButton("Roller Forward (While Held)")
131+
// .setupWhileHeldCommand(claw.rollerFWD(), claw.rollerSTOP());
132+
// tab.addButton("Roller Reverse (While Held)")
133+
// .setupWhileHeldCommand(claw.rollerRVS(), claw.rollerSTOP());
134+
// tab.addButton("Roller Hold (While Held)")
135+
// .setupWhileHeldCommand(claw.holdAlgae(), claw.holdAlgae());
136136
tab.addButton("Pivot Up (While Held)")
137137
.setupWhileHeldCommand(endEffector.pivotUP(), endEffector.pivotSTOP());
138138
tab.addButton("Pivot Down (While Held)")
@@ -168,6 +168,14 @@ private void buildEndEffectorTab() {
168168
/ 2));
169169
tab.addButton("Pivot Zero (When Pressed)")
170170
.setupOnPressCommandIgnoringDisabled(endEffector.setPivotZero());
171+
172+
tab.addButton("Toggle First Sensor (When Pressed)")
173+
.getTrigger()
174+
.onTrue(
175+
Commands.runOnce(
176+
() -> {
177+
container.setSimCanRange1(false, 0.0);
178+
}));
171179
}
172180

173181
private void buildElevatorTab() {

‎src/main/java/frc/robot/subsystems/end_effector/Claw.java‎

Lines changed: 9 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -1,9 +1,5 @@
11
package frc.robot.subsystems.end_effector;
22

3-
import java.util.function.DoubleSupplier;
4-
5-
import org.littletonrobotics.junction.Logger;
6-
73
import edu.wpi.first.wpilibj2.command.Command;
84
import edu.wpi.first.wpilibj2.command.Commands;
95
import edu.wpi.first.wpilibj2.command.InstantCommand;
@@ -20,8 +16,10 @@
2016
import frc.robot.subsystems.superstructure.CoralStateTracker;
2117
import frc.robot.subsystems.superstructure.SuperstructureState;
2218
import frc.robot.util.RobotTime;
19+
import java.util.function.DoubleSupplier;
20+
import org.littletonrobotics.junction.Logger;
2321

24-
public class Claw extends SubsystemBase {
22+
public class Claw extends ServoMotorSubsystem<MotorInputsAutoLogged, MotorIO> {
2523

2624
private final RobotState robotState;
2725
private final CanRangeIO firstCANRangeIO;
@@ -55,8 +53,6 @@ public void periodic() {
5553
Logger.processInputs(getName() + "/firstCanRange", firstRangeAutoLog);
5654
Logger.processInputs(getName() + "/firstCanRange", secondRangeAutoLog);
5755

58-
59-
6056
CoralStateTracker.updateFirstEndEffector(firstSensorTriggered);
6157
CoralStateTracker.updateSecondEndEffector(secondSensorTriggered);
6258

@@ -96,7 +92,9 @@ public boolean isCoralAtSecondSensor() {
9692
}
9793

9894
public boolean hasAlgae() {
99-
return isMotorStalled(EndEffectorConstants.ROLLER_STALLED_CURRENT, EndEffectorConstants.ROLLER_STALLED_RPS) && !isCoralInClaw();
95+
return isMotorStalled(
96+
EndEffectorConstants.ROLLER_STALLED_CURRENT, EndEffectorConstants.ROLLER_STALLED_RPS)
97+
&& !isCoralInClaw();
10098
}
10199

102100
public Command clawDefault() {
@@ -217,15 +215,15 @@ public Command setClawStateCommand(ClawState state) {
217215
});
218216
}
219217

220-
private Command setVoltage(DoubleSupplier voltageSupplier) {
218+
public Command setVoltage(DoubleSupplier voltageSupplier) {
221219
return new InstantCommand(() -> setVoltageImpl(voltageSupplier.getAsDouble()), this);
222220
}
223221

224-
private Command setPosition(DoubleSupplier positionSupplier) {
222+
public Command setPosition(DoubleSupplier positionSupplier) {
225223
return new InstantCommand(() -> setPositionSetpointImpl(positionSupplier.getAsDouble()), this);
226224
}
227225

228-
private Command setTorque(DoubleSupplier torqueSupplier) {
226+
public Command setTorque(DoubleSupplier torqueSupplier) {
229227
return new InstantCommand(() -> setTorqueCurrentFOCImpl(torqueSupplier.getAsDouble()), this);
230228
}
231229
}

0 commit comments

Comments
 (0)