Skip to content

Commit 7fec4bd

Browse files
AllenGong12AllenGong12
committed
Hopefully started/made all of the Wrist IO files correctly. I had to comment out a piece of code that caused an error in generated, take a gander at that.
1 parent 8612040 commit 7fec4bd

8 files changed

Lines changed: 294 additions & 14 deletions

File tree

‎FRC-2025-Team-4026-Offseason/src/main/java/frc/robot/generated/TunerConstants.java‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -195,11 +195,11 @@ public class TunerConstants {
195195
* Creates a CommandSwerveDrivetrain instance.
196196
* This should only be called once in your robot program,.
197197
*/
198-
public static Drive createDrivetrain() {
199-
return new Drive(
200-
DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight
201-
);
202-
}
198+
//public static Drive createDrivetrain() {
199+
// return new Drive(
200+
// DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight
201+
// );
202+
// }
203203

204204

205205
/**

‎FRC-2025-Team-4026-Offseason/src/main/java/frc/robot/subsystems/superstructure/wrist/Wrist.java‎

Lines changed: 74 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,77 @@
11
package frc.robot.subsystems.superstructure.wrist;
22

3-
public class Wrist {
3+
import org.littletonrobotics.junction.Logger;
4+
5+
import com.ctre.phoenix6.controls.NeutralOut;
6+
import com.ctre.phoenix6.controls.TorqueCurrentFOC;
7+
import com.ctre.phoenix6.controls.VoltageOut;
8+
import edu.wpi.first.wpilibj2.command.SubsystemBase;
9+
10+
11+
12+
public class Wrist extends SubsystemBase{
13+
private final String inputsName;
14+
private final WristIO io;
15+
private double volts = 0.0;
16+
protected final WristIOInputsAutoLogged inputs = new WristIOInputsAutoLogged();
17+
private final TorqueCurrentFOC TorqueCurrentOut = new TorqueCurrentFOC(WristConstants.PERPENDICULAR_CURRENT);
18+
private final VoltageOut VoltageOut = new VoltageOut(0.0);
19+
private final NeutralOut neutralOut = new NeutralOut();
20+
21+
22+
23+
private boolean brakeModeEnabled = true;
24+
public Wrist(String inputsName, WristIO io) {
25+
26+
this.inputsName = inputsName;
27+
this.io = io;
28+
29+
}
30+
public void periodic() {
31+
io.updateInputs(inputs);
32+
Logger.processInputs(inputsName, inputs);
33+
34+
35+
io.setVolts(volts);
36+
37+
38+
//LoggedTracer.record(name);
39+
// this doesn't work mayhbe important idk
40+
Logger.recordOutput(inputsName + "/BrakeModeEnabled", brakeModeEnabled);
41+
}
42+
43+
public void setBrakeMode(boolean enabled) {
44+
if (brakeModeEnabled == enabled) return;
45+
brakeModeEnabled = enabled;
46+
io.setBrakeMode(enabled);
47+
}
48+
49+
public double getTorqueCurrent() {
50+
return inputs.data.torqueCurrentAmps();
51+
}
52+
53+
public double getVolts() {
54+
return inputs.data.appliedVolts();
55+
}
56+
public double getPosition() {
57+
return inputs.data.positionRad();
58+
}
59+
60+
61+
public void setCurrent(double current, WristIOTalonFX wristMotor){
62+
wristMotor.wristMotor.setControl(TorqueCurrentOut.withOutput(current));
63+
}
64+
65+
public void setVolts(double volts, WristIOTalonFX wristMotor){
66+
wristMotor.wristMotor.setControl(VoltageOut.withOutput(volts));
67+
}
68+
69+
70+
public void stop(WristIOTalonFX wristMotor) {
71+
wristMotor.wristMotor.setControl(neutralOut);
72+
}
73+
74+
75+
476

5-
}
77+
}

‎FRC-2025-Team-4026-Offseason/src/main/java/frc/robot/subsystems/superstructure/wrist/WristConstants.java‎

Lines changed: 26 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,29 @@
11
package frc.robot.subsystems.superstructure.wrist;
22

3+
import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
4+
import com.ctre.phoenix6.configs.MotorOutputConfigs;
5+
import com.ctre.phoenix6.configs.TalonFXConfiguration;
6+
import com.ctre.phoenix6.signals.NeutralModeValue;
7+
38
public class WristConstants {
4-
5-
}
9+
10+
public static final double PARALLEL_CURRENT = -15;
11+
public static final double REDUCED_PARALLEL_CURRENT = -15;
12+
public static final double PERPENDICULAR_CURRENT = 15;
13+
public static final double REDUCED_PERPENDICULAR_CURRENT = 15;
14+
15+
public static final double MAX_SLAMMED_VELOCITY = 0.5;
16+
17+
public static final double SLAM_DEBOUNCE_TIME = 0.1;
18+
19+
public static final CurrentLimitsConfigs CURRENT_LIMITS_CONFIGS = new CurrentLimitsConfigs()
20+
.withStatorCurrentLimitEnable(true)
21+
.withStatorCurrentLimit(20);
22+
public static final MotorOutputConfigs MOTOR_OUTPUT_CONFIGS = new MotorOutputConfigs()
23+
.withNeutralMode(NeutralModeValue.Brake);
24+
25+
public static final TalonFXConfiguration MOTOR_CONFIG = new TalonFXConfiguration()
26+
.withCurrentLimits(CURRENT_LIMITS_CONFIGS)
27+
.withMotorOutput(MOTOR_OUTPUT_CONFIGS);
28+
29+
}

‎FRC-2025-Team-4026-Offseason/src/main/java/frc/robot/subsystems/superstructure/wrist/WristIO.java‎

Lines changed: 29 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,33 @@
11
package frc.robot.subsystems.superstructure.wrist;
22

3-
public class WristIO {
3+
import org.littletonrobotics.junction.AutoLog;
4+
5+
public interface WristIO {
6+
7+
@AutoLog
8+
class WristIOInputs {
9+
public WristIOData data = new WristIOData(0, 0, 0, 0, 0, false);
10+
}
11+
12+
record WristIOData(
13+
double positionRad,
14+
double velocityRadPerSec,
15+
double appliedVolts,
16+
double torqueCurrentAmps,
17+
double supplyCurrentAmps,
18+
boolean connected
19+
) {}
20+
21+
default void updateInputs(WristIOInputs inputs){};
22+
23+
default void setCurrent(double current){};
424

25+
default void setVolts(double volts){};
26+
27+
default void stop(){};
28+
29+
default void runPosition(double positionRad, double feedforwardVolts){};
30+
31+
default void setBrakeMode(boolean enabled){};
32+
533
}

‎FRC-2025-Team-4026-Offseason/src/main/java/frc/robot/subsystems/superstructure/wrist/WristIOSim.java‎

Lines changed: 47 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,50 @@
11
package frc.robot.subsystems.superstructure.wrist;
22

3-
public class WristIOSim {
4-
3+
import edu.wpi.first.math.MathUtil;
4+
import edu.wpi.first.math.system.plant.DCMotor;
5+
import edu.wpi.first.math.system.plant.LinearSystemId;
6+
import edu.wpi.first.wpilibj.DriverStation;
7+
import edu.wpi.first.wpilibj.simulation.DCMotorSim;
8+
9+
public class WristIOSim implements WristIO {
10+
private final DCMotorSim sim;
11+
private final DCMotor gearbox;
12+
private double appliedVoltage = 0.0;
13+
//fix this stuff
14+
public WristIOSim(DCMotor motorModel, double reduction, double moi) {
15+
16+
gearbox = motorModel;
17+
sim =
18+
new DCMotorSim(LinearSystemId.createDCMotorSystem(motorModel, moi, reduction), motorModel);
19+
}
20+
@Override
21+
public void updateInputs(WristIOInputs inputs) {
22+
if (DriverStation.isDisabled()) {
23+
setVolts(0.0);
24+
}
25+
26+
sim.update(0/*find a loopperiod constant number to put here eventually*/);
27+
inputs.data =
28+
new WristIOData(
29+
sim.getAngularPositionRad(),
30+
sim.getAngularVelocityRadPerSec(),
31+
appliedVoltage,
32+
gearbox.getCurrent(sim.getAngularVelocityRadPerSec(), appliedVoltage),
33+
sim.getCurrentDrawAmps(),
34+
false);
35+
}
36+
@Override
37+
public void setVolts(double volts) {
38+
appliedVoltage = MathUtil.clamp(volts, -12.0, 12.0);
39+
sim.setInputVoltage(appliedVoltage);
40+
}
41+
@Override
42+
public void setCurrent(double amps) {
43+
setVolts(gearbox.getVoltage(gearbox.getTorque(amps), sim.getAngularVelocityRadPerSec()));
44+
}
45+
46+
@Override
47+
public void stop() {
48+
setVolts(0.0);
49+
}
550
}

‎FRC-2025-Team-4026-Offseason/src/main/java/frc/robot/subsystems/superstructure/wrist/WristIOTalonFX.java‎

Lines changed: 73 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,76 @@
11
package frc.robot.subsystems.superstructure.wrist;
22

3-
public class WristIOTalonFX {
4-
3+
import static frc.robot.util.PhoenixUtil.tryUntilOk;
4+
5+
import com.ctre.phoenix6.BaseStatusSignal;
6+
import com.ctre.phoenix6.StatusSignal;
7+
import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC;
8+
import com.ctre.phoenix6.hardware.TalonFX;
9+
import edu.wpi.first.math.util.Units;
10+
import edu.wpi.first.units.measure.Angle;
11+
import edu.wpi.first.units.measure.AngularVelocity;
12+
import edu.wpi.first.units.measure.Current;
13+
import edu.wpi.first.units.measure.Voltage;
14+
15+
public class WristIOTalonFX implements WristIO{
16+
public TalonFX wristMotor;
17+
18+
19+
private StatusSignal<Angle> position;
20+
private StatusSignal<AngularVelocity> velocity;
21+
private StatusSignal<Voltage> appliedVolts;
22+
private StatusSignal<Current> supplyCurrent;
23+
private StatusSignal<Current> torqueCurrent;
24+
private final PositionTorqueCurrentFOC positionTorqueCurrentRequest =
25+
new PositionTorqueCurrentFOC(0.0).withUpdateFreqHz(0.0);
26+
27+
//add port later
28+
public void WristIoTalonFx(){
29+
30+
wristMotor = new TalonFX(0);
31+
32+
position = wristMotor.getPosition();
33+
velocity = wristMotor.getVelocity();
34+
appliedVolts = wristMotor.getMotorVoltage();
35+
supplyCurrent = wristMotor.getSupplyCurrent();
36+
torqueCurrent = wristMotor.getTorqueCurrent();
37+
38+
tryUntilOk(5,() -> BaseStatusSignal.setUpdateFrequencyForAll(40.0,
39+
position, velocity, supplyCurrent, torqueCurrent));
40+
//replace with port
41+
tryUntilOk(5, () -> wristMotor.optimizeBusUtilization());
42+
tryUntilOk(5, () -> wristMotor.getConfigurator().apply(WristConstants.MOTOR_CONFIG));
43+
}
44+
public void periodic(){
45+
if(wristMotor.hasResetOccurred()){
46+
wristMotor.optimizeBusUtilization();
47+
wristMotor.getPosition().setUpdateFrequency(40);
48+
}
49+
50+
51+
52+
}
53+
54+
@Override
55+
public void updateInputs(WristIOInputs inputs){
56+
inputs.data =
57+
new WristIOData(
58+
Units.rotationsToRadians(position.getValueAsDouble()), // with reduction?
59+
Units.rotationsToRadians(velocity.getValueAsDouble()),
60+
appliedVolts.getValueAsDouble(),
61+
torqueCurrent.getValueAsDouble(),
62+
supplyCurrent.getValueAsDouble(),
63+
64+
BaseStatusSignal.isAllGood(
65+
position, velocity, supplyCurrent, torqueCurrent));
66+
}
67+
@Override
68+
public void runPosition(double positionRad, double feedforward){
69+
wristMotor.setControl(
70+
positionTorqueCurrentRequest
71+
.withPosition(Units.radiansToRotations(positionRad))
72+
.withFeedForward(feedforward));
73+
74+
}
75+
576
}

‎FRC-2025-Team-4026-Offseason/src/main/java/frc/robot/util/PhoenixUtil.java‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -7,6 +7,7 @@
77

88
package frc.robot.util;
99

10+
import com.ctre.phoenix6.BaseStatusSignal;
1011
import com.ctre.phoenix6.StatusCode;
1112
import java.util.function.Supplier;
1213

@@ -18,4 +19,5 @@ public static void tryUntilOk(int maxAttempts, Supplier<StatusCode> command) {
1819
if (error.isOK()) break;
1920
}
2021
}
22+
2123
}

‎vendordeps/WPILibNewCommands.json‎

Lines changed: 38 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,38 @@
1+
{
2+
"fileName": "WPILibNewCommands.json",
3+
"name": "WPILib-New-Commands",
4+
"version": "1.0.0",
5+
"uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266",
6+
"frcYear": "2025",
7+
"mavenUrls": [],
8+
"jsonUrl": "",
9+
"javaDependencies": [
10+
{
11+
"groupId": "edu.wpi.first.wpilibNewCommands",
12+
"artifactId": "wpilibNewCommands-java",
13+
"version": "wpilib"
14+
}
15+
],
16+
"jniDependencies": [],
17+
"cppDependencies": [
18+
{
19+
"groupId": "edu.wpi.first.wpilibNewCommands",
20+
"artifactId": "wpilibNewCommands-cpp",
21+
"version": "wpilib",
22+
"libName": "wpilibNewCommands",
23+
"headerClassifier": "headers",
24+
"sourcesClassifier": "sources",
25+
"sharedLibrary": true,
26+
"skipInvalidPlatforms": true,
27+
"binaryPlatforms": [
28+
"linuxathena",
29+
"linuxarm32",
30+
"linuxarm64",
31+
"windowsx86-64",
32+
"windowsx86",
33+
"linuxx86-64",
34+
"osxuniversal"
35+
]
36+
}
37+
]
38+
}

0 commit comments

Comments
 (0)