Skip to content
This repository was archived by the owner on May 19, 2024. It is now read-only.

Commit c47d04d

Browse files
haydenherouxhaydenheroux
committed
feat: Use intake goal.
1 parent c2e7eca commit c47d04d

10 files changed

+92
-101
lines changed

Diff for: ‎simgui.json

+2-1
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@
1919
"/SmartDashboard/Arm Mechanism": "Mechanism2d",
2020
"/SmartDashboard/Mechanism": "Mechanism2d",
2121
"/SmartDashboard/SendableChooser[0]": "String Chooser",
22+
"/SmartDashboard/Superstructure": "Mechanism2d",
2223
"/SmartDashboard/VisionSystemSim-visionSystem/Sim Field": "Field2d",
2324
"/SmartDashboard/arm mech": "Mechanism2d"
2425
},
@@ -33,7 +34,7 @@
3334
"visible": true
3435
}
3536
},
36-
"/SmartDashboard/Mechanism": {
37+
"/SmartDashboard/Superstructure": {
3738
"window": {
3839
"visible": true
3940
}

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

-2
Original file line numberDiff line numberDiff line change
@@ -82,9 +82,7 @@ private void configureBindings() {
8282
operatorController.rightTrigger().onTrue(superstructure.shoot());
8383

8484
operatorController.a().onTrue(superstructure.amp());
85-
operatorController.b().onTrue(superstructure.intake());
8685
operatorController.x().onTrue(superstructure.stow());
87-
operatorController.y().onTrue(superstructure.shoot());
8886
}
8987

9088
/**

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

+1-1
Original file line numberDiff line numberDiff line change
@@ -76,7 +76,7 @@ public void setShoulderPosition(double positionRotations) {
7676
shoulderMotor.setPosition(positionRotations);
7777
}
7878

79-
public void setShoulderSetpoint(double positionRotations, double velocityRotationsPerSecond) {
79+
public void setSetpoint(double positionRotations, double velocityRotationsPerSecond) {
8080
shoulderMotor.setSetpoint(positionRotations, velocityRotationsPerSecond);
8181
}
8282
}

Diff for: ‎src/main/java/frc/robot/intake/Intake.java

+2-23
Original file line numberDiff line numberDiff line change
@@ -2,11 +2,8 @@
22

33
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
44
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
5-
import edu.wpi.first.wpilibj2.command.Command;
6-
import edu.wpi.first.wpilibj2.command.Commands;
75
import frc.lib.Subsystem;
86
import frc.lib.Telemetry;
9-
import frc.robot.intake.IntakeConstants.RollerMotorConstants;
107
import frc.robot.intake.RollerMotorIO.RollerMotorIOValues;
118

129
/** Subsystem class for the intake subsystem. */
@@ -60,25 +57,7 @@ public double getRollerVelocity() {
6057
return rollerMotorValues.velocityRotationsPerSecond;
6158
}
6259

63-
/**
64-
* Returns a command that intakes using the rollers.
65-
*
66-
* @return a command that intakes using the rollers.
67-
*/
68-
public Command intake() {
69-
return Commands.runEnd(
70-
() -> rollerMotor.setSetpoint(RollerMotorConstants.INTAKE_VELOCITY),
71-
() -> rollerMotor.setSetpoint(0.0));
72-
}
73-
74-
/**
75-
* Returns a command that outtakes using the rollers.
76-
*
77-
* @return a command that outtakes using the rollers.
78-
*/
79-
public Command outtake() {
80-
return Commands.runEnd(
81-
() -> rollerMotor.setSetpoint(RollerMotorConstants.OUTTAKE_VELOCITY),
82-
() -> rollerMotor.setSetpoint(0.0));
60+
public void setSetpoint(double rollerVelocityRotationsPerSecond) {
61+
rollerMotor.setSetpoint(rollerVelocityRotationsPerSecond);
8362
}
8463
}

Diff for: ‎src/main/java/frc/robot/intake/IntakeConstants.java

+1-1
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,7 @@
55
/** Constants for the intake subsystem. */
66
public class IntakeConstants {
77
/** Constants for the roller motor. */
8-
public static class RollerMotorConstants {
8+
public static class RollerConstants {
99
/** Velocity to apply when intaking in rotations per second. */
1010
public static final double INTAKE_VELOCITY = 1.0;
1111

Diff for: ‎src/main/java/frc/robot/shooter/Shooter.java

+4-35
Original file line numberDiff line numberDiff line change
@@ -2,14 +2,10 @@
22

33
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
44
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
5-
import edu.wpi.first.wpilibj2.command.Command;
6-
import edu.wpi.first.wpilibj2.command.Commands;
75
import frc.lib.Subsystem;
86
import frc.lib.Telemetry;
97
import frc.robot.shooter.FlywheelMotorIO.FlywheelMotorIOValues;
108
import frc.robot.shooter.SerializerMotorIO.SerializerMotorIOValues;
11-
import frc.robot.shooter.ShooterConstants.FlywheelConstants;
12-
import frc.robot.shooter.ShooterConstants.SerializerConstants;
139

1410
/** Subsystem class for the shooter subsystem. */
1511
public class Shooter extends Subsystem {
@@ -81,36 +77,9 @@ public double getSerializerVelocity() {
8177
return serializerMotorValues.velocityRotationsPerSecond;
8278
}
8379

84-
/**
85-
* Intakes a note.
86-
*
87-
* @return a command that intakes a note.
88-
*/
89-
public Command intake() {
90-
return Commands.runEnd(
91-
() -> serializerMotor.setSetpoint(SerializerConstants.INTAKE_VELOCITY),
92-
() -> serializerMotor.setSetpoint(0.0));
93-
}
94-
95-
/**
96-
* Serializes a note.
97-
*
98-
* @return a command that serializes a note.
99-
*/
100-
public Command serialize() {
101-
return Commands.runEnd(
102-
() -> serializerMotor.setSetpoint(SerializerConstants.SERIALIZE_VELOCITY),
103-
() -> serializerMotor.setSetpoint(0.0));
104-
}
105-
106-
/**
107-
* Spins the flywheel.
108-
*
109-
* @return a command that spins the flywheel.
110-
*/
111-
public Command spin() {
112-
return Commands.runEnd(
113-
() -> flywheelMotor.setSetpoint(FlywheelConstants.SHOOT_VELOCITY),
114-
() -> flywheelMotor.setSetpoint(0.0));
80+
public void setSetpoint(
81+
double flywheelVelocityRotationsPerSecond, double serializerVelocityRotationsPerSecond) {
82+
flywheelMotor.setSetpoint(flywheelVelocityRotationsPerSecond);
83+
serializerMotor.setSetpoint(serializerVelocityRotationsPerSecond);
11584
}
11685
}

Diff for: ‎src/main/java/frc/robot/shooter/ShooterConstants.java

+14-8
Original file line numberDiff line numberDiff line change
@@ -8,19 +8,25 @@ public static class SerializerConstants {
88
/** Velocity to apply while intaking in rotations per second. */
99
public static final double INTAKE_VELOCITY = 4.75;
1010

11-
/** Velocity to apply while serializing in rotations per second. */
12-
public static final double SERIALIZE_VELOCITY = 4.75;
11+
/** Velocity to apply while serializing for a speaker shot in rotations per second. */
12+
public static final double SPEAKER_SERIALIZER_VELOCITY = 4.75;
1313

14-
/** Maximum tengential speed in meters per second. */
15-
public static final double MAXIMUM_TANGENTIAL_SPEED = 4.75;
14+
/** Velocity to apply while serializing for an amp shot in rotations per second. */
15+
public static final double AMP_SERIALIZE_VELOCITY = 2.5;
16+
17+
/** Maximum tengential speed in rotations per second. */
18+
public static final double MAXIMUM_SPEED = 4.75;
1619
}
1720

1821
/** Constants for the flywheel motor used in the shooter subsystem. */
1922
public static class FlywheelConstants {
20-
/** Velocity to apply while shooting in rotations per second. */
21-
public static final double SHOOT_VELOCITY = 5.65;
23+
/** Velocity to apply while shooting into the speaker in rotations per second. */
24+
public static final double SPEAKER_VELOCITY = 5.65;
25+
26+
/** Velocity to apply while shooting into the amp in rotations per second. */
27+
public static final double AMP_VELOCITY = 2.5;
2228

23-
/** Maximum tangential speed in meters per second. */
24-
public static final double MAXIMUM_TANGENTIAL_SPEED = 5.65;
29+
/** Maximum tangential speed in rotations per second. */
30+
public static final double MAXIMUM_SPEED = 5.65;
2531
}
2632
}

Diff for: ‎src/main/java/frc/robot/superstructure/Superstructure.java

+29-15
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,10 @@
99
import frc.lib.Telemetry;
1010
import frc.robot.arm.Arm;
1111
import frc.robot.intake.Intake;
12+
import frc.robot.intake.IntakeConstants.RollerConstants;
1213
import frc.robot.shooter.Shooter;
14+
import frc.robot.shooter.ShooterConstants.FlywheelConstants;
15+
import frc.robot.shooter.ShooterConstants.SerializerConstants;
1316

1417
/** Subsystem class for the superstructure subsystem. */
1518
public class Superstructure extends Subsystem {
@@ -141,8 +144,14 @@ public SuperstructureState getState() {
141144
private void updateSetpoint() {
142145
setpoint = SuperstructureState.nextSetpoint(setpoint, goal);
143146

144-
arm.setShoulderSetpoint(
147+
arm.setSetpoint(
145148
setpoint.shoulderAngleRotations().position, setpoint.shoulderAngleRotations().velocity);
149+
150+
shooter.setSetpoint(
151+
setpoint.flywheelVelocityRotationsPerSecond(),
152+
setpoint.serializerVelocityRotationsPerSecond());
153+
154+
intake.setSetpoint(setpoint.rollerVelocityRotationsPerSecond());
146155
}
147156

148157
public void setPosition(SuperstructureState state) {
@@ -151,41 +160,46 @@ public void setPosition(SuperstructureState state) {
151160

152161
public void setGoal(SuperstructureState goal) {
153162
this.goal = goal;
154-
155-
// resetMotionProfile();
156163
}
157164

158-
// private void resetMotionProfile() {
159-
// setpoint = measurement;
160-
// }
161-
162165
public boolean at(SuperstructureState goal) {
163166
updateMeasurement();
164167

165168
return measurement.at(goal);
166169
}
167170

168-
public Command to(SuperstructureState goal) {
171+
private Command to(SuperstructureState goal) {
169172
return new ToGoal(goal);
170173
}
171174

172-
public Command initial() {
173-
return to(SuperstructureState.INITIAL);
174-
}
175-
176175
public Command stow() {
177176
return to(SuperstructureState.STOW);
178177
}
179178

180179
public Command intake() {
181-
return to(SuperstructureState.INTAKE);
180+
return to(SuperstructureState.INTAKE)
181+
.andThen(
182+
to(
183+
SuperstructureState.INTAKE
184+
.withSerializerVelocity(SerializerConstants.INTAKE_VELOCITY)
185+
.withRollerVelocity(RollerConstants.INTAKE_VELOCITY)));
182186
}
183187

184188
public Command shoot() {
185-
return to(SuperstructureState.SHOOT);
189+
return to(SuperstructureState.SHOOT)
190+
.andThen(
191+
to(SuperstructureState.SHOOT.withFlywheelVelocity(FlywheelConstants.SPEAKER_VELOCITY))
192+
.withTimeout(1.0))
193+
.andThen(
194+
to(
195+
SuperstructureState.SHOOT
196+
.withFlywheelVelocity(FlywheelConstants.SPEAKER_VELOCITY)
197+
.withSerializerVelocity(SerializerConstants.SPEAKER_SERIALIZER_VELOCITY)));
186198
}
187199

188200
public Command amp() {
189-
return to(SuperstructureState.AMP);
201+
return to(SuperstructureState.AMP)
202+
.andThen(to(SuperstructureState.AMP_SHOOT).withTimeout(1.0))
203+
.andThen(to(SuperstructureState.AMP_SERIALIZE));
190204
}
191205
}

Diff for: ‎src/main/java/frc/robot/superstructure/SuperstructureMechanism.java

+31-15
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,7 @@
1111
import frc.lib.InterpolatableColor;
1212
import frc.robot.RobotConstants;
1313
import frc.robot.arm.ArmConstants.ShoulderMotorConstants;
14+
import frc.robot.intake.IntakeConstants.RollerConstants;
1415
import frc.robot.shooter.ShooterConstants.FlywheelConstants;
1516
import frc.robot.shooter.ShooterConstants.SerializerConstants;
1617

@@ -21,7 +22,7 @@ public class SuperstructureMechanism {
2122

2223
private final Mechanism2d mechanism;
2324

24-
private MechanismLigament2d shoulder, flywheel, serializer;
25+
private MechanismLigament2d shoulder, flywheel, serializer, rollers;
2526

2627
private final double WIDTH =
2728
2
@@ -46,22 +47,23 @@ public class SuperstructureMechanism {
4647

4748
private final double SERIALIZER_LENGTH = Units.inchesToMeters(7.5);
4849

50+
private final Translation2d ORIGIN_TO_ROLLERS =
51+
new Translation2d(Units.inchesToMeters(5.75), Units.inchesToMeters(2.5));
52+
4953
private final Color8Bit DEFAULT_COLOR = new Color8Bit(Color.kLightGray);
5054

51-
private final InterpolatableColor flywheelColor =
55+
private final InterpolatableColor FLYWHEEL_COLOR =
5256
new InterpolatableColor(Color.kLightGray, Color.kSalmon);
53-
private final InterpolatableColor serializerColor =
57+
58+
private final InterpolatableColor SERIALIZER_COLOR =
5459
new InterpolatableColor(Color.kLightGray, Color.kCornflowerBlue);
5560

61+
private final InterpolatableColor ROLLERS_COLOR =
62+
new InterpolatableColor(Color.kLightGray, Color.kSpringGreen);
63+
5664
private SuperstructureMechanism() {
5765
mechanism = new Mechanism2d(WIDTH, HEIGHT);
5866

59-
initializeArmMechanism();
60-
61-
initializeFramePerimeterMechanisms();
62-
}
63-
64-
private void initializeArmMechanism() {
6567
Translation2d armRootTranslation = ORIGIN.plus(ORIGIN_TO_SHOULDER_BASE);
6668

6769
double armThickness = Units.inchesToMeters(2) * 100;
@@ -91,9 +93,16 @@ private void initializeArmMechanism() {
9193
shoulder.append(
9294
new MechanismLigament2d(
9395
"serializer", SERIALIZER_LENGTH, -35, armThickness, DEFAULT_COLOR));
94-
}
9596

96-
private void initializeFramePerimeterMechanisms() {
97+
Translation2d rollersRootTranslation = ORIGIN.plus(ORIGIN_TO_ROLLERS);
98+
99+
rollers =
100+
mechanism
101+
.getRoot("intake", rollersRootTranslation.getX(), rollersRootTranslation.getY())
102+
.append(
103+
new MechanismLigament2d(
104+
"rollers", Units.inchesToMeters(12), 0, armThickness, DEFAULT_COLOR));
105+
97106
double framePerimeterThickness = Units.inchesToMeters(1) * 10;
98107

99108
mechanism
@@ -136,16 +145,23 @@ public void updateSuperstructure(SuperstructureState state) {
136145

137146
flywheel.setColor(
138147
new Color8Bit(
139-
flywheelColor.sample(
148+
FLYWHEEL_COLOR.sample(
140149
Math.abs(state.flywheelVelocityRotationsPerSecond()),
141150
0,
142-
FlywheelConstants.MAXIMUM_TANGENTIAL_SPEED)));
151+
FlywheelConstants.MAXIMUM_SPEED)));
143152

144153
serializer.setColor(
145154
new Color8Bit(
146-
serializerColor.sample(
155+
SERIALIZER_COLOR.sample(
147156
Math.abs(state.serializerVelocityRotationsPerSecond()),
148157
0,
149-
SerializerConstants.MAXIMUM_TANGENTIAL_SPEED)));
158+
SerializerConstants.MAXIMUM_SPEED)));
159+
160+
rollers.setColor(
161+
new Color8Bit(
162+
ROLLERS_COLOR.sample(
163+
Math.abs(state.rollerVelocityRotationsPerSecond()),
164+
0,
165+
RollerConstants.MAXIMUM_SPEED)));
150166
}
151167
}

Diff for: ‎src/main/java/frc/robot/superstructure/SuperstructureState.java

+8
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,8 @@
44
import edu.wpi.first.math.geometry.Rotation2d;
55
import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
66
import frc.robot.RobotConstants;
7+
import frc.robot.shooter.ShooterConstants.FlywheelConstants;
8+
import frc.robot.shooter.ShooterConstants.SerializerConstants;
79
import frc.robot.superstructure.SuperstructureConstants.ShoulderAngleConstants;
810
import java.util.Objects;
911

@@ -28,6 +30,12 @@ public record SuperstructureState(
2830

2931
public static final SuperstructureState AMP = new SuperstructureState(ShoulderAngleConstants.AMP);
3032

33+
public static final SuperstructureState AMP_SHOOT =
34+
AMP.withFlywheelVelocity(FlywheelConstants.AMP_VELOCITY);
35+
36+
public static final SuperstructureState AMP_SERIALIZE =
37+
AMP_SHOOT.withSerializerVelocity(SerializerConstants.AMP_SERIALIZE_VELOCITY);
38+
3139
/**
3240
* Creates a new superstructure state.
3341
*

0 commit comments

Comments
 (0)