Skip to content

Commit 509991f

Browse files
mwitcpalekmwitcpalek
authored
Merge pull request #4 from strykeforce/Funnel
Initial Funnel Subsystem
2 parents 2f2d067 + 31f5be5 commit 509991f

File tree

4 files changed

+227
-3
lines changed

4 files changed

+227
-3
lines changed

‎src/main/java/frc/robot/constants/FunnelConstants.java‎

Lines changed: 60 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,60 @@
1+
package frc.robot.constants;
2+
3+
import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
4+
import com.ctre.phoenix6.configs.HardwareLimitSwitchConfigs;
5+
import com.ctre.phoenix6.configs.MotorOutputConfigs;
6+
import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs;
7+
import com.ctre.phoenix6.configs.TalonFXSConfiguration;
8+
import com.ctre.phoenix6.signals.ForwardLimitSourceValue;
9+
import com.ctre.phoenix6.signals.ForwardLimitTypeValue;
10+
import com.ctre.phoenix6.signals.NeutralModeValue;
11+
import com.ctre.phoenix6.signals.ReverseLimitSourceValue;
12+
import com.ctre.phoenix6.signals.ReverseLimitTypeValue;
13+
14+
public class FunnelConstants {
15+
public static final double kFunnelPercentOutput = 0;
16+
17+
public static int FunnelFxsId = 0;
18+
public static final int kFunnelBeamCounts = 3;
19+
20+
public static TalonFXSConfiguration getFXSConfig() {
21+
TalonFXSConfiguration fxsConfig = new TalonFXSConfiguration();
22+
23+
CurrentLimitsConfigs current =
24+
new CurrentLimitsConfigs()
25+
.withStatorCurrentLimit(10)
26+
.withStatorCurrentLimitEnable(false)
27+
.withStatorCurrentLimit(20)
28+
.withSupplyCurrentLimit(10)
29+
.withSupplyCurrentLowerLimit(8)
30+
.withSupplyCurrentLowerTime(0.02)
31+
.withSupplyCurrentLimitEnable(true);
32+
fxsConfig.CurrentLimits = current;
33+
34+
HardwareLimitSwitchConfigs hwLimit =
35+
new HardwareLimitSwitchConfigs()
36+
.withForwardLimitAutosetPositionEnable(false)
37+
.withForwardLimitEnable(false)
38+
.withForwardLimitType(ForwardLimitTypeValue.NormallyOpen)
39+
.withForwardLimitSource(ForwardLimitSourceValue.LimitSwitchPin)
40+
.withReverseLimitAutosetPositionEnable(false)
41+
.withReverseLimitEnable(false)
42+
.withReverseLimitType(ReverseLimitTypeValue.NormallyOpen)
43+
.withReverseLimitSource(ReverseLimitSourceValue.LimitSwitchPin);
44+
fxsConfig.HardwareLimitSwitch = hwLimit;
45+
46+
SoftwareLimitSwitchConfigs swLimit =
47+
new SoftwareLimitSwitchConfigs()
48+
.withForwardSoftLimitEnable(false)
49+
.withReverseSoftLimitEnable(false);
50+
fxsConfig.SoftwareLimitSwitch = swLimit;
51+
52+
MotorOutputConfigs motorOut =
53+
new MotorOutputConfigs()
54+
.withDutyCycleNeutralDeadband(0.01)
55+
.withNeutralMode(NeutralModeValue.Coast);
56+
fxsConfig.MotorOutput = motorOut;
57+
58+
return fxsConfig;
59+
}
60+
}

‎src/main/java/frc/robot/subsystems/funnel/FunnelIO.java‎

Lines changed: 20 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,20 @@
1+
package frc.robot.subsystems.funnel;
2+
3+
import edu.wpi.first.units.measure.AngularVelocity;
4+
import org.littletonrobotics.junction.AutoLog;
5+
import org.strykeforce.telemetry.TelemetryService;
6+
7+
public interface FunnelIO {
8+
9+
@AutoLog
10+
public static class FunnelIOInputs {
11+
public AngularVelocity velocity;
12+
public boolean isRevBeamBroken = false;
13+
}
14+
15+
public default void setPct(double percentOutput) {}
16+
17+
public default void updateInputs(FunnelIOInputs inputs) {}
18+
19+
public default void registerWith(TelemetryService telemetryService) {}
20+
}

‎src/main/java/frc/robot/subsystems/funnel/FunnelIOFXS.java‎

Lines changed: 58 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,58 @@
1+
package frc.robot.subsystems.funnel;
2+
3+
import com.ctre.phoenix6.BaseStatusSignal;
4+
import com.ctre.phoenix6.StatusSignal;
5+
import com.ctre.phoenix6.configs.TalonFXSConfiguration;
6+
import com.ctre.phoenix6.configs.TalonFXSConfigurator;
7+
import com.ctre.phoenix6.controls.DutyCycleOut;
8+
import com.ctre.phoenix6.hardware.TalonFXS;
9+
import com.ctre.phoenix6.signals.ReverseLimitValue;
10+
import edu.wpi.first.units.measure.AngularVelocity;
11+
import frc.robot.constants.FunnelConstants;
12+
import org.slf4j.Logger;
13+
import org.slf4j.LoggerFactory;
14+
import org.strykeforce.telemetry.TelemetryService;
15+
16+
public class FunnelIOFXS implements FunnelIO {
17+
// Private Objects
18+
private Logger logger;
19+
private TalonFXS talonfxs;
20+
21+
// FX Acces Objects
22+
TalonFXSConfigurator configurator;
23+
StatusSignal<AngularVelocity> curVelocity;
24+
StatusSignal<ReverseLimitValue> curRevLimit;
25+
26+
private DutyCycleOut dutyCycleRequest = new DutyCycleOut(0.0).withEnableFOC(false);
27+
28+
public FunnelIOFXS() {
29+
logger = LoggerFactory.getLogger(this.getClass());
30+
talonfxs = new TalonFXS(FunnelConstants.FunnelFxsId);
31+
32+
// Config controller
33+
configurator = talonfxs.getConfigurator();
34+
configurator.apply(new TalonFXSConfiguration());
35+
configurator.apply(FunnelConstants.getFXSConfig());
36+
37+
// Attach status signals
38+
curVelocity = talonfxs.getVelocity();
39+
curRevLimit = talonfxs.getReverseLimit();
40+
}
41+
42+
@Override
43+
public void setPct(double percentOutput) {
44+
talonfxs.setControl(dutyCycleRequest.withOutput(percentOutput));
45+
}
46+
47+
@Override
48+
public void updateInputs(FunnelIOInputs inputs) {
49+
BaseStatusSignal.refreshAll(curVelocity, curRevLimit);
50+
inputs.velocity = curVelocity.getValue();
51+
inputs.isRevBeamBroken = curRevLimit.getValue().value == 1;
52+
}
53+
54+
@Override
55+
public void registerWith(TelemetryService telemetryService) {
56+
telemetryService.register(talonfxs, true);
57+
}
58+
}

‎src/main/java/frc/robot/subsystems/funnel/FunnelSubsystem.java‎

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

3-
public class FunnelSubsystem {
3+
import frc.robot.constants.FunnelConstants;
4+
import frc.robot.standards.OpenLoopSubsystem;
5+
import java.util.Set;
6+
import org.littletonrobotics.junction.Logger;
7+
import org.strykeforce.telemetry.TelemetryService;
8+
import org.strykeforce.telemetry.measurable.MeasurableSubsystem;
9+
import org.strykeforce.telemetry.measurable.Measure;
10+
11+
public class FunnelSubsystem extends MeasurableSubsystem implements OpenLoopSubsystem {
12+
13+
// Private Variables
14+
private final FunnelIO io;
15+
private final FunnelIOInputsAutoLogged inputs = new FunnelIOInputsAutoLogged();
16+
private float totalBreaks = 0;
17+
18+
public FunnelState curState = FunnelState.HasNotSeenCoral;
19+
20+
// Constructor
21+
public FunnelSubsystem(FunnelIO io) {
22+
this.io = io;
23+
}
24+
25+
// Getter/Setter Methods
26+
public FunnelState getState() {
27+
return curState;
28+
}
429

530
public boolean hasCoral() {
6-
// TODO Auto-generated method stub
7-
throw new UnsupportedOperationException("Unimplemented method 'hasCoral'");
31+
return curState == FunnelState.HasSeenCoral;
32+
}
33+
34+
@Override
35+
public void periodic() {
36+
// Update Inputs
37+
io.updateInputs(inputs);
38+
Logger.processInputs("funnelInputs", inputs);
39+
40+
switch (curState) {
41+
case HasSeenCoral:
42+
break;
43+
case HasNotSeenCoral:
44+
if (inputs.isRevBeamBroken == true) {
45+
totalBreaks += 1;
46+
} else {
47+
totalBreaks = 0;
48+
}
49+
50+
if (totalBreaks >= FunnelConstants.kFunnelBeamCounts) {
51+
curState = FunnelState.HasSeenCoral;
52+
}
53+
break;
54+
}
55+
56+
// Log Outputs
57+
Logger.recordOutput("Funnel/curState", curState);
58+
Logger.recordOutput("Funnel/totalBreaks", totalBreaks);
59+
}
60+
61+
// Grapher
62+
@Override
63+
public void registerWith(TelemetryService telemetryService) {
64+
io.registerWith(telemetryService);
65+
super.registerWith(telemetryService);
66+
}
67+
68+
@Override
69+
public Set<Measure> getMeasures() {
70+
return Set.of(new Measure("State", () -> curState.ordinal()));
71+
}
72+
73+
// State Enum
74+
public enum FunnelState {
75+
HasSeenCoral,
76+
HasNotSeenCoral
77+
}
78+
79+
@Override
80+
public void setPercent(double pct) {
81+
io.setPct(pct);
82+
}
83+
84+
public void StartMotor() {
85+
setPercent(FunnelConstants.kFunnelPercentOutput);
86+
}
87+
88+
public void StopMotor() {
89+
setPercent(0.0);
90+
}
91+
92+
public void ClearCoral() {
93+
curState = FunnelState.HasNotSeenCoral;
894
}
995
}

0 commit comments

Comments
 (0)