Skip to content

Commit e121375

Browse files
mwitcpalekmwitcpalek
authored
Merge pull request #1 from strykeforce/biscuit
Initial Biscuit Subsystem
2 parents 3543661 + 46aebd4 commit e121375

File tree

5 files changed

+277
-7
lines changed

5 files changed

+277
-7
lines changed

‎.vscode/launch.json‎

Lines changed: 9 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -4,18 +4,24 @@
44
// For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
55
"version": "0.2.0",
66
"configurations": [
7-
7+
{
8+
"type": "java",
9+
"name": "Main",
10+
"request": "launch",
11+
"mainClass": "frc.robot.Main",
12+
"projectName": "reefscape"
13+
},
814
{
915
"type": "wpilib",
1016
"name": "WPILib Desktop Debug",
1117
"request": "launch",
12-
"desktop": true,
18+
"desktop": true
1319
},
1420
{
1521
"type": "wpilib",
1622
"name": "WPILib roboRIO Debug",
1723
"request": "launch",
18-
"desktop": false,
24+
"desktop": false
1925
}
2026
]
2127
}

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

Lines changed: 101 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,103 @@
11
package frc.robot.constants;
22

3-
public class BiscuitConstants {}
3+
import static edu.wpi.first.units.Units.Rotations;
4+
5+
import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
6+
import com.ctre.phoenix6.configs.HardwareLimitSwitchConfigs;
7+
import com.ctre.phoenix6.configs.MotionMagicConfigs;
8+
import com.ctre.phoenix6.configs.MotorOutputConfigs;
9+
import com.ctre.phoenix6.configs.Slot0Configs;
10+
import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs;
11+
import com.ctre.phoenix6.configs.TalonFXConfiguration;
12+
import com.ctre.phoenix6.configs.VoltageConfigs;
13+
import com.ctre.phoenix6.signals.ForwardLimitSourceValue;
14+
import com.ctre.phoenix6.signals.ForwardLimitTypeValue;
15+
import com.ctre.phoenix6.signals.GravityTypeValue;
16+
import com.ctre.phoenix6.signals.NeutralModeValue;
17+
import com.ctre.phoenix6.signals.ReverseLimitSourceValue;
18+
import com.ctre.phoenix6.signals.ReverseLimitTypeValue;
19+
import edu.wpi.first.units.measure.Angle;
20+
21+
public class BiscuitConstants {
22+
// These are all wrong right now because we don't have any actual info
23+
24+
public static Angle kZero = Rotations.of(42); // Will need to be experimentally determined
25+
public static int talonID = 3;
26+
public static double kCloseEnough = 2137473647; // This is a little out of wack.
27+
public static final Angle kMaxFwd = Rotations.of(100);
28+
public static final Angle kMaxRev = Rotations.of(-100);
29+
// public static Angle Level1 = ;
30+
31+
// Disables the TalonFX by setting it's voltage to zero. Not very shocking.
32+
public static VoltageConfigs disableTalon() {
33+
VoltageConfigs voltage =
34+
new VoltageConfigs().withPeakForwardVoltage(0.0).withPeakReverseVoltage(0.0);
35+
getFXConfig().Voltage = voltage;
36+
return voltage;
37+
}
38+
39+
// I copied and pasted this because I'm lazy
40+
public static TalonFXConfiguration getFXConfig() {
41+
TalonFXConfiguration fxConfig = new TalonFXConfiguration();
42+
43+
CurrentLimitsConfigs current =
44+
new CurrentLimitsConfigs()
45+
.withStatorCurrentLimit(10)
46+
.withStatorCurrentLimitEnable(false)
47+
.withStatorCurrentLimit(20)
48+
.withSupplyCurrentLimit(10)
49+
.withSupplyCurrentLowerLimit(8)
50+
.withSupplyCurrentLowerTime(0.02)
51+
.withSupplyCurrentLimitEnable(true);
52+
fxConfig.CurrentLimits = current;
53+
54+
HardwareLimitSwitchConfigs hwLimit =
55+
new HardwareLimitSwitchConfigs()
56+
.withForwardLimitAutosetPositionEnable(false)
57+
.withForwardLimitEnable(false)
58+
.withForwardLimitType(ForwardLimitTypeValue.NormallyOpen)
59+
.withForwardLimitSource(ForwardLimitSourceValue.LimitSwitchPin)
60+
.withReverseLimitAutosetPositionEnable(false)
61+
.withReverseLimitEnable(false)
62+
.withReverseLimitType(ReverseLimitTypeValue.NormallyOpen)
63+
.withReverseLimitSource(ReverseLimitSourceValue.LimitSwitchPin);
64+
fxConfig.HardwareLimitSwitch = hwLimit;
65+
66+
SoftwareLimitSwitchConfigs swLimit =
67+
new SoftwareLimitSwitchConfigs()
68+
.withForwardSoftLimitEnable(true)
69+
.withForwardSoftLimitThreshold(kMaxFwd)
70+
.withReverseSoftLimitEnable(true)
71+
.withReverseSoftLimitThreshold(kMaxRev);
72+
fxConfig.SoftwareLimitSwitch = swLimit;
73+
74+
Slot0Configs slot0 =
75+
new Slot0Configs()
76+
.withKP(0)
77+
.withKI(0)
78+
.withKD(0)
79+
.withGravityType(GravityTypeValue.Elevator_Static)
80+
.withKG(0)
81+
.withKS(0)
82+
.withKV(0)
83+
.withKA(0);
84+
fxConfig.Slot0 = slot0;
85+
86+
MotionMagicConfigs motionMagic =
87+
new MotionMagicConfigs()
88+
.withMotionMagicAcceleration(0)
89+
.withMotionMagicCruiseVelocity(0)
90+
.withMotionMagicExpo_kA(0)
91+
.withMotionMagicExpo_kV(0)
92+
.withMotionMagicJerk(0);
93+
fxConfig.MotionMagic = motionMagic;
94+
95+
MotorOutputConfigs motorOut =
96+
new MotorOutputConfigs()
97+
.withDutyCycleNeutralDeadband(0.01)
98+
.withNeutralMode(NeutralModeValue.Coast);
99+
fxConfig.MotorOutput = motorOut;
100+
101+
return fxConfig;
102+
}
103+
}

‎src/main/java/frc/robot/subsystems/biscuit/BiscuitIO.java‎

Lines changed: 26 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,28 @@
11
package frc.robot.subsystems.biscuit;
22

3-
public class BiscuitIO {}
3+
import static edu.wpi.first.units.Units.Rotations;
4+
import static edu.wpi.first.units.Units.RotationsPerSecond;
5+
6+
import edu.wpi.first.units.measure.Angle;
7+
import edu.wpi.first.units.measure.AngularVelocity;
8+
import org.littletonrobotics.junction.AutoLog;
9+
import org.strykeforce.telemetry.TelemetryService;
10+
11+
public interface BiscuitIO {
12+
13+
@AutoLog
14+
public class BiscuitIOInputs {
15+
public Angle position = Rotations.of(0);
16+
public AngularVelocity velocity = RotationsPerSecond.of(0);
17+
public boolean fwdLimitSwitchOpen = false;
18+
public boolean didZero;
19+
}
20+
21+
public default void setPosition(Angle position) {}
22+
23+
public default void updateInputs(BiscuitIOInputs inputs) {}
24+
25+
public default void registerWith(TelemetryService telemetry) {}
26+
27+
public default void zero() {}
28+
}

‎src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFX.java‎

Lines changed: 81 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,83 @@
11
package frc.robot.subsystems.biscuit;
22

3-
public class BiscuitIOFX {}
3+
import com.ctre.phoenix6.BaseStatusSignal;
4+
import com.ctre.phoenix6.StatusSignal;
5+
import com.ctre.phoenix6.configs.TalonFXConfiguration;
6+
import com.ctre.phoenix6.configs.TalonFXConfigurator;
7+
import com.ctre.phoenix6.controls.MotionMagicDutyCycle;
8+
import com.ctre.phoenix6.hardware.TalonFX;
9+
import com.ctre.phoenix6.signals.ForwardLimitTypeValue;
10+
import edu.wpi.first.units.measure.Angle;
11+
import edu.wpi.first.units.measure.AngularVelocity;
12+
import edu.wpi.first.wpilibj.Alert;
13+
import edu.wpi.first.wpilibj.Alert.AlertType;
14+
import frc.robot.constants.BiscuitConstants;
15+
import frc.robot.subsystems.biscuit.BiscuitIO.BiscuitIOInputs;
16+
import org.slf4j.Logger;
17+
import org.slf4j.LoggerFactory;
18+
import org.strykeforce.telemetry.TelemetryService;
19+
20+
public class BiscuitIOFX implements BiscuitIO {
21+
22+
private Logger logger;
23+
private TalonFX talon;
24+
25+
private final Angle sensorInitial;
26+
private StatusSignal<Angle> position;
27+
private StatusSignal<AngularVelocity> velocity;
28+
private StatusSignal<ForwardLimitTypeValue> fwdLimitSwitch;
29+
private boolean didZero;
30+
private boolean fwdLimitSwitchOpen;
31+
private Angle offset;
32+
private Alert rangeAlert = new Alert("Biscuit overextended! Shuting down!", AlertType.kError);
33+
34+
TalonFXConfigurator configurator;
35+
private MotionMagicDutyCycle positionRequest =
36+
new MotionMagicDutyCycle(0).withEnableFOC(false).withFeedForward(0);
37+
38+
public BiscuitIOFX() {
39+
// Logger initialization with class name
40+
logger = LoggerFactory.getLogger(this.getClass());
41+
// Moter initialization with ID from constants
42+
talon = new TalonFX(BiscuitConstants.talonID);
43+
// Set the starting encoder position
44+
sensorInitial = talon.getPosition().getValue();
45+
46+
// Reset and configure moter settings
47+
configurator = talon.getConfigurator();
48+
configurator.apply(new TalonFXConfiguration());
49+
configurator.apply(BiscuitConstants.getFXConfig());
50+
// Set our variables
51+
velocity = talon.getVelocity();
52+
position = talon.getPosition();
53+
}
54+
55+
public void setPosition(Angle position) {
56+
talon.setControl(positionRequest.withPosition(position));
57+
}
58+
59+
public void updateInputs(BiscuitIOInputs inputs) {
60+
BaseStatusSignal.refreshAll(velocity, position, fwdLimitSwitch);
61+
inputs.velocity = velocity.getValue();
62+
inputs.position = position.getValue();
63+
inputs.fwdLimitSwitchOpen = fwdLimitSwitch.getValueAsDouble() == 1;
64+
inputs.didZero = didZero;
65+
}
66+
67+
public void registerWith(TelemetryService telemetry) {
68+
telemetry.register(talon, true);
69+
}
70+
71+
public void zero() {
72+
didZero = false;
73+
if (fwdLimitSwitchOpen == true) {
74+
Angle pos = position.getValue();
75+
offset = BiscuitConstants.kZero.minus(pos);
76+
didZero = true;
77+
} else {
78+
rangeAlert.set(true);
79+
logger.error("Biscuit overextended! Shutting down movement!");
80+
configurator.apply(BiscuitConstants.disableTalon());
81+
}
82+
}
83+
}

‎src/main/java/frc/robot/subsystems/biscuit/BiscuitSubsystem.java‎

Lines changed: 60 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,62 @@
1+
// Blessed by the great tech-priests of the Adeptus Mechanicus
2+
13
package frc.robot.subsystems.biscuit;
24

3-
public class BiscuitSubsystem {}
5+
import static edu.wpi.first.units.Units.Rotations;
6+
7+
import edu.wpi.first.units.measure.*;
8+
import frc.robot.constants.BiscuitConstants;
9+
import java.util.Set;
10+
import org.littletonrobotics.junction.Logger;
11+
import org.strykeforce.telemetry.TelemetryService;
12+
import org.strykeforce.telemetry.measurable.MeasurableSubsystem;
13+
import org.strykeforce.telemetry.measurable.Measure;
14+
15+
public class BiscuitSubsystem extends MeasurableSubsystem {
16+
17+
private BiscuitIO io;
18+
private BiscuitIOInputsAutoLogged inputs = new BiscuitIOInputsAutoLogged();
19+
private Angle setPoint;
20+
21+
public void setPosition(Angle position) {
22+
io.setPosition(position);
23+
setPoint = position;
24+
}
25+
26+
public Angle getPosition(Angle position) {
27+
return inputs.position;
28+
}
29+
30+
public AngularVelocity getVelocity(AngularVelocity velocity) {
31+
return inputs.velocity;
32+
}
33+
34+
@Override
35+
public void periodic() {
36+
io.updateInputs(inputs);
37+
Logger.processInputs(getName(), inputs);
38+
Logger.recordOutput("Biscuit setPoint", setPoint);
39+
Logger.recordOutput("Is Biscuit Finished", isFinished() ? 1.0 : 0.0);
40+
}
41+
42+
@Override
43+
public void registerWith(TelemetryService telemetry) {
44+
super.registerWith(telemetry);
45+
io.registerWith(telemetry);
46+
}
47+
48+
public boolean isFinished() {
49+
return setPoint.minus(inputs.position).abs(Rotations) <= BiscuitConstants.kCloseEnough;
50+
}
51+
52+
@Override
53+
public Set<Measure> getMeasures() {
54+
return Set.of(
55+
new Measure("Is Biscuit Finished", () -> isFinished() ? 1.0 : 0.0),
56+
new Measure("Biscuit Set Point", () -> setPoint.in(Rotations)));
57+
}
58+
59+
public void zero() {
60+
io.zero();
61+
}
62+
}

0 commit comments

Comments
 (0)