Skip to content

Commit 6e403ef

Browse files
charlesmackenziecharlesmackenzie
committed
Correctly interact with Pheonix6 API
1 parent 6e61694 commit 6e403ef

12 files changed

+89
-27
lines changed

Diff for: ‎simgui-ds.json

+8
Original file line numberDiff line numberDiff line change
@@ -88,5 +88,13 @@
8888
"buttonCount": 0,
8989
"povCount": 0
9090
}
91+
],
92+
"robotJoysticks": [
93+
{},
94+
{},
95+
{
96+
"guid": "030000006d0400001dc2000014400000",
97+
"useGamepad": true
98+
}
9199
]
92100
}

Diff for: ‎src/main/java/frc/robot/Constants.java

+1-1
Original file line numberDiff line numberDiff line change
@@ -29,7 +29,7 @@ public enum Mode {
2929
REPLAY,
3030
}
3131

32-
public static final Mode mode = Mode.SIM;
32+
public static final Mode mode = Mode.REAL;
3333

3434
public static final double loopTime = 0.02;
3535

Diff for: ‎src/main/java/frc/robot/RobotContainer.java

+7-7
Original file line numberDiff line numberDiff line change
@@ -241,13 +241,13 @@ public void enabledInit() {
241241

242242
ledManager.setOff(false);
243243

244-
// if (DriverStation.isTeleop()) {
245-
// arm.homeTelescope();
246-
// if (!RobotState.getInstance().hasIntaked()) {
247-
// arm.homeWrist();
248-
// }
249-
// // pivot.normalConstrain();
250-
// }
244+
if (DriverStation.isTeleop()) {
245+
arm.homeTelescope();
246+
// if (!RobotState.getInstance().hasIntaked()) {
247+
// arm.homeWrist();
248+
// }
249+
// pivot.normalConstrain();
250+
}
251251

252252
arm.setSetpoint(ArmPositions.stow);
253253
}

Diff for: ‎src/main/java/frc/robot/RobotState.java

+3
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,7 @@
11
package frc.robot;
22

3+
import org.littletonrobotics.junction.Logger;
4+
35
import edu.wpi.first.math.Matrix;
46
import edu.wpi.first.math.VecBuilder;
57
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
@@ -111,6 +113,7 @@ public GamePieceMode getMode() {
111113

112114
public void setMode(Constants.GamePieceMode mode) {
113115
gamePieceMode = mode;
116+
Logger.recordOutput("Arm/GamePieceMode", mode.name());
114117
}
115118

116119
public boolean hasIntaked() {

Diff for: ‎src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java

+4
Original file line numberDiff line numberDiff line change
@@ -86,6 +86,10 @@ public ArmSubsystem(PivotIO pivotIO, TelescopeIO telescopeIO, WristIO wristIO) {
8686
Units.degreesToRadians(630),
8787
Units.degreesToRadians(810)),
8888
() -> pivot.getPosition());
89+
90+
// telescope.setActive(false);
91+
wrist.setActive(false);
92+
pivot.setActive(false);
8993
}
9094

9195
@Override

Diff for: ‎src/main/java/frc/robot/subsystems/arm/pivot/PivotIOFalcon.java

+23-5
Original file line numberDiff line numberDiff line change
@@ -1,10 +1,11 @@
11
package frc.robot.subsystems.arm.pivot;
22

3-
import com.ctre.phoenix6.controls.CoastOut;
3+
import com.ctre.phoenix6.configs.MotorOutputConfigs;
4+
import com.ctre.phoenix6.configs.TalonFXConfigurator;
45
import com.ctre.phoenix6.controls.Follower;
5-
import com.ctre.phoenix6.controls.StaticBrake;
66
import com.ctre.phoenix6.controls.VoltageOut;
77
import com.ctre.phoenix6.hardware.TalonFX;
8+
import com.ctre.phoenix6.signals.NeutralModeValue;
89

910
import edu.wpi.first.wpilibj.DutyCycleEncoder;
1011
import frc.robot.Constants;
@@ -21,12 +22,16 @@ public class PivotIOFalcon implements PivotIO {
2122
private double lastPosition;
2223

2324
public PivotIOFalcon() {
25+
TalonFXConfigurator rightConfig = rightMotor.getConfigurator();
26+
rightConfig.apply(new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Brake));
27+
28+
TalonFXConfigurator leftConfig = leftMotor.getConfigurator();
29+
leftConfig.apply(new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Brake));
30+
2431
rightMotor.setInverted(false);
2532

2633
leftMotor.setControl(new Follower(CANDevices.rightPivotMotorID, true));
2734

28-
rightMotor.setControl(new StaticBrake());
29-
3035
//I have no idea how to set a current limit with the API
3136

3237
lastPosition = getPosition();
@@ -44,7 +49,20 @@ public void updateInputs(PivotIOInputsAutoLogged inputs) {
4449

4550
@Override
4651
public void setBrakeMode(boolean brake) {
47-
rightMotor.setControl(brake ? new StaticBrake() : new CoastOut());
52+
//TODO: find a more effient way to do this
53+
if (brake) {
54+
TalonFXConfigurator rightConfig = rightMotor.getConfigurator();
55+
rightConfig.apply(new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Brake));
56+
57+
TalonFXConfigurator leftConfig = leftMotor.getConfigurator();
58+
leftConfig.apply(new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Brake));
59+
} else {
60+
TalonFXConfigurator rightConfig = rightMotor.getConfigurator();
61+
rightConfig.apply(new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Coast));
62+
63+
TalonFXConfigurator leftConfig = leftMotor.getConfigurator();
64+
leftConfig.apply(new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Coast));
65+
}
4866
}
4967

5068
@Override

Diff for: ‎src/main/java/frc/robot/subsystems/arm/pivot/PivotIOSim.java

+2-2
Original file line numberDiff line numberDiff line change
@@ -12,9 +12,9 @@ public class PivotIOSim implements PivotIO {
1212
// TODO: simulate pink-arm dynamics more accurately
1313
private SingleJointedArmSim armSim = new SingleJointedArmSim(
1414
DCMotor.getFalcon500(2),
15-
PivotConstants.armToMotorGearRatio,
15+
1 / PivotConstants.armToMotorGearRatio,
1616
// TODO: weigh the arm, investigate calculating MOI manually
17-
SingleJointedArmSim.estimateMOI(PivotConstants.lengthWOTeleM, 0.5),
17+
SingleJointedArmSim.estimateMOI(PivotConstants.lengthWOTeleM, 2),
1818
PivotConstants.lengthWOTeleM,
1919
PivotConstants.maxFwdRotationRad,
2020
PivotConstants.maxBackRotationRad,

Diff for: ‎src/main/java/frc/robot/subsystems/arm/telescope/TelescopeIOFalcon.java

+5-2
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,7 @@
55
import com.ctre.phoenix6.controls.VoltageOut;
66
import com.ctre.phoenix6.hardware.TalonFX;
77

8+
import edu.wpi.first.math.MathUtil;
89
import frc.robot.Constants.CANDevices;
910
import frc.robot.Constants.TelescopeConstants;
1011

@@ -15,6 +16,8 @@ public class TelescopeIOFalcon implements TelescopeIO {
1516
public TelescopeIOFalcon() {
1617
motor.setInverted(false);
1718
motor.setControl(new StaticBrake());
19+
20+
motor.setPosition(0);
1821

1922
//I have no idea how to set a current limit from the API
2023
}
@@ -36,11 +39,11 @@ public void setBrakeMode(boolean brake) {
3639

3740
@Override
3841
public void setOutput(double volts) {
39-
motor.setControl(new VoltageOut(volts));
42+
motor.setControl(new VoltageOut(MathUtil.clamp(volts, -3, 3)));
4043
}
4144

4245
@Override
4346
public void setSensorPosition(double pos) {
44-
motor.setPosition(pos / (2 * Math.PI) / TelescopeConstants.conversionM);
47+
motor.setPosition(pos / ((2 * Math.PI) * TelescopeConstants.conversionM));
4548
}
4649
}

Diff for: ‎src/main/java/frc/robot/subsystems/arm/telescope/TelescopeIOSim.java

+3-1
Original file line numberDiff line numberDiff line change
@@ -19,7 +19,7 @@ public class TelescopeIOSim implements TelescopeIO {
1919
// TODO: find actual radius
2020
0.03,
2121
// TODO: find actual gear ratio
22-
0.125),
22+
8),
2323
DCMotor.getFalcon500(1),
2424
0.0,
2525
TelescopeConstants.maxPosM,
@@ -46,6 +46,8 @@ public void updateInputs(TelescopeIOInputsAutoLogged inputs) {
4646
public void setOutput(double volts) {
4747
if (DriverStation.isEnabled()) {
4848
appliedVolts = MathUtil.clamp(volts, -12, 12);
49+
} else {
50+
appliedVolts = 0;
4951
}
5052
}
5153
}

Diff for: ‎src/main/java/frc/robot/subsystems/arm/wrist/Wrist.java

+15-5
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,8 @@
88
import edu.wpi.first.math.controller.PIDController;
99
import edu.wpi.first.math.trajectory.TrapezoidProfile;
1010
import edu.wpi.first.wpilibj.Timer;
11+
import frc.robot.Constants;
12+
import frc.robot.Robot;
1113
import frc.robot.Constants.WristConstants;
1214
import frc.robot.subsystems.arm.GenericArmJoint;
1315

@@ -16,7 +18,10 @@ public class Wrist extends GenericArmJoint {
1618
private final WristIO io;
1719
private final WristIOInputsAutoLogged inputs = new WristIOInputsAutoLogged();
1820

19-
private final PIDController controller = new PIDController(WristConstants.kP, WristConstants.kI, WristConstants.kD);
21+
//FIXME: Refactor WristIO to remove this branch
22+
private final PIDController controller = Robot.isReal() ?
23+
new PIDController(WristConstants.kP, WristConstants.kI, WristConstants.kD)
24+
: new PIDController(1, 0, 0, Constants.loopTime);
2025
private final ArmFeedforward feedforward = new ArmFeedforward(
2126
WristConstants.kS,
2227
WristConstants.kG,
@@ -130,10 +135,15 @@ private void resetOffset() {
130135
protected double calculateControl(TrapezoidProfile.State setpoint) {
131136
double adjustedPosition = getPosition() + pivotAngleSupplier.getAsDouble();
132137

133-
// TODO: Investigate the merits of having separate PID constants for when the
134-
// wrist isn't moving
135-
return controller.calculate(adjustedPosition, setpoint.position)
136-
+ feedforward.calculate(setpoint.position, setpoint.velocity);
138+
//FIXME: Refactor WristIO to remove this branch
139+
if (Robot.isReal()) {
140+
// TODO: Investigate the merits of having separate PID constants for when the
141+
// wrist isn't moving
142+
return controller.calculate(adjustedPosition, setpoint.position)
143+
+ feedforward.calculate(setpoint.position, setpoint.velocity);
144+
} else {
145+
return controller.calculate(adjustedPosition, setpoint.position);
146+
}
137147
}
138148

139149
@Override

Diff for: ‎src/main/java/frc/robot/subsystems/arm/wrist/WristIOFalcon.java

+12-1
Original file line numberDiff line numberDiff line change
@@ -1,9 +1,12 @@
11
package frc.robot.subsystems.arm.wrist;
22

3+
import com.ctre.phoenix6.configs.MotorOutputConfigs;
4+
import com.ctre.phoenix6.configs.TalonFXConfigurator;
35
import com.ctre.phoenix6.controls.CoastOut;
46
import com.ctre.phoenix6.controls.StaticBrake;
57
import com.ctre.phoenix6.controls.VoltageOut;
68
import com.ctre.phoenix6.hardware.TalonFX;
9+
import com.ctre.phoenix6.signals.NeutralModeValue;
710

811
import frc.robot.Constants.CANDevices;
912
import frc.robot.Constants.WristConstants;
@@ -17,6 +20,8 @@ public WristIOFalcon() {
1720

1821
motor.setControl(new StaticBrake());
1922

23+
TalonFXConfigurator config = motor.getConfigurator();
24+
config.apply(new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Brake));
2025
//I have no idea how to set a current limit from the API
2126
}
2227

@@ -30,7 +35,13 @@ public void updateInputs(WristIOInputsAutoLogged inputs) {
3035

3136
@Override
3237
public void setBrakeMode(boolean brake) {
33-
motor.setControl(brake ? new StaticBrake() : new CoastOut());
38+
if (brake) {
39+
TalonFXConfigurator rightConfig = motor.getConfigurator();
40+
rightConfig.apply(new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Brake));
41+
} else {
42+
TalonFXConfigurator rightConfig = motor.getConfigurator();
43+
rightConfig.apply(new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Coast));
44+
}
3445
}
3546

3647
@Override

Diff for: ‎src/main/java/frc/robot/subsystems/arm/wrist/WristIOSim.java

+6-3
Original file line numberDiff line numberDiff line change
@@ -11,13 +11,14 @@
1111

1212
public class WristIOSim implements WristIO {
1313

14+
// TODO: simulate pink-arm dynamics more accurately
1415
private SingleJointedArmSim armSim = new SingleJointedArmSim(
1516
LinearSystemId.createSingleJointedArmSystem(
1617
DCMotor.getFalcon500(1),
17-
SingleJointedArmSim.estimateMOI(WristConstants.intakeLengthM, 1),
18-
WristConstants.gearRatio),
18+
SingleJointedArmSim.estimateMOI(WristConstants.intakeLengthM, 2),
19+
1 / WristConstants.gearRatio),
1920
DCMotor.getFalcon500(1),
20-
WristConstants.gearRatio,
21+
1 / WristConstants.gearRatio,
2122
WristConstants.intakeLengthM,
2223
WristConstants.negativeLimitRad,
2324
WristConstants.positiveLimitRad,
@@ -44,6 +45,8 @@ public void updateInputs(WristIOInputsAutoLogged inputs) {
4445
public void setOutput(double volts) {
4546
if (DriverStation.isEnabled()) {
4647
appliedVolts = MathUtil.clamp(volts, -12, 12);
48+
} else {
49+
appliedVolts = 0.0;
4750
}
4851
}
4952
}

0 commit comments

Comments
 (0)