Skip to content

Commit 74d24ac

Browse files
mwitcpalekmwitcpalek
authored
Merge pull request #5 from strykeforce/coral
Initial Coral Subsystem
2 parents 3216c96 + 52f009e commit 74d24ac

File tree

10 files changed

+493
-6
lines changed

10 files changed

+493
-6
lines changed

‎src/main/java/frc/robot/Robot.java‎

Lines changed: 29 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -6,7 +6,11 @@
66

77
import edu.wpi.first.wpilibj2.command.Command;
88
import edu.wpi.first.wpilibj2.command.CommandScheduler;
9+
import frc.robot.constants.BuildConstants;
910
import org.littletonrobotics.junction.LoggedRobot;
11+
import org.littletonrobotics.junction.Logger;
12+
import org.littletonrobotics.junction.networktables.NT4Publisher;
13+
import org.littletonrobotics.junction.wpilog.WPILOGWriter;
1014

1115
public class Robot extends LoggedRobot {
1216
private Command m_autonomousCommand;
@@ -17,6 +21,31 @@ public Robot() {
1721
m_robotContainer = new RobotContainer();
1822
}
1923

24+
@Override
25+
public void robotInit() {
26+
if (isReal()) {
27+
Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME);
28+
Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE);
29+
Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA);
30+
Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE);
31+
Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH);
32+
switch (BuildConstants.DIRTY) {
33+
case 0:
34+
Logger.recordMetadata("GitDirty", "All Changes Committed");
35+
break;
36+
case 1:
37+
Logger.recordMetadata("GitDirty", "Uncommitted changes");
38+
break;
39+
default:
40+
Logger.recordMetadata("GitDirty", "Unknown");
41+
break;
42+
}
43+
Logger.addDataReceiver(new WPILOGWriter("/home/lvuser/logs"));
44+
Logger.addDataReceiver(new NT4Publisher());
45+
}
46+
Logger.start();
47+
}
48+
2049
@Override
2150
public void robotPeriodic() {
2251
CommandScheduler.getInstance().run();

‎src/main/java/frc/robot/RobotContainer.java‎

Lines changed: 36 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -8,8 +8,14 @@
88
import edu.wpi.first.wpilibj.XboxController;
99
import edu.wpi.first.wpilibj2.command.Command;
1010
import edu.wpi.first.wpilibj2.command.Commands;
11+
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
12+
import frc.robot.commands.coral.EnableEjectBeamCommand;
13+
import frc.robot.commands.coral.OpenLoopCoralCommand;
1114
import frc.robot.commands.drive.DriveTeleopCommand;
1215
import frc.robot.controllers.FlyskyJoystick;
16+
import frc.robot.subsystems.coral.CoralIO;
17+
import frc.robot.subsystems.coral.CoralIOFX;
18+
import frc.robot.subsystems.coral.CoralSubsystem;
1319
import frc.robot.subsystems.drive.DriveSubsystem;
1420
import frc.robot.subsystems.drive.Swerve;
1521
import org.strykeforce.telemetry.TelemetryController;
@@ -21,25 +27,53 @@ public class RobotContainer {
2127

2228
private final XboxController xboxController = new XboxController(1);
2329
private final Joystick driveJoystick = new Joystick(0);
30+
2431
private final FlyskyJoystick flysky = new FlyskyJoystick(driveJoystick);
2532
private final TelemetryService telemetryService = new TelemetryService(TelemetryController::new);
2633

34+
private final CoralSubsystem coralSubsystem;
35+
private final CoralIO coralIO;
36+
2737
public RobotContainer() {
2838
swerve = new Swerve();
2939
driveSubsystem = new DriveSubsystem(swerve);
3040

31-
configureBindings();
41+
coralIO = new CoralIOFX();
42+
coralSubsystem = new CoralSubsystem(coralIO);
43+
44+
configureTelemetry();
3245
configureDriverBindings();
46+
configureOperatorBindings();
3347
}
3448

35-
private void configureBindings() {}
49+
private void configureTelemetry() {
50+
telemetryService.register(driveSubsystem);
51+
telemetryService.register(coralSubsystem);
52+
telemetryService.start();
53+
}
3654

3755
private void configureDriverBindings() {
3856
driveSubsystem.setDefaultCommand(
3957
new DriveTeleopCommand(
4058
() -> flysky.getFwd(), () -> flysky.getStr(), () -> flysky.getYaw(), driveSubsystem));
4159
}
4260

61+
private void configureOperatorBindings() {
62+
// Stop Coral
63+
new JoystickButton(xboxController, XboxController.Button.kB.value)
64+
.onTrue(new OpenLoopCoralCommand(coralSubsystem, 0));
65+
66+
// Intake Coral
67+
new JoystickButton(xboxController, XboxController.Button.kY.value)
68+
.onTrue(new OpenLoopCoralCommand(coralSubsystem, -0.5))
69+
.onTrue(new EnableEjectBeamCommand(false, coralSubsystem));
70+
71+
// Eject Coral
72+
new JoystickButton(xboxController, XboxController.Button.kA.value)
73+
.onTrue(new OpenLoopCoralCommand(coralSubsystem, -0.5))
74+
.onTrue(new EnableEjectBeamCommand(true, coralSubsystem));
75+
}
76+
4377
public Command getAutonomousCommand() {
4478
return Commands.print("No autonomous command configured");
4579
}

‎src/main/java/frc/robot/commands/coral/EnableEjectBeamCommand.java‎

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,19 @@
1+
package frc.robot.commands.coral;
2+
3+
import edu.wpi.first.wpilibj2.command.InstantCommand;
4+
import frc.robot.subsystems.coral.CoralSubsystem;
5+
6+
public class EnableEjectBeamCommand extends InstantCommand {
7+
private CoralSubsystem coralSubsystem;
8+
private Boolean enable;
9+
10+
public EnableEjectBeamCommand(Boolean enable, CoralSubsystem coralSubsystem) {
11+
this.coralSubsystem = coralSubsystem;
12+
this.enable = enable;
13+
}
14+
15+
@Override
16+
public void initialize() {
17+
coralSubsystem.enableEjectBeam(enable);
18+
}
19+
}

‎src/main/java/frc/robot/commands/coral/OpenLoopCoralCommand.java‎

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,19 @@
1+
package frc.robot.commands.coral;
2+
3+
import edu.wpi.first.wpilibj2.command.InstantCommand;
4+
import frc.robot.subsystems.coral.CoralSubsystem;
5+
6+
public class OpenLoopCoralCommand extends InstantCommand {
7+
private CoralSubsystem coralSubsystem;
8+
private double pct;
9+
10+
public OpenLoopCoralCommand(CoralSubsystem coralSubsystem, double pct) {
11+
this.coralSubsystem = coralSubsystem;
12+
this.pct = pct;
13+
}
14+
15+
@Override
16+
public void initialize() {
17+
coralSubsystem.setPct(pct);
18+
}
19+
}

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

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

3-
public class CoralConstants {}
3+
import static edu.wpi.first.units.Units.RotationsPerSecond;
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.TalonFXSConfiguration;
11+
import com.ctre.phoenix6.signals.ForwardLimitSourceValue;
12+
import com.ctre.phoenix6.signals.ForwardLimitTypeValue;
13+
import com.ctre.phoenix6.signals.GravityTypeValue;
14+
import com.ctre.phoenix6.signals.NeutralModeValue;
15+
import com.ctre.phoenix6.signals.ReverseLimitSourceValue;
16+
import com.ctre.phoenix6.signals.ReverseLimitTypeValue;
17+
import edu.wpi.first.units.measure.AngularVelocity;
18+
19+
public class CoralConstants {
20+
public static int kCoralFxId = 0;
21+
22+
public static final AngularVelocity kCloseEnough = RotationsPerSecond.of(0.1);
23+
24+
public static final AngularVelocity kIntakingSpeed = RotationsPerSecond.of(-1);
25+
public static final AngularVelocity kEjectingSpeed = RotationsPerSecond.of(1);
26+
27+
// Coral Talon FX Config
28+
public static TalonFXSConfiguration getFXConfig() {
29+
TalonFXSConfiguration fxsConfig = new TalonFXSConfiguration();
30+
31+
CurrentLimitsConfigs current =
32+
new CurrentLimitsConfigs()
33+
.withStatorCurrentLimit(10)
34+
.withStatorCurrentLimitEnable(false)
35+
.withSupplyCurrentLimit(10)
36+
.withSupplyCurrentLowerLimit(8)
37+
.withSupplyCurrentLowerTime(0.02)
38+
.withSupplyCurrentLimitEnable(true);
39+
fxsConfig.CurrentLimits = current;
40+
41+
HardwareLimitSwitchConfigs hwLimit =
42+
new HardwareLimitSwitchConfigs()
43+
.withForwardLimitAutosetPositionEnable(false)
44+
.withForwardLimitEnable(false)
45+
.withForwardLimitType(ForwardLimitTypeValue.NormallyOpen)
46+
.withForwardLimitSource(ForwardLimitSourceValue.LimitSwitchPin)
47+
.withReverseLimitAutosetPositionEnable(false)
48+
.withReverseLimitEnable(true)
49+
.withReverseLimitType(ReverseLimitTypeValue.NormallyOpen)
50+
.withReverseLimitSource(ReverseLimitSourceValue.LimitSwitchPin);
51+
fxsConfig.HardwareLimitSwitch = hwLimit;
52+
53+
Slot0Configs slot0 =
54+
new Slot0Configs()
55+
.withKP(0)
56+
.withKI(0)
57+
.withKD(0)
58+
.withGravityType(GravityTypeValue.Elevator_Static)
59+
.withKG(0)
60+
.withKS(0)
61+
.withKV(0)
62+
.withKA(0);
63+
fxsConfig.Slot0 = slot0;
64+
65+
MotionMagicConfigs motionMagic =
66+
new MotionMagicConfigs()
67+
.withMotionMagicAcceleration(0)
68+
.withMotionMagicCruiseVelocity(0)
69+
.withMotionMagicExpo_kA(0)
70+
.withMotionMagicExpo_kV(0)
71+
.withMotionMagicJerk(0);
72+
fxsConfig.MotionMagic = motionMagic;
73+
74+
MotorOutputConfigs motorOut =
75+
new MotorOutputConfigs()
76+
.withDutyCycleNeutralDeadband(0.01)
77+
.withNeutralMode(NeutralModeValue.Coast);
78+
fxsConfig.MotorOutput = motorOut;
79+
80+
return fxsConfig;
81+
}
82+
}

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

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,7 @@
11
package frc.robot.subsystems.algae;
22

3+
import com.ctre.phoenix6.signals.ReverseLimitValue;
4+
import edu.wpi.first.units.measure.Angle;
35
import edu.wpi.first.units.measure.AngularVelocity;
46
import org.littletonrobotics.junction.AutoLog;
57
import org.strykeforce.telemetry.TelemetryService;
@@ -8,6 +10,8 @@ public interface algaeIO {
810
@AutoLog
911
public static class IOInputs {
1012
public AngularVelocity velocity;
13+
public Angle location;
14+
public ReverseLimitValue reverseLimitSwitch;
1115
}
1216

1317
public default void updateInputs(IOInputs inputs) {}

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

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

3-
public class algaeIOFX {}
3+
import com.ctre.phoenix6.StatusSignal;
4+
import com.ctre.phoenix6.configs.TalonFXConfigurator;
5+
import com.ctre.phoenix6.controls.MotionMagicDutyCycle;
6+
import com.ctre.phoenix6.hardware.TalonFX;
7+
import com.ctre.phoenix6.signals.ReverseLimitValue;
8+
import edu.wpi.first.units.measure.Angle;
9+
import edu.wpi.first.units.measure.AngularVelocity;
10+
import frc.robot.constants.AlgaeConstants;
11+
import org.slf4j.Logger;
12+
import org.slf4j.LoggerFactory;
13+
import org.strykeforce.telemetry.TelemetryService;
14+
15+
/** Implementation of the algaeIO interface using TalonFX motors. */
16+
public class algaeIOFX implements algaeIO {
17+
private Logger logger;
18+
private final TalonFX talonFX;
19+
private StatusSignal<AngularVelocity> currVelocity;
20+
private StatusSignal<ReverseLimitValue> reverseLimitSwitch;
21+
private StatusSignal<Angle> currAngle;
22+
23+
// FX Access objects
24+
TalonFXConfigurator configurator;
25+
private MotionMagicDutyCycle positionRequest =
26+
new MotionMagicDutyCycle(0).withEnableFOC(false).withFeedForward(0).withSlot(0);
27+
28+
// Constructor to initialize the TalonFX motor controller
29+
public algaeIOFX() {
30+
logger = LoggerFactory.getLogger(this.getClass());
31+
talonFX = new TalonFX(AlgaeConstants.kFxId);
32+
reverseLimitSwitch = talonFX.getReverseLimit();
33+
currVelocity = talonFX.getVelocity();
34+
currAngle = talonFX.getPosition();
35+
}
36+
37+
@Override
38+
public void updateInputs(IOInputs inputs) {
39+
// inputs.reverseLimitSwitch = reverseLimitSwitch.refresh().getValue().value == 1;
40+
inputs.velocity = currVelocity.refresh().getValue();
41+
inputs.location = currAngle.refresh().getValue();
42+
}
43+
44+
@Override
45+
public void setPosition(double position) {
46+
talonFX.setPosition(position);
47+
}
48+
49+
public Angle getPosition() {
50+
return currAngle.getValue();
51+
}
52+
53+
@Override
54+
public void zero() {
55+
// not doing one for now
56+
}
57+
58+
@Override
59+
public void registerWith(TelemetryService telemetryService) {
60+
telemetryService.register(talonFX, true);
61+
}
62+
}

‎src/main/java/frc/robot/subsystems/coral/CoralIO.java‎

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

3-
public class CoralIO {}
3+
import static edu.wpi.first.units.Units.RotationsPerSecond;
4+
5+
import edu.wpi.first.units.measure.AngularVelocity;
6+
import org.littletonrobotics.junction.AutoLog;
7+
import org.strykeforce.telemetry.TelemetryService;
8+
9+
public interface CoralIO {
10+
11+
@AutoLog
12+
public static class CoralIOInputs {
13+
public AngularVelocity velocity = RotationsPerSecond.of(0.0);
14+
public boolean isFwdBeamBroken = false;
15+
public boolean isRevBeamBroken = false;
16+
}
17+
18+
public default void setVelocity(AngularVelocity velocity) {}
19+
20+
public default void setPct(double percentOutput) {}
21+
22+
public default void enableFwdLimitSwitch(boolean enabled) {}
23+
24+
public default void enableRevLimitSwitch(boolean enabled) {}
25+
26+
public default void updateInputs(CoralIOInputs inputs) {}
27+
28+
public default void registerWith(TelemetryService telemetryService) {}
29+
}

0 commit comments

Comments
 (0)