Skip to content

Commit 92d272f

Browse files
KaydenLee456KaydenLee456
committed
Started some work on IO layer, subsystem
1 parent e173c04 commit 92d272f

File tree

4 files changed

+197
-4
lines changed

4 files changed

+197
-4
lines changed

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

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

3-
public class ElevatorConstants {}
3+
import org.littletonrobotics.junction.AutoLog;
4+
import org.strykeforce.telemetry.TelemetryService;
5+
6+
import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
7+
import com.ctre.phoenix6.configs.HardwareLimitSwitchConfigs;
8+
import com.ctre.phoenix6.configs.MotionMagicConfigs;
9+
import com.ctre.phoenix6.configs.MotorOutputConfigs;
10+
import com.ctre.phoenix6.configs.Slot0Configs;
11+
import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs;
12+
import com.ctre.phoenix6.configs.TalonFXConfiguration;
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.InvertedValue;
17+
import com.ctre.phoenix6.signals.NeutralModeValue;
18+
import com.ctre.phoenix6.signals.ReverseLimitSourceValue;
19+
import com.ctre.phoenix6.signals.ReverseLimitTypeValue;
20+
21+
public class ElevatorConstants {
22+
23+
public static final double kCloseEnough;
24+
public static final double kMaxFwd;
25+
public static final double kMaxRev;
26+
public static final double kZeroTicks;
27+
28+
public static final int kFxIDMain = 20;
29+
public static final int kFxIDFollow = 21;
30+
31+
//Make constants for Level 1 and have all the others be offsets of those constants
32+
33+
public static TalonFXConfiguration getBothFXConfig() {
34+
TalonFXConfiguration fxConfig = new TalonFXConfiguration();
35+
36+
CurrentLimitsConfigs current =
37+
new CurrentLimitsConfigs()
38+
.withStatorCurrentLimit()
39+
.withStatorCurrentLimitEnable()
40+
.withSupplyCurrentLimit()
41+
.withSupplyCurrentThreshold()
42+
.withSupplyCurrentLimitEnable()
43+
.withSupplyTimeThreshold();
44+
fxConfig.CurrentLimits = current;
45+
46+
HardwareLimitSwitchConfigs hwLimit =
47+
new HardwareLimitSwitchConfigs()
48+
.withForwardLimitAutosetPositionEnable()
49+
.withForwardLimitEnable()
50+
.withForwardLimitType(ForwardLimitTypeValue.NormallyOpen)
51+
.withForwardLimitSource(ForwardLimitSourceValue.LimitSwitchPin)
52+
.withReverseLimitAutosetPositionEnable(false)
53+
.withReverseLimitEnable(false)
54+
.withReverseLimitType(ReverseLimitTypeValue.NormallyOpen)
55+
.withReverseLimitSource(ReverseLimitSourceValue.LimitSwitchPin);
56+
fxConfig.HardwareLimitSwitch = hwLimit;
57+
58+
SoftwareLimitSwitchConfigs swLimit =
59+
new SoftwareLimitSwitchConfigs()
60+
.withForwardSoftLimitEnable(false)
61+
.withForwardSoftLimitThreshold(kMaxFwd)
62+
.withReverseSoftLimitEnable(false)
63+
.withReverseSoftLimitThreshold(kMaxRev);
64+
fxConfig.SoftwareLimitSwitch = swLimit;
65+
66+
Slot0Configs slot0 =
67+
new Slot0Configs()
68+
.withKP(0.4)
69+
.withKI(0.1)
70+
.withKD(0)
71+
.withGravityType(GravityTypeValue.Elevator_Static)
72+
.withKG(0)
73+
.withKS(0)
74+
.withKV(0.12)
75+
.withKA(0);
76+
fxConfig.Slot0 = slot0;
77+
78+
MotionMagicConfigs motionMagic =
79+
new MotionMagicConfigs()
80+
.withMotionMagicAcceleration(130)
81+
.withMotionMagicCruiseVelocity(0)
82+
.withMotionMagicExpo_kA(0)
83+
.withMotionMagicExpo_kV(0)
84+
.withMotionMagicJerk(1000);
85+
fxConfig.MotionMagic = motionMagic;
86+
87+
MotorOutputConfigs motorOut =
88+
new MotorOutputConfigs()
89+
.withDutyCycleNeutralDeadband(0.01)
90+
.withNeutralMode(NeutralModeValue.Coast)
91+
.withInverted(InvertedValue.CounterClockwise_Positive);
92+
fxConfig.MotorOutput = motorOut;
93+
94+
return fxConfig;
95+
}
96+
}

‎src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java‎

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

3-
public class ElevatorIO {}
3+
import org.littletonrobotics.junction.AutoLog;
4+
import org.strykeforce.telemetry.TelemetryService;
5+
6+
public interface ElevatorIO {
7+
public double position = 0.0;
8+
public double velocityLeft = 0.0;
9+
public double velocityRight = 0.0;
10+
11+
public default void updateInputs(ExiterIOInputs inputs) {}
12+
13+
public default void registerWith(TelemetryService telemetryService) {}
14+
15+
}

‎src/main/java/frc/robot/subsystems/elevator/ElevatorIOFX.java‎

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

3-
public class ElevatorIOFX {}
3+
import org.slf4j.Logger;
4+
import org.slf4j.LoggerFactory;
5+
import org.strykeforce.telemetry.TelemetryService;
6+
7+
import com.ctre.phoenix.motorcontrol.FollowerType;
8+
import com.ctre.phoenix6.configs.TalonFXConfigurator;
9+
import com.ctre.phoenix6.hardware.TalonFX;
10+
import com.ctre.phoenix6.signals.NeutralModeValue;
11+
12+
import frc.robot.constants.ElevatorConstants;
13+
14+
public class ElevatorIOFX implements ElevatorIO {
15+
private Logger logger;
16+
private TalonFX talonFxLeft;
17+
private TalonFX talonFxRight;
18+
19+
// FX Access objects
20+
TalonFXConfigurator configuratorLeft;
21+
TalonFXConfigurator configuratorRight;
22+
23+
public ExiterIOFX() {
24+
logger = LoggerFactory.getLogger(this.getClass());
25+
talonFxLeft = new TalonFX(ElevatorConstants.kFxIDMain);
26+
talonFxRight = new TalonFX(ElevatorConstants.kFxIDFollow);
27+
28+
29+
}
30+
31+
/*
32+
SetpointType.FOLLOWER -> {
33+
when (followerType) {
34+
FollowerType.STANDARD -> controlRequest =
35+
Follower(setpoint.toInt(), talonFxService.activeOpposeMain)
36+
37+
FollowerType.STRICT -> controlRequest = StrictFollower(setpoint.toInt())
38+
}
39+
}
40+
41+
SetpointType.NEUTRAL -> {
42+
when (talonFxService.activeNeutralOut) {
43+
NeutralModeValue.Coast -> controlRequest = CoastOut()
44+
NeutralModeValue.Brake -> controlRequest = StaticBrake()
45+
}
46+
}
47+
48+
SetpointType. -> {
49+
controlRequest = MusicTone(setpoint)
50+
}
51+
52+
}
53+
54+
//run Talon
55+
if(bus == "rio") {
56+
talonFxService.active.forEach {
57+
logger.info { "Control Request: ${controlRequest.name}: ${controlRequest.controlInfo}" }
58+
it.setControl(controlRequest)
59+
}
60+
} else if(bus == "canivore") {
61+
talonFxFDService.active.forEach {
62+
logger.info { "Control Request: ${controlRequest.name}: ${controlRequest.controlInfo}" }
63+
it.setControl(controlRequest)
64+
}
65+
} else throw IllegalArgumentException()
66+
*/
67+
68+
@Override
69+
public void updateInputs(ExiterIOInputs inputs) {
70+
inputs.velocityLeft = currLeftVelocity.refresh().getValue();
71+
inputs.velocityRight = currRightVelocity.refresh().getValue();
72+
}
73+
74+
@Override
75+
public void registerWith(TelemetryService telemetryService) {
76+
telemetryService.register(talonFxLeft, true);
77+
telemetryService.register(talonFxRight, true);
78+
}
79+
80+
}
81+
82+
//how to make subsystem extend closed-pos?
83+

‎src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java‎

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

3-
public class ElevatorSubsystem {}
3+
public class ElevatorSubsystem {
4+
// Private Variables
5+
private final ElevatorIO io;
6+
7+
// Constructor
8+
public void ExiterSubsystem(ElevatorIO io) {
9+
this.io = io;
10+
}
11+
}

0 commit comments

Comments
 (0)