Skip to content

Commit 5f7705f

Browse files
Superbro525AltSuperbro525Alt
committed
ready to test
1 parent ce41925 commit 5f7705f

7 files changed

Lines changed: 136 additions & 94 deletions

File tree

‎src/main/java/org/curtinfrc/frc2026/Constants.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -10,7 +10,7 @@ public final class Constants {
1010
public static final int middleMagRollerMotorID = 20;
1111
public static final int indexerMagRollerMotorID = 15;
1212

13-
public static final RobotType robotType = RobotType.SIM;
13+
public static final RobotType robotType = RobotType.DEV;
1414
public static final double ROBOT_X = 0.85;
1515
public static final double ROBOT_Y = 0.85;
1616
public static final double FIELD_LENGTH = 16.540988;

‎src/main/java/org/curtinfrc/frc2026/Robot.java‎

Lines changed: 5 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -174,7 +174,7 @@ public Robot() {
174174
drive::getRotation,
175175
new VisionIOPhotonVision(
176176
cameraConfigs[0].name(), cameraConfigs[0].robotToCamera()));
177-
hoodedShooter = new HoodedShooter(new HoodIO() {}, new ShooterIO() {});
177+
hoodedShooter = new HoodedShooter(new ShooterIO() {}, new HoodIO() {}, drive::getPose);
178178
}
179179
case DEV -> {
180180
drive =
@@ -199,7 +199,7 @@ public Robot() {
199199
Constants.middleMagRollerMotorID, InvertedValue.Clockwise_Positive),
200200
new MagRollerIODev(
201201
Constants.indexerMagRollerMotorID, InvertedValue.Clockwise_Positive));
202-
hoodedShooter = new HoodedShooter(new HoodIODev(), new ShooterIODev());
202+
hoodedShooter = new HoodedShooter(new ShooterIODev(), new HoodIODev(), drive::getPose);
203203
}
204204
case SIM -> {
205205
drive =
@@ -218,7 +218,7 @@ public Robot() {
218218
cameraConfigs[0].name(), cameraConfigs[0].robotToCamera(), drive::getPose));
219219
mag = new Mag(new MagRollerIO() {}, new MagRollerIO() {}, new MagRollerIO() {});
220220
intake = new Intake(new IntakeIOSim());
221-
hoodedShooter = new HoodedShooter(new HoodIOSim(), new ShooterIOSim());
221+
hoodedShooter = new HoodedShooter(new ShooterIOSim(), new HoodIOSim(), drive::getPose);
222222
}
223223
}
224224
} else {
@@ -231,7 +231,7 @@ public Robot() {
231231
new ModuleIO() {});
232232
vision = new Vision(drive::addVisionMeasurement, drive::getRotation, new VisionIO() {});
233233
mag = new Mag(new MagRollerIO() {}, new MagRollerIO() {}, new MagRollerIO() {});
234-
hoodedShooter = new HoodedShooter(new HoodIO() {}, new ShooterIO() {});
234+
hoodedShooter = new HoodedShooter(new ShooterIO() {}, new HoodIO() {}, drive::getPose);
235235
}
236236

237237
drive.setPose(
@@ -255,8 +255,7 @@ public Robot() {
255255
}
256256

257257
hoodedShooter.setDefaultCommand(
258-
hoodedShooter.setHoodedShooterPositionAndVelocity(
259-
shotAngle.get() / 360, shotSpeed.get(), () -> controller.a().getAsBoolean()));
258+
hoodedShooter.setHoodedShooterPositionAndVelocity(shotAngle.get() / 360, shotSpeed.get()));
260259

261260
// controller
262261
// .leftTrigger()

‎src/main/java/org/curtinfrc/frc2026/subsystems/hoodedshooter/HoodIO.java‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,8 @@ public static class HoodIOInputs {
99
public boolean motorConnected;
1010
public double motorTemperature;
1111
public double positionRotations;
12-
public double absolutePositionRotations;
12+
public double hoodPositionDegrees;
13+
public double encoderPositionRotations;
1314
public double angularVelocityRotationsPerSecond;
1415
public double currentAmps;
1516
public double appliedVolts;
@@ -21,5 +22,5 @@ public default void setVoltage(double voltage) {}
2122

2223
public default void setVoltageV(Voltage voltage) {}
2324

24-
public default void setPosition(double position) {}
25+
public default void setPosition(double positionRotations) {}
2526
}

‎src/main/java/org/curtinfrc/frc2026/subsystems/hoodedshooter/HoodIODev.java‎

Lines changed: 37 additions & 24 deletions
Original file line numberDiff line numberDiff line change
@@ -34,21 +34,26 @@ public class HoodIODev implements HoodIO {
3434
public static final int MOTOR_ID = 17;
3535
public static final int ENCODER_ID = 21;
3636

37-
public static final double GEAR_RATIO = 2.67;
38-
public static final double ENCODER_MAGNET_OFFSET = -0.0585;
39-
public static final double FORWARD_LIMIT_ROTATIONS = 1.5;
40-
public static final double REVERSE_LIMIT_ROTATIONS = 0;
37+
public static final double GEAR_RATIO = 8.2; // 12:32 * 10:82
38+
public static final double MOTOR_TO_SENSOR_RATIO = 2.66666667;
39+
public static final double FORWARD_LIMIT_ROTATIONS = 100;
40+
public static final double REVERSE_LIMIT_ROTATIONS = -100;
41+
public static final double LIMIT_BUFFER_ROTATIONS = 0.1;
4142
public static final double STOWED_OUT_POSITION_THRESHOLD = 0.4;
43+
public static final double ENCODER_MAGNET_OFFSET = -0.051025;
44+
public static final double ZERO_DEGREE_OFFSET_DEGREES = 53;
4245

43-
public static final double GRAVITY_POSITION_OFFSET = -0.08686111111;
44-
public static final double KP = 25.0;
46+
public static final double GRAVITY_POSITION_OFFSET = -0.0869;
47+
public static final double KP_STOWED = 155.6;
48+
public static final double KP_OUT = 155.6;
4549
public static final double KI = 0.0;
46-
public static final double KD = 0.25;
47-
public static final double KS_STOWED = 0.60;
48-
public static final double KS_OUT = 0.20;
49-
public static final double KV = 0.15; // temp
50-
public static final double KA = 0.0;
51-
public static final double KG = 0.80;
50+
public static final double KD = 5.05;
51+
52+
public static final double KS_STOWED = 0.6;
53+
public static final double KS_OUT = 0.3;
54+
public static final double KV = 4.81;
55+
public static final double KA = 0.03;
56+
public static final double KG = 0.14;
5257

5358
public static final double MM_CRUISE_VELOCITY = 2700;
5459
public static final double MM_ACCLERATION = 16;
@@ -62,8 +67,10 @@ public class HoodIODev implements HoodIO {
6267
.withInverted(InvertedValue.CounterClockwise_Positive))
6368
.withFeedback(
6469
new FeedbackConfigs()
65-
.withFeedbackRemoteSensorID(ENCODER_ID)
66-
.withFeedbackSensorSource(FeedbackSensorSourceValue.RotorSensor)
70+
.withFeedbackRemoteSensorID(ENCODER_ID) // Ties encoder with motor
71+
.withFeedbackSensorSource(
72+
FeedbackSensorSourceValue.FusedCANcoder) // Ties encoder with motor
73+
.withRotorToSensorRatio(MOTOR_TO_SENSOR_RATIO)
6774
.withSensorToMechanismRatio(GEAR_RATIO))
6875
.withCurrentLimits(
6976
new CurrentLimitsConfigs().withSupplyCurrentLimit(30).withStatorCurrentLimit(60))
@@ -75,7 +82,7 @@ public class HoodIODev implements HoodIO {
7582
.withReverseSoftLimitEnable(true))
7683
.withSlot0(
7784
new Slot0Configs()
78-
.withKP(KP)
85+
.withKP(KP_STOWED)
7986
.withKI(KI)
8087
.withKD(KD)
8188
.withKS(KS_STOWED)
@@ -86,7 +93,7 @@ public class HoodIODev implements HoodIO {
8693
.withGravityType(GravityTypeValue.Arm_Cosine))
8794
.withSlot1(
8895
new Slot1Configs()
89-
.withKP(KP)
96+
.withKP(KP_OUT)
9097
.withKI(KI)
9198
.withKD(KD)
9299
.withKS(KS_OUT)
@@ -113,7 +120,7 @@ public class HoodIODev implements HoodIO {
113120
private final StatusSignal<AngularVelocity> velocity = motor.getVelocity();
114121
private final StatusSignal<Current> current = motor.getStatorCurrent();
115122
private final StatusSignal<Voltage> voltage = motor.getMotorVoltage();
116-
private final StatusSignal<Angle> absolutePosition = encoder.getAbsolutePosition();
123+
private final StatusSignal<Angle> encoderPosition = encoder.getPosition();
117124
private final StatusSignal<Temperature> temperature = motor.getDeviceTemp();
118125

119126
private final VoltageOut voltageRequest =
@@ -126,12 +133,16 @@ public HoodIODev() {
126133
tryUntilOk(5, () -> encoder.getConfigurator().apply(encoderConfig));
127134

128135
BaseStatusSignal.setUpdateFrequencyForAll(
129-
50.0, velocity, voltage, current, position, absolutePosition);
136+
50.0, velocity, voltage, current, position, encoderPosition);
130137
motor.optimizeBusUtilization();
131-
PhoenixUtil.registerSignals(false, velocity, voltage, current, position, absolutePosition);
132-
133-
PhoenixUtil.refreshAll();
134-
tryUntilOk(5, () -> motor.setPosition(absolutePosition.getValueAsDouble()));
138+
PhoenixUtil.registerSignals(false, velocity, voltage, current, position, encoderPosition);
139+
140+
// PhoenixUtil.refreshAll();
141+
// tryUntilOk(
142+
// 5,
143+
// () ->
144+
// motor.setPosition(
145+
// absolutePosition.getValueAsDouble() / GEAR_RATIO + ZERO_DEGREE_OFFSET));
135146
}
136147

137148
@Override
@@ -146,7 +157,9 @@ public void updateInputs(HoodIOInputs inputs) {
146157
inputs.appliedVolts = voltage.getValueAsDouble();
147158
inputs.currentAmps = current.getValueAsDouble();
148159
inputs.positionRotations = position.getValueAsDouble();
149-
inputs.absolutePositionRotations = absolutePosition.getValueAsDouble();
160+
inputs.hoodPositionDegrees =
161+
(encoderPosition.getValueAsDouble() * 360 / GEAR_RATIO) + ZERO_DEGREE_OFFSET_DEGREES;
162+
inputs.encoderPositionRotations = encoderPosition.getValueAsDouble();
150163
inputs.angularVelocityRotationsPerSecond = velocity.getValueAsDouble();
151164
}
152165

@@ -162,7 +175,7 @@ public void setVoltageV(Voltage voltage) {
162175

163176
@Override
164177
public void setPosition(double position) {
165-
var request = positionRequest.withPosition(position);
178+
var request = positionRequest.withPosition(position - (ZERO_DEGREE_OFFSET_DEGREES / 360));
166179
if (this.position.getValueAsDouble() > STOWED_OUT_POSITION_THRESHOLD) {
167180
request.withSlot(1);
168181
} else {

‎src/main/java/org/curtinfrc/frc2026/subsystems/hoodedshooter/HoodedShooter.java‎

Lines changed: 75 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,10 @@
66
import static edu.wpi.first.units.Units.RotationsPerSecond;
77
import static edu.wpi.first.units.Units.Volts;
88

9+
import edu.wpi.first.math.geometry.Pose2d;
10+
import edu.wpi.first.math.geometry.Rotation3d;
11+
import edu.wpi.first.math.geometry.Transform3d;
12+
import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
913
import edu.wpi.first.units.measure.MutAngle;
1014
import edu.wpi.first.units.measure.MutAngularVelocity;
1115
import edu.wpi.first.units.measure.MutVoltage;
@@ -14,19 +18,33 @@
1418
import edu.wpi.first.wpilibj2.command.Command;
1519
import edu.wpi.first.wpilibj2.command.SubsystemBase;
1620
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
17-
import java.util.function.BooleanSupplier;
1821
import java.util.function.Supplier;
22+
// import org.curtinfrc.frc2026.sim.BallSim;
23+
// import org.curtinfrc.frc2026.util.FieldConstants;
1924
import org.littletonrobotics.junction.Logger;
2025

2126
public class HoodedShooter extends SubsystemBase {
27+
// public static final Translation2d HUB_LOCATION =
28+
// new Translation2d(
29+
// FieldConstants.Hub.topCenterPoint.getX(), FieldConstants.Hub.topCenterPoint.getY());
2230
public static final double WHEEL_DIAMETER = 0.101;
31+
public static final Transform3d SHOOTER_TRANSFORM =
32+
new Transform3d(0, 0, 1, new Rotation3d()); // Not confirmed
33+
34+
public static final InterpolatingDoubleTreeMap DISTANCE_TO_SHOOTER_VELOCITY =
35+
new InterpolatingDoubleTreeMap();
36+
37+
public static final InterpolatingDoubleTreeMap DISTANCE_TO_HOOD_ANGLE =
38+
new InterpolatingDoubleTreeMap();
2339

2440
private final HoodIO hoodIO;
2541
private final HoodIOInputsAutoLogged hoodInputs = new HoodIOInputsAutoLogged();
2642

2743
private final ShooterIO shooterIO;
2844
private final ShooterIOInputsAutoLogged shooterInputs = new ShooterIOInputsAutoLogged();
2945

46+
private final Supplier<Pose2d> robotPose;
47+
3048
private final Alert hoodMotorDisconnectedAlert;
3149
private final Alert hoodMotorTempAlert;
3250
private final Alert[] shooterMotorDisconnectedAlerts = new Alert[3];
@@ -40,16 +58,31 @@ public class HoodedShooter extends SubsystemBase {
4058
private final SysIdRoutine sysIdRoutineHood;
4159

4260
private boolean hoodSoftLimitedForward() {
43-
return hoodInputs.positionRotations > HoodIODev.FORWARD_LIMIT_ROTATIONS - 0.1;
61+
return hoodInputs.positionRotations
62+
> HoodIODev.FORWARD_LIMIT_ROTATIONS - HoodIODev.LIMIT_BUFFER_ROTATIONS;
4463
}
4564

4665
private boolean hoodSoftLimitedReverse() {
47-
return hoodInputs.positionRotations < HoodIODev.REVERSE_LIMIT_ROTATIONS + 0.1;
66+
return hoodInputs.positionRotations
67+
< HoodIODev.REVERSE_LIMIT_ROTATIONS + HoodIODev.LIMIT_BUFFER_ROTATIONS;
4868
}
4969

50-
public HoodedShooter(HoodIO hoodIO, ShooterIO shooterIO) {
51-
this.hoodIO = hoodIO;
70+
// public BallSim ballSim = new BallSim(0.0, new Rotation2d(0.0), new Pose3d());
71+
72+
public HoodedShooter(ShooterIO shooterIO, HoodIO hoodIO, Supplier<Pose2d> robotPose) {
5273
this.shooterIO = shooterIO;
74+
this.hoodIO = hoodIO;
75+
this.robotPose = robotPose;
76+
77+
DISTANCE_TO_SHOOTER_VELOCITY.put(3.05, 0.0);
78+
DISTANCE_TO_SHOOTER_VELOCITY.put(2.035, 0.0);
79+
DISTANCE_TO_SHOOTER_VELOCITY.put(5.474, 0.0);
80+
DISTANCE_TO_SHOOTER_VELOCITY.put(4.0, 0.0);
81+
82+
DISTANCE_TO_HOOD_ANGLE.put(3.05, 0.0);
83+
DISTANCE_TO_HOOD_ANGLE.put(2.035, 0.0);
84+
DISTANCE_TO_HOOD_ANGLE.put(5.474, 0.0);
85+
DISTANCE_TO_HOOD_ANGLE.put(4.0, 0.0);
5386

5487
this.hoodMotorDisconnectedAlert = new Alert("Hood motor disconnected.", AlertType.kError);
5588
this.hoodMotorTempAlert =
@@ -106,8 +139,8 @@ public void periodic() {
106139
shooterIO.updateInputs(shooterInputs);
107140
Logger.processInputs("Hood", hoodInputs);
108141
Logger.processInputs("Shooter", shooterInputs);
142+
// Logger.recordOutput("Ball", ballSim.update(0.02));
109143

110-
// Update alerts
111144
hoodMotorDisconnectedAlert.set(!hoodInputs.motorConnected);
112145
hoodMotorTempAlert.set(hoodInputs.motorTemperature > 60); // in celcius
113146
for (int motor = 0; motor < 3; motor++) {
@@ -116,16 +149,40 @@ public void periodic() {
116149
}
117150
}
118151

119-
public Command setHoodPosition(double position) {
120-
return run(() -> hoodIO.setPosition(position));
152+
// public Command shootAtHub() { // this assumes that the robot is facing the target
153+
// return run(
154+
// () -> {
155+
// double distanceLength = HUB_LOCATION.minus(robotPose.get().getTranslation()).getNorm();
156+
157+
// double hoodAngle = DISTANCE_TO_HOOD_ANGLE.get(distanceLength);
158+
// hoodAngle = (90 / 360); // angle conversion to rotations
159+
// double shooterVelocity = DISTANCE_TO_SHOOTER_VELOCITY.get(3.04833887);
160+
// hoodIO.setPosition(hoodAngle);
161+
// // shooterIO.setVelocity(20);
162+
163+
// ballSim =
164+
// new BallSim(
165+
// shooterVelocity,
166+
// Rotation2d.fromDegrees(89),
167+
// new Pose3d(robotPose.get())
168+
// .transformBy(new Transform3d(0.2, 0.0, 0.4, Rotation3d.kZero)));
169+
// });
170+
// }
171+
172+
public Command setHoodPosition(double positionDegrees) {
173+
return run(
174+
() -> {
175+
hoodIO.setPosition(positionDegrees / 360);
176+
Logger.recordOutput("ROBOERT SMILE", positionDegrees);
177+
});
121178
}
122179

123-
public Command stopHood() {
124-
return run(() -> hoodIO.setVoltage(0));
180+
public Command setHoodVoltage(double voltage) {
181+
return run(() -> hoodIO.setVoltage(voltage));
125182
}
126183

127-
public Command setHoodVoltage(Supplier<Double> voltage) {
128-
return run(() -> hoodIO.setVoltage(voltage.get()));
184+
public Command stopHood() {
185+
return run(() -> hoodIO.setVoltage(0));
129186
}
130187

131188
public Command setShooterVoltage(double voltage) {
@@ -136,24 +193,24 @@ public Command stopShooter() {
136193
return run(() -> shooterIO.setVoltage(0));
137194
}
138195

139-
public Command setShooterVelocity(double velocity, BooleanSupplier useFeedforward) {
140-
return run(() -> shooterIO.setVelocity(velocity, useFeedforward));
196+
public Command setShooterVelocity(double velocityMetresPerSecond) {
197+
return run(() -> shooterIO.setVelocity(velocityMetresPerSecond));
141198
}
142199

143200
public Command setHoodedShooterPositionAndVelocity(
144-
double position, double velocity, BooleanSupplier useFeedforward) {
201+
double positionDegrees, double velocityMetresPerSecond) {
145202
return run(
146203
() -> {
147-
hoodIO.setPosition(position);
148-
shooterIO.setVelocity(velocity, useFeedforward);
204+
hoodIO.setPosition(positionDegrees);
205+
shooterIO.setVelocity(velocityMetresPerSecond);
149206
});
150207
}
151208

152209
public Command stopHoodedShooter() {
153210
return run(
154211
() -> {
155212
hoodIO.setVoltage(0);
156-
shooterIO.setVoltage(0);
213+
shooterIO.setVelocity(0);
157214
});
158215
}
159216

‎src/main/java/org/curtinfrc/frc2026/subsystems/hoodedshooter/ShooterIO.java‎

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,6 @@
11
package org.curtinfrc.frc2026.subsystems.hoodedshooter;
22

33
import edu.wpi.first.units.measure.Voltage;
4-
import java.util.function.BooleanSupplier;
54
import org.littletonrobotics.junction.AutoLog;
65

76
public interface ShooterIO {
@@ -22,5 +21,5 @@ public default void setVoltage(double voltage) {}
2221

2322
public default void setVoltageV(Voltage voltage) {}
2423

25-
public default void setVelocity(double velocity, BooleanSupplier f) {}
24+
public default void setVelocity(double velocity) {}
2625
}

0 commit comments

Comments
 (0)