Skip to content

Commit 09cd8f4

Browse files
imroot07imroot07
committed
started climb io layers
1 parent 8669d82 commit 09cd8f4

File tree

6 files changed

+351
-2
lines changed

6 files changed

+351
-2
lines changed

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

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

3-
public class ClimbConstants {}
3+
import static edu.wpi.first.units.Units.Degrees;
4+
import static edu.wpi.first.units.Units.DegreesPerSecond;
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.NeutralModeValue;
17+
import com.ctre.phoenix6.signals.ReverseLimitSourceValue;
18+
import com.ctre.phoenix6.signals.ReverseLimitTypeValue;
19+
20+
import edu.wpi.first.units.measure.Angle;
21+
import edu.wpi.first.units.measure.AngularVelocity;
22+
23+
public class ClimbConstants {
24+
public static int kWheelFxId = 0;
25+
public static int kArmFxId = 0;
26+
27+
public static final AngularVelocity kWheelCloseEnough = DegreesPerSecond.of(5.0);
28+
public static final Angle kArmCloseEnough = Degrees.of(5.0);
29+
public static final Angle kArmMaxFwd = Degrees.of(100);
30+
public static final Angle kArmMaxRev = Degrees.of(-100);
31+
public static final Angle kArmZeroTicks = Degrees.of(1530);
32+
33+
public static TalonFXConfiguration getWheelFXConfig() {
34+
TalonFXConfiguration wheelFxConfig = new TalonFXConfiguration();
35+
36+
CurrentLimitsConfigs current =
37+
new CurrentLimitsConfigs()
38+
.withStatorCurrentLimit(10)
39+
.withStatorCurrentLimitEnable(false)
40+
.withStatorCurrentLimit(20)
41+
.withSupplyCurrentLimit(10)
42+
.withSupplyCurrentLowerLimit(8)
43+
.withSupplyCurrentLowerTime(0.02)
44+
.withSupplyCurrentLimitEnable(true);
45+
wheelFxConfig.CurrentLimits = current;
46+
47+
HardwareLimitSwitchConfigs hwLimit =
48+
new HardwareLimitSwitchConfigs()
49+
.withForwardLimitAutosetPositionEnable(false)
50+
.withForwardLimitEnable(false)
51+
.withForwardLimitType(ForwardLimitTypeValue.NormallyOpen)
52+
.withForwardLimitSource(ForwardLimitSourceValue.LimitSwitchPin)
53+
.withReverseLimitAutosetPositionEnable(false)
54+
.withReverseLimitEnable(false)
55+
.withReverseLimitType(ReverseLimitTypeValue.NormallyOpen)
56+
.withReverseLimitSource(ReverseLimitSourceValue.LimitSwitchPin);
57+
wheelFxConfig.HardwareLimitSwitch = hwLimit;
58+
59+
Slot0Configs slot0 =
60+
new Slot0Configs()
61+
.withKP(0)
62+
.withKI(0)
63+
.withKD(0)
64+
.withGravityType(GravityTypeValue.Elevator_Static)
65+
.withKG(0)
66+
.withKS(0)
67+
.withKV(0)
68+
.withKA(0);
69+
wheelFxConfig.Slot0 = slot0;
70+
71+
MotionMagicConfigs motionMagic =
72+
new MotionMagicConfigs()
73+
.withMotionMagicAcceleration(0)
74+
.withMotionMagicCruiseVelocity(0)
75+
.withMotionMagicExpo_kA(0)
76+
.withMotionMagicExpo_kV(0)
77+
.withMotionMagicJerk(0);
78+
wheelFxConfig.MotionMagic = motionMagic;
79+
80+
MotorOutputConfigs motorOut =
81+
new MotorOutputConfigs()
82+
.withDutyCycleNeutralDeadband(0.01)
83+
.withNeutralMode(NeutralModeValue.Coast);
84+
wheelFxConfig.MotorOutput = motorOut;
85+
86+
return wheelFxConfig;
87+
}
88+
89+
public static TalonFXConfiguration getArmFxConfig() {
90+
TalonFXConfiguration armFxConfig = new TalonFXConfiguration();
91+
92+
CurrentLimitsConfigs current =
93+
new CurrentLimitsConfigs()
94+
.withStatorCurrentLimit(10)
95+
.withStatorCurrentLimitEnable(false)
96+
.withStatorCurrentLimit(20)
97+
.withSupplyCurrentLimit(10)
98+
.withSupplyCurrentLowerLimit(8)
99+
.withSupplyCurrentLowerTime(0.02)
100+
.withSupplyCurrentLimitEnable(true);
101+
armFxConfig.CurrentLimits = current;
102+
103+
HardwareLimitSwitchConfigs hwLimit =
104+
new HardwareLimitSwitchConfigs()
105+
.withForwardLimitAutosetPositionEnable(false)
106+
.withForwardLimitEnable(false)
107+
.withForwardLimitType(ForwardLimitTypeValue.NormallyOpen)
108+
.withForwardLimitSource(ForwardLimitSourceValue.LimitSwitchPin)
109+
.withReverseLimitAutosetPositionEnable(false)
110+
.withReverseLimitEnable(false)
111+
.withReverseLimitType(ReverseLimitTypeValue.NormallyOpen)
112+
.withReverseLimitSource(ReverseLimitSourceValue.LimitSwitchPin);
113+
armFxConfig.HardwareLimitSwitch = hwLimit;
114+
115+
SoftwareLimitSwitchConfigs swLimit =
116+
new SoftwareLimitSwitchConfigs()
117+
.withForwardSoftLimitEnable(true)
118+
.withForwardSoftLimitThreshold(kArmMaxFwd)
119+
.withReverseSoftLimitEnable(true)
120+
.withReverseSoftLimitThreshold(kArmMaxRev);
121+
armFxConfig.SoftwareLimitSwitch = swLimit;
122+
123+
Slot0Configs slot0 =
124+
new Slot0Configs()
125+
.withKP(0)
126+
.withKI(0)
127+
.withKD(0)
128+
.withGravityType(GravityTypeValue.Elevator_Static)
129+
.withKG(0)
130+
.withKS(0)
131+
.withKV(0)
132+
.withKA(0);
133+
armFxConfig.Slot0 = slot0;
134+
135+
MotionMagicConfigs motionMagic =
136+
new MotionMagicConfigs()
137+
.withMotionMagicAcceleration(0)
138+
.withMotionMagicCruiseVelocity(0)
139+
.withMotionMagicExpo_kA(0)
140+
.withMotionMagicExpo_kV(0)
141+
.withMotionMagicJerk(0);
142+
armFxConfig.MotionMagic = motionMagic;
143+
144+
MotorOutputConfigs motorOut =
145+
new MotorOutputConfigs()
146+
.withDutyCycleNeutralDeadband(0.01)
147+
.withNeutralMode(NeutralModeValue.Coast);
148+
armFxConfig.MotorOutput = motorOut;
149+
150+
return armFxConfig;
151+
}
152+
}

‎src/main/java/frc/robot/subsystems/climb/ClimbArmIO.java‎

Lines changed: 23 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,23 @@
1+
package frc.robot.subsystems.climb;
2+
3+
import static edu.wpi.first.units.Units.Rotations;
4+
5+
import org.littletonrobotics.junction.AutoLog;
6+
import org.strykeforce.telemetry.TelemetryService;
7+
8+
import edu.wpi.first.units.measure.Angle;
9+
10+
public interface ClimbArmIO {
11+
12+
@AutoLog static class ClimbArmIOInputs {
13+
public Angle position = Rotations.of(0.0);
14+
}
15+
16+
public default void setPosition(Angle position) {}
17+
18+
public default void updateInputs(ClimbArmIOInputs inputs) {}
19+
20+
public default void zero() {}
21+
22+
public default void registerWith(TelemetryService telemetryService) {}
23+
}

‎src/main/java/frc/robot/subsystems/climb/ClimbArmIOFX.java‎

Lines changed: 66 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,66 @@
1+
package frc.robot.subsystems.climb;
2+
3+
import static edu.wpi.first.units.Units.Degrees;
4+
5+
import org.slf4j.Logger;
6+
import org.slf4j.LoggerFactory;
7+
8+
import com.ctre.phoenix6.StatusSignal;
9+
import com.ctre.phoenix6.configs.TalonFXConfiguration;
10+
import com.ctre.phoenix6.configs.TalonFXConfigurator;
11+
import com.ctre.phoenix6.controls.MotionMagicDutyCycle;
12+
import com.ctre.phoenix6.hardware.TalonFX;
13+
14+
import edu.wpi.first.units.measure.Angle;
15+
import frc.robot.constants.ClimbConstants;
16+
import frc.robot.constants.ExampleConstants;
17+
18+
public class ClimbArmIOFX implements ClimbArmIO {
19+
private Logger logger;
20+
private TalonFX talonFx;
21+
22+
private final Angle absSensorInitial;
23+
private Angle relSetpointOffset;
24+
private Angle setpoint;
25+
26+
TalonFXConfigurator configurator;
27+
private MotionMagicDutyCycle positionRequest = new MotionMagicDutyCycle(0).withEnableFOC(false).withFeedForward(0).withSlot(0);
28+
StatusSignal<Angle> currPosition;
29+
30+
public ClimbArmIOFX() {
31+
logger = LoggerFactory.getLogger(this.getClass());
32+
talonFx = new TalonFX(ClimbConstants.kArmFxId);
33+
absSensorInitial =
34+
talonFx.getPosition().getValue(); // relative encoder starts up as absolute position offset
35+
36+
configurator = talonFx.getConfigurator();
37+
configurator.apply(new TalonFXConfiguration()); // Factory default motor controller
38+
configurator.apply(ClimbConstants.getArmFxConfig());
39+
40+
currPosition = talonFx.getPosition();
41+
}
42+
43+
@Override
44+
public void setPosition(Angle position) {
45+
setpoint = position.plus(relSetpointOffset);
46+
47+
logger.info("Setting position to {} degrees", setpoint.in(Degrees));
48+
49+
talonFx.setControl(positionRequest.withPosition(setpoint));
50+
}
51+
52+
@Override
53+
public void updateInputs(ClimbArmIOInputs inputs) {
54+
inputs.position = currPosition.refresh().getValue();
55+
}
56+
57+
@Override
58+
public void zero() {
59+
relSetpointOffset = ExampleConstants.kZeroTicks;
60+
logger.info(
61+
"Abs: {}, Zero Pos: {}, Offset: {}",
62+
absSensorInitial,
63+
ExampleConstants.kZeroTicks,
64+
absSensorInitial.minus(ExampleConstants.kZeroTicks));
65+
}
66+
}

‎src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java‎

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

3-
public class ClimbSubsystem {}
3+
import static edu.wpi.first.units.Units.Rotations;
4+
5+
import org.strykeforce.telemetry.measurable.MeasurableSubsystem;
6+
7+
import edu.wpi.first.units.measure.Angle;
8+
import frc.robot.standards.ClosedLoopPosSubsystem;
9+
import frc.robot.subsystems.climb.ClimbArmIO.ClimbArmIOInputs;
10+
import frc.robot.subsystems.example.ExampleIOInputsAutoLogged;
11+
import frc.robot.subsystems.example.ExampleSubsystem.ExampleState;
12+
13+
public class ClimbSubsystem extends MeasurableSubsystem implements ClosedLoopPosSubsystem {
14+
private final ClimbWheelIO wheelIo;
15+
private final ClimbArmIO armIo;
16+
17+
private final ClimbWheelIOInputsAutoLogged wheelInputs = new ExampleIOInputsAutoLogged();
18+
private final ClimbArmIOInputsAutoLogged armInputs = new ExampleIOInputsAutoLogged();
19+
20+
private Angle setpoint = Rotations.of(0.0);
21+
private ClimbState curState = ClimbState.INIT;
22+
23+
public ClimbSubsystem(ClimbWheelIO wheelIo, ClimbArmIO armIo) {
24+
this.wheelIo = wheelIo;
25+
this.armIo=armIo;
26+
27+
zero();
28+
}
29+
30+
@Override
31+
public void zero() {
32+
armIo.zero();
33+
curState = ClimbState.ZEROED;
34+
}
35+
36+
public enum ClimbState {
37+
INIT,
38+
ZEROED
39+
}
40+
}

‎src/main/java/frc/robot/subsystems/climb/ClimbWheelIO.java‎

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

‎src/main/java/frc/robot/subsystems/climb/ClimbWheelIOFX.java‎

Lines changed: 53 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,53 @@
1+
package frc.robot.subsystems.climb;
2+
3+
import static edu.wpi.first.units.Units.DegreesPerSecond;
4+
5+
import org.slf4j.Logger;
6+
import org.slf4j.LoggerFactory;
7+
import org.strykeforce.telemetry.TelemetryService;
8+
9+
import com.ctre.phoenix6.StatusSignal;
10+
import com.ctre.phoenix6.configs.TalonFXConfiguration;
11+
import com.ctre.phoenix6.configs.TalonFXConfigurator;
12+
import com.ctre.phoenix6.controls.MotionMagicVelocityDutyCycle;
13+
import com.ctre.phoenix6.hardware.TalonFX;
14+
15+
import edu.wpi.first.units.measure.AngularVelocity;
16+
import frc.robot.constants.ClimbConstants;
17+
18+
public class ClimbWheelIOFX implements ClimbWheelIO {
19+
private Logger logger;
20+
private TalonFX talonFx;
21+
22+
TalonFXConfigurator configurator;
23+
private MotionMagicVelocityDutyCycle velocityRequest = new MotionMagicVelocityDutyCycle(0).withEnableFOC(false).withSlot(0);
24+
StatusSignal<AngularVelocity> curVelocity;
25+
26+
public ClimbWheelIOFX() {
27+
logger = LoggerFactory.getLogger(this.getClass());
28+
talonFx = new TalonFX(ClimbConstants.kWheelFxId);
29+
30+
configurator = talonFx.getConfigurator();
31+
configurator.apply(new TalonFXConfiguration()); // Factory default motor controller
32+
configurator.apply(ClimbConstants.getWheelFXConfig());
33+
34+
curVelocity = talonFx.getVelocity();
35+
}
36+
37+
@Override
38+
public void setVelocity(AngularVelocity velocity) {
39+
logger.info("Setting velocity to {} degrees per second", velocity.in(DegreesPerSecond));
40+
41+
talonFx.setControl(velocityRequest.withVelocity(velocity));
42+
}
43+
44+
@Override
45+
public void updateInputs(ClimbWheelIOInputs inputs) {
46+
inputs.velocity = curVelocity.refresh().getValue();
47+
}
48+
49+
@Override
50+
public void registerWith(TelemetryService telemetryService) {
51+
telemetryService.register(talonFx, true);
52+
}
53+
}

0 commit comments

Comments
 (0)