Skip to content

Commit 326cd57

Browse files
EpicPigGuyEpicPigGuy
committed
begin algae system
1 parent e173c04 commit 326cd57

File tree

4 files changed

+176
-2
lines changed

4 files changed

+176
-2
lines changed

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

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

3-
public class AlgaeConstants {}
3+
import static edu.wpi.first.units.Units.Degrees;
4+
import static edu.wpi.first.units.Units.Rotations;
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+
import edu.wpi.first.units.measure.*;
20+
public class AlgaeConstants {
21+
22+
23+
public static int kFxId = 4; //CHANGE TO MOTOR NUMBER
24+
25+
public static final Angle kCloseEnough = Degrees.of(5);
26+
public static final Angle kMaxFwd = Rotations.of(100);
27+
public static final Angle kMaxRev = Rotations.of(-100);
28+
public static final Angle kZeroTicks = Rotations.of(1530);
29+
30+
// Example Talon FX Config
31+
public static TalonFXConfiguration getFXConfig() {
32+
TalonFXConfiguration fxConfig = new TalonFXConfiguration();
33+
34+
CurrentLimitsConfigs current =
35+
new CurrentLimitsConfigs()
36+
.withStatorCurrentLimit(10)
37+
.withStatorCurrentLimitEnable(false)
38+
.withStatorCurrentLimit(20)
39+
.withSupplyCurrentLimit(10)
40+
.withSupplyCurrentLowerLimit(8)
41+
.withSupplyCurrentLowerTime(0.02)
42+
.withSupplyCurrentLimitEnable(true);
43+
fxConfig.CurrentLimits = current;
44+
45+
HardwareLimitSwitchConfigs hwLimit =
46+
new HardwareLimitSwitchConfigs()
47+
.withForwardLimitAutosetPositionEnable(false)
48+
.withForwardLimitEnable(false)
49+
.withForwardLimitType(ForwardLimitTypeValue.NormallyOpen)
50+
.withForwardLimitSource(ForwardLimitSourceValue.LimitSwitchPin)
51+
.withReverseLimitAutosetPositionEnable(false)
52+
.withReverseLimitEnable(false)
53+
.withReverseLimitType(ReverseLimitTypeValue.NormallyOpen)
54+
.withReverseLimitSource(ReverseLimitSourceValue.LimitSwitchPin);
55+
fxConfig.HardwareLimitSwitch = hwLimit;
56+
57+
SoftwareLimitSwitchConfigs swLimit =
58+
new SoftwareLimitSwitchConfigs()
59+
.withForwardSoftLimitEnable(true)
60+
.withForwardSoftLimitThreshold(kMaxFwd)
61+
.withReverseSoftLimitEnable(true)
62+
.withReverseSoftLimitThreshold(kMaxRev);
63+
fxConfig.SoftwareLimitSwitch = swLimit;
64+
65+
Slot0Configs slot0 =
66+
new Slot0Configs()
67+
.withKP(0)
68+
.withKI(0)
69+
.withKD(0)
70+
.withGravityType(GravityTypeValue.Elevator_Static)
71+
.withKG(0)
72+
.withKS(0)
73+
.withKV(0)
74+
.withKA(0);
75+
fxConfig.Slot0 = slot0;
76+
77+
MotionMagicConfigs motionMagic =
78+
new MotionMagicConfigs()
79+
.withMotionMagicAcceleration(0)
80+
.withMotionMagicCruiseVelocity(0)
81+
.withMotionMagicExpo_kA(0)
82+
.withMotionMagicExpo_kV(0)
83+
.withMotionMagicJerk(0);
84+
fxConfig.MotionMagic = motionMagic;
85+
86+
MotorOutputConfigs motorOut =
87+
new MotorOutputConfigs()
88+
.withDutyCycleNeutralDeadband(0.01)
89+
.withNeutralMode(NeutralModeValue.Coast);
90+
fxConfig.MotorOutput = motorOut;
91+
92+
return fxConfig;
93+
}
94+
}

‎src/main/java/frc/robot/subsystems/algae/AlgaeSubsystem.java‎

Lines changed: 13 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,15 @@
11
package frc.robot.subsystems.algae;
2+
package frc.robot.standards;
3+
import edu.wpi.first.units.measure.AngularVelocity;
24

3-
public class AlgaeSubsystem {}
5+
6+
public class AlgaeSubsystem {
7+
8+
9+
10+
public void setSpeed(AngularVelocity speed){};
11+
12+
public AngularVelocity getSpeed(){};
13+
14+
public boolean atSpeed(){};
15+
}

‎src/main/java/frc/robot/subsystems/algae/algaeIO.java‎

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

‎src/main/java/frc/robot/subsystems/algae/algaeIOFX.java‎

Lines changed: 50 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,50 @@
1+
package frc.robot.subsystems.algae;
2+
3+
import org.slf4j.Logger;
4+
import org.slf4j.LoggerFactory;
5+
import org.strykeforce.IOInputsAutoLogged;
6+
7+
import com.ctre.phoenix.motorcontrol.ControlMode;
8+
import com.ctre.phoenix6.StatusSignal;
9+
import com.ctre.phoenix6.configs.TalonFXConfigurator;
10+
import com.ctre.phoenix6.controls.MotionMagicDutyCycle;
11+
import com.ctre.phoenix6.hardware.TalonFX;
12+
import com.ctre.phoenix6.signals.ReverseLimitValue;
13+
14+
import edu.wpi.first.units.measure.AngularVelocity;
15+
import frc.robot.constants.AlgaeConstants;
16+
17+
public class algaeIOFX implements algaeIO {
18+
private Logger logger;
19+
private final TalonFX talonFX;
20+
private IOInputs inputs;
21+
22+
// FX Access objects
23+
TalonFXConfigurator configurator;
24+
private MotionMagicDutyCycle positionRequest =
25+
new MotionMagicDutyCycle(0).withEnableFOC(false).withFeedForward(0).withSlot(0);
26+
StatusSignal<AngularVelocity> currVelocity;
27+
StatusSignal<ReverseLimitValue> reverseLimitSwitch;
28+
29+
public algaeIOFX() {
30+
final IOInputsAutoLogged inputs = new IOInputsAutoLogged();
31+
logger = LoggerFactory.getLogger(this.getClass());
32+
talonFX = new TalonFX(AlgaeConstants.kFxId);
33+
reverseLimitSwitch = talonFX.getReverseLimit();
34+
currVelocity = talonFX.getVelocity();
35+
reverseLimitSwitch = talonFX.getReverseLimit();
36+
}
37+
38+
public void updateInputs(IOInputs inputs) {
39+
inputs.velocity = currVelocity.refresh().getValue();
40+
}
41+
public void setPosition(double position) {
42+
// Set the Talon FX to position control mode and set the target position
43+
talonFX.setPosition(position);
44+
}
45+
46+
public void zero() {
47+
}
48+
49+
public void registerWith(TelemetryService telemetryService) {}
50+
}

0 commit comments

Comments
 (0)