Skip to content

Commit d99ffe1

Browse files
dracco1993tricktrap
dracco1993
and
tricktrap
authored
Midnight day 3, going to master (#13)
* Add reset gyro to teleop too * Make LL conditional * Basic SparkFlex intake for Sunday night show * YAMS-ify it, baby! * Add Hopper subsystem * Update the turret to use a Nova * Working hopper with operator controls * Ops * Add intake arm mechanism. Note that we still need to test this on real hardware and verify setpoints and limits. * "Big cook" * Kicker controls --------- Co-authored-by: Patrick Frampton <pframpton@salesforce.com>
1 parent 1783bc2 commit d99ffe1

File tree

11 files changed

+516
-92
lines changed

11 files changed

+516
-92
lines changed

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

Lines changed: 21 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -63,20 +63,36 @@ public static class CoralConstants {
6363
}
6464

6565
public static class ShooterConstants {
66-
// 2 Neos, 4in shooter wheels, 4:1 gearbox reduction
66+
// 2 Neos, 4in shooter wheels
6767
public static final int kLeaderMotorId = 15;
6868
public static final int kFollowerMotorId = 16;
6969
}
7070

7171
public static class TurretConstants {
72-
// 2 Neos, 12in diameter, 25:1 gearbox, 10:1 pivot gearing, non-continuous (270
73-
// FOV)
74-
public static final int kLeaderMotorId = 17;
75-
public static final int kFollowerMotorId = 18;
72+
// 1 Neo, 6.875 in diameter, 4:1 gearbox, 10:1 pivot gearing, non-continuous
73+
// 360 deg
74+
public static final int kMotorId = 17;
7675
}
7776

7877
public static class HoodConstants {
7978
// 1 Neo, 0-90 degree variability, 50:1 reduction
8079
public static final int kMotorId = 19;
8180
}
81+
82+
// Intake subsystem CAN IDs start at 30
83+
public static class IntakeConstants {
84+
// SparkFlex controlling the intake flywheel
85+
public static final int kPivotMotorId = 30;
86+
public static final int kRollerMotorId = 31;
87+
}
88+
89+
// Hopper subsystem CAN IDs start at 40
90+
public static class HopperConstants {
91+
public static final int kHopperMotorId = 40;
92+
}
93+
94+
// Kicker subsystem CAN IDs start at 50
95+
public static class KickerConstants {
96+
public static final int kKickerMotorId = 50;
97+
}
8298
}

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

Lines changed: 14 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -17,12 +17,24 @@
1717
import frc.robot.controls.DriverControls;
1818
import frc.robot.controls.OperatorControls;
1919
import frc.robot.controls.PoseControls;
20+
import frc.robot.subsystems.HopperSubsystem;
21+
import frc.robot.subsystems.IntakeSubsystem;
22+
import frc.robot.subsystems.KickerSubsystem;
23+
import frc.robot.subsystems.ShooterSubsystem;
24+
import frc.robot.subsystems.Superstructure;
2025
import frc.robot.subsystems.SwerveSubsystem;
2126
import swervelib.SwerveDrive;
2227

2328
public class RobotContainer {
2429
private final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "swerve"));
2530

31+
private final IntakeSubsystem intake = new IntakeSubsystem();
32+
private final HopperSubsystem hopper = new HopperSubsystem();
33+
private final KickerSubsystem kicker = new KickerSubsystem();
34+
private final ShooterSubsystem shooter = new ShooterSubsystem();
35+
36+
private final Superstructure superstructure = new Superstructure(shooter, null, null, intake, hopper, kicker);
37+
2638
private final SendableChooser<Command> autoChooser;
2739

2840
/**
@@ -53,8 +65,8 @@ public RobotContainer() {
5365

5466
private void configureBindings() {
5567
// Set up controllers
56-
DriverControls.configure(ControllerConstants.kDriverControllerPort, drivebase, null);
57-
OperatorControls.configure(ControllerConstants.kOperatorControllerPort, drivebase, null);
68+
DriverControls.configure(ControllerConstants.kDriverControllerPort, drivebase, superstructure);
69+
OperatorControls.configure(ControllerConstants.kOperatorControllerPort, drivebase, superstructure);
5870
PoseControls.configure(ControllerConstants.kPoseControllerPort, drivebase);
5971
}
6072

‎src/main/java/frc/robot/controls/DriverControls.java‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -88,6 +88,7 @@ public static void configure(int port, SwerveSubsystem drivetrain, Superstructur
8888
} else if (Robot.isSimulation()) {
8989
controller.back().whileTrue(fireAlgae(drivetrain));
9090
} else {
91+
controller.y().onTrue((Commands.runOnce(drivetrain::zeroGyro)));
9192
controller.leftBumper().whileTrue(Commands.runOnce(drivetrain::lock, drivetrain).repeatedly());
9293
}
9394
}

‎src/main/java/frc/robot/controls/OperatorControls.java‎

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

3+
import static edu.wpi.first.units.Units.Degrees;
4+
35
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
46
import frc.robot.subsystems.Superstructure;
57
import frc.robot.subsystems.SwerveSubsystem;
@@ -8,5 +10,30 @@ public class OperatorControls {
810

911
public static void configure(int port, SwerveSubsystem drivetrain, Superstructure superstructure) {
1012
CommandXboxController controller = new CommandXboxController(port);
13+
14+
// Intake controls - A to intake, B to eject
15+
controller.a().whileTrue(superstructure.intakeCommand());
16+
controller.b().whileTrue(superstructure.ejectCommand());
17+
18+
// Hopper controls - X to run hopper forward, Y to run backward
19+
controller.x().whileTrue(superstructure.hopperFeedCommand());
20+
controller.y().whileTrue(superstructure.hopperReverseCommand());
21+
22+
// Shooter controls - Right bumper to shoot
23+
controller.rightBumper().whileTrue(superstructure.shootCommand());
24+
controller.leftBumper().whileTrue(superstructure.stopShootingCommand());
25+
26+
// Kicker controls
27+
controller.back().whileTrue(superstructure.kickerFeedCommand());
28+
controller.start().whileTrue(superstructure.kickerStopCommand());
29+
30+
// Intake pivot controls. Setpoints need to be tested and finalized.
31+
32+
// 0 for default
33+
// -45 for collection
34+
// +25 just because. We can add more setpoints if necessary.
35+
// controller.povUp().whileTrue(superstructure.setIntakePivotAngle(Degrees.of(25)));
36+
// controller.povRight().whileTrue(superstructure.setIntakePivotAngle(Degrees.of(0)));
37+
// controller.povDown().whileTrue(superstructure.setIntakePivotAngle(Degrees.of(-45)));
1138
}
1239
}

‎src/main/java/frc/robot/subsystems/HopperSubsystem.java‎

Lines changed: 84 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,84 @@
1+
package frc.robot.subsystems;
2+
3+
import com.revrobotics.spark.SparkLowLevel.MotorType;
4+
import com.thethriftybot.ThriftyNova;
5+
6+
import edu.wpi.first.math.system.plant.DCMotor;
7+
import static edu.wpi.first.units.Units.Amps;
8+
import static edu.wpi.first.units.Units.Inches;
9+
import static edu.wpi.first.units.Units.Pounds;
10+
import static edu.wpi.first.units.Units.RPM;
11+
import edu.wpi.first.wpilibj2.command.Command;
12+
import edu.wpi.first.wpilibj2.command.SubsystemBase;
13+
import frc.robot.Constants;
14+
import yams.gearing.GearBox;
15+
import yams.gearing.MechanismGearing;
16+
import yams.mechanisms.config.FlyWheelConfig;
17+
import yams.mechanisms.velocity.FlyWheel;
18+
import yams.motorcontrollers.SmartMotorController;
19+
import yams.motorcontrollers.SmartMotorControllerConfig;
20+
import yams.motorcontrollers.SmartMotorControllerConfig.ControlMode;
21+
import yams.motorcontrollers.SmartMotorControllerConfig.MotorMode;
22+
import yams.motorcontrollers.SmartMotorControllerConfig.TelemetryVerbosity;
23+
import yams.motorcontrollers.local.NovaWrapper;
24+
25+
public class HopperSubsystem extends SubsystemBase {
26+
27+
private static final double HOPPER_SPEED = 1.0;
28+
29+
// Nova motor controller with NEO motor
30+
private ThriftyNova hopperNova = new ThriftyNova(Constants.HopperConstants.kHopperMotorId);
31+
32+
private SmartMotorControllerConfig smcConfig = new SmartMotorControllerConfig(this)
33+
.withControlMode(ControlMode.OPEN_LOOP)
34+
.withTelemetry("HopperMotor", TelemetryVerbosity.HIGH)
35+
.withGearing(new MechanismGearing(GearBox.fromReductionStages(4))) // 4:1 gear reduction
36+
.withMotorInverted(false)
37+
.withIdleMode(MotorMode.BRAKE)
38+
.withStatorCurrentLimit(Amps.of(40));
39+
40+
private SmartMotorController smc = new NovaWrapper(hopperNova, DCMotor.getNEO(1), smcConfig);
41+
42+
private final FlyWheelConfig hopperConfig = new FlyWheelConfig(smc)
43+
.withDiameter(Inches.of(4))
44+
.withMass(Pounds.of(0.5))
45+
.withUpperSoftLimit(RPM.of(6000))
46+
.withLowerSoftLimit(RPM.of(-6000))
47+
.withTelemetry("Hopper", TelemetryVerbosity.HIGH);
48+
49+
private FlyWheel hopper = new FlyWheel(hopperConfig);
50+
51+
public HopperSubsystem() {
52+
}
53+
54+
/**
55+
* Command to run the hopper forward while held.
56+
*/
57+
public Command feedCommand() {
58+
return hopper.set(HOPPER_SPEED).finallyDo(() -> smc.setDutyCycle(0)).withName("Hopper.Feed");
59+
}
60+
61+
/**
62+
* Command to run the hopper in reverse while held.
63+
*/
64+
public Command reverseCommand() {
65+
return hopper.set(-HOPPER_SPEED).finallyDo(() -> smc.setDutyCycle(0)).withName("Hopper.Reverse");
66+
}
67+
68+
/**
69+
* Command to stop the hopper.
70+
*/
71+
public Command stopCommand() {
72+
return hopper.set(0).withName("Hopper.Stop");
73+
}
74+
75+
@Override
76+
public void periodic() {
77+
hopper.updateTelemetry();
78+
}
79+
80+
@Override
81+
public void simulationPeriodic() {
82+
hopper.simIterate();
83+
}
84+
}

‎src/main/java/frc/robot/subsystems/IntakeSubsystem.java‎

Lines changed: 126 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,126 @@
1+
package frc.robot.subsystems;
2+
3+
import com.thethriftybot.ThriftyNova;
4+
5+
import edu.wpi.first.math.controller.ArmFeedforward;
6+
import edu.wpi.first.math.system.plant.DCMotor;
7+
import edu.wpi.first.units.measure.Angle;
8+
9+
import static edu.wpi.first.units.Units.Amps;
10+
import static edu.wpi.first.units.Units.Degrees;
11+
import static edu.wpi.first.units.Units.DegreesPerSecond;
12+
import static edu.wpi.first.units.Units.DegreesPerSecondPerSecond;
13+
import static edu.wpi.first.units.Units.Feet;
14+
import static edu.wpi.first.units.Units.Inches;
15+
import static edu.wpi.first.units.Units.Pounds;
16+
import static edu.wpi.first.units.Units.RPM;
17+
import static edu.wpi.first.units.Units.Seconds;
18+
19+
import edu.wpi.first.wpilibj2.command.Command;
20+
import edu.wpi.first.wpilibj2.command.Commands;
21+
import edu.wpi.first.wpilibj2.command.SubsystemBase;
22+
import frc.robot.Constants;
23+
import yams.gearing.GearBox;
24+
import yams.gearing.MechanismGearing;
25+
import yams.mechanisms.config.ArmConfig;
26+
import yams.mechanisms.config.FlyWheelConfig;
27+
import yams.mechanisms.positional.Arm;
28+
import yams.mechanisms.velocity.FlyWheel;
29+
import yams.motorcontrollers.SmartMotorController;
30+
import yams.motorcontrollers.SmartMotorControllerConfig;
31+
import yams.motorcontrollers.SmartMotorControllerConfig.ControlMode;
32+
import yams.motorcontrollers.SmartMotorControllerConfig.MotorMode;
33+
import yams.motorcontrollers.SmartMotorControllerConfig.TelemetryVerbosity;
34+
import yams.motorcontrollers.local.NovaWrapper;
35+
36+
public class IntakeSubsystem extends SubsystemBase {
37+
38+
private static final double INTAKE_SPEED = 0.1;
39+
40+
// ThriftyNova controlling the intake roller
41+
private ThriftyNova rollerNova = new ThriftyNova(Constants.IntakeConstants.kRollerMotorId);
42+
43+
private SmartMotorControllerConfig smcConfig = new SmartMotorControllerConfig(this)
44+
.withControlMode(ControlMode.OPEN_LOOP)
45+
.withTelemetry("IntakeMotor", TelemetryVerbosity.HIGH)
46+
.withGearing(new MechanismGearing(GearBox.fromReductionStages(1))) // Direct drive, adjust if geared
47+
.withMotorInverted(false)
48+
.withIdleMode(MotorMode.COAST)
49+
.withStatorCurrentLimit(Amps.of(40));
50+
51+
private SmartMotorController smc = new NovaWrapper(rollerNova, DCMotor.getNeoVortex(1), smcConfig);
52+
53+
private final FlyWheelConfig intakeConfig = new FlyWheelConfig(smc)
54+
.withDiameter(Inches.of(4))
55+
.withMass(Pounds.of(0.5))
56+
.withUpperSoftLimit(RPM.of(6000))
57+
.withLowerSoftLimit(RPM.of(-6000))
58+
.withTelemetry("Intake", TelemetryVerbosity.HIGH);
59+
60+
private FlyWheel intake = new FlyWheel(intakeConfig);
61+
62+
// 5:1, 5:1, 60/18 reduction
63+
private SmartMotorControllerConfig intakePivotSmartMotorConfig = new SmartMotorControllerConfig(this)
64+
.withControlMode(ControlMode.CLOSED_LOOP)
65+
.withClosedLoopController(200, 0, 0, DegreesPerSecond.of(360), DegreesPerSecondPerSecond.of(360))
66+
.withFeedforward(new ArmFeedforward(0, 0, 0.1))
67+
.withTelemetry("IntakePivotMotor", TelemetryVerbosity.HIGH)
68+
.withGearing(new MechanismGearing(GearBox.fromReductionStages(5, 5, 60.0 / 18)))
69+
.withMotorInverted(true)
70+
.withIdleMode(MotorMode.BRAKE)
71+
.withStatorCurrentLimit(Amps.of(10))
72+
.withClosedLoopRampRate(Seconds.of(0.1));
73+
74+
private ThriftyNova pivotMotor = new ThriftyNova(Constants.IntakeConstants.kPivotMotorId);
75+
76+
private SmartMotorController intakePivotController = new NovaWrapper(pivotMotor, DCMotor.getNeoVortex(1),
77+
intakePivotSmartMotorConfig);
78+
79+
private final ArmConfig intakePivotConfig = new ArmConfig(intakePivotController)
80+
.withSoftLimits(Degrees.of(-95), Degrees.of(45)) // TODO: Find and set proper limits once setpoints and range is
81+
// known
82+
.withHardLimit(Degrees.of(-100), Degrees.of(50))
83+
.withStartingPosition(Degrees.of(-90))
84+
.withLength(Feet.of(1))
85+
.withMass(Pounds.of(2)) // Reis says: 2 pounds, not a lot
86+
.withTelemetry("IntakePivot", TelemetryVerbosity.HIGH);
87+
88+
private Arm intakePivot = new Arm(intakePivotConfig);
89+
90+
public IntakeSubsystem() {
91+
}
92+
93+
/**
94+
* Command to run the intake while held.
95+
*/
96+
public Command intakeCommand() {
97+
return intake.set(INTAKE_SPEED).finallyDo(() -> smc.setDutyCycle(0)).withName("Intake.Run");
98+
}
99+
100+
/**
101+
* Command to eject while held.
102+
*/
103+
public Command ejectCommand() {
104+
return intake.set(-INTAKE_SPEED).finallyDo(() -> smc.setDutyCycle(0)).withName("Intake.Eject");
105+
}
106+
107+
public Command setPivotAngle(Angle angle) {
108+
return intakePivot.setAngle(angle).withName("IntakePivot.SetAngle");
109+
}
110+
111+
public Command rezero() {
112+
return Commands.runOnce(() -> pivotMotor.setEncoderPosition(0), this).withName("IntakePivot.Rezero");
113+
}
114+
115+
@Override
116+
public void periodic() {
117+
intake.updateTelemetry();
118+
intakePivot.updateTelemetry();
119+
}
120+
121+
@Override
122+
public void simulationPeriodic() {
123+
intake.simIterate();
124+
intakePivot.simIterate();
125+
}
126+
}

0 commit comments

Comments
 (0)