Team4388/2026KPopRobotHunters
1
0
Fork 0
mirror of https://github.com/Team4388/2026KPopRobotHunters.git synced 2026-06-08 16:28:05 -06:00
Code Issues Packages Projects Releases Wiki Activity

One controller and elastic layout for demo

Browse Source
This commit is contained in:
mimigamin
2026-04-27 23:50:59 -06:00
parent a15a6d08bf
commit 41461c9b07
6 changed files with 127 additions and 259 deletions
Download Patch File Download Diff File Expand all files Collapse all files
elastic-layout.json
+81
View File
@@ -2,6 +2,87 @@
"version": 1.0,
"grid_size": 128,
"tabs": [
{
"name": "DEMO",
"grid_layout": {
"layouts": [],
"containers": [
{
"title": "Controller Binds",
"x": 0.0,
"y": 0.0,
"width": 384.0,
"height": 512.0,
"type": "Large Text Display",
"properties": {
"topic": "/SmartDashboard/Controller Binds",
"period": 0.06,
"data_type": "string"
}
},
{
"title": "Demo % output",
"x": 512.0,
"y": 128.0,
"width": 256.0,
"height": 128.0,
"type": "Text Display",
"properties": {
"topic": "/SmartDashboard/Demo % output",
"period": 0.06,
"data_type": "double",
"show_submit_button": true
}
},
{
"title": "Indexer FWD % Output",
"x": 512.0,
"y": 256.0,
"width": 256.0,
"height": 128.0,
"type": "Text Display",
"properties": {
"topic": "/SmartDashboard/Indexer FWD % Output",
"period": 0.06,
"data_type": "double",
"show_submit_button": true
}
},
{
"title": "IndexerTargetOutput",
"x": 896.0,
"y": 0.0,
"width": 256.0,
"height": 256.0,
"type": "Graph",
"properties": {
"topic": "/AdvantageKit/Shooter/IndexerTargetOutput",
"period": 0.033,
"data_type": "double",
"time_displayed": 5.0,
"color": 4278238420,
"line_width": 2.0
}
},
{
"title": "IndexerOutput",
"x": 896.0,
"y": 256.0,
"width": 256.0,
"height": 256.0,
"type": "Graph",
"properties": {
"topic": "/AdvantageKit/Shooter/IndexerOutput",
"period": 0.033,
"data_type": "double",
"time_displayed": 5.0,
"color": 4278238420,
"line_width": 2.0
}
}
]
}
},
{
"name": "Teleoperated",
"grid_layout": {
src/main/java/frc4388/robot/RobotContainer.java
+17 -131
View File
@@ -12,37 +12,31 @@
package frc4388.robot;
import java.io.File;
import java.util.Optional;
import com.pathplanner.lib.auto.NamedCommands;
import com.pathplanner.lib.commands.PathPlannerAuto;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
// Commands
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.WaitCommand;
import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc4388.robot.commands.waitSupplier;
import frc4388.robot.commands.Swerve.StayInPosition;
import frc4388.robot.commands.alignment.AutoAlign;
import frc4388.robot.constants.Constants;
import frc4388.robot.constants.FieldConstants;
import frc4388.robot.constants.Constants.OIConstants;
import frc4388.robot.constants.Constants.SimConstants.Mode;
import frc4388.robot.constants.FieldConstants;
import frc4388.robot.subsystems.intake.Intake;
import frc4388.robot.subsystems.intake.Intake.IntakeMode;
import frc4388.robot.subsystems.led.LED;
@@ -50,7 +44,6 @@ import frc4388.robot.subsystems.shooter.Shooter;
import frc4388.robot.subsystems.shooter.ShooterConstants;
import frc4388.robot.subsystems.swerve.SimpleSwerveSim;
import frc4388.robot.subsystems.swerve.SwerveDrive;
import frc4388.robot.subsystems.vision.Lidar;
import frc4388.robot.subsystems.vision.Vision;
import frc4388.utility.DeferredBlock;
import frc4388.utility.compute.FieldPositions;
@@ -109,74 +102,12 @@ public class RobotContainer {
private Command autoCommand;
private Command IntakeExtended = new SequentialCommandGroup(
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.ExtendingRolling), m_robotIntake)
);
private Command LabubuGrowl = new SequentialCommandGroup(
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.LabubuGrowl), m_robotIntake)
);
// private Command LidarIntake = new SequentialCommandGroup(
// // Right now this will just go to the closest ball constantly updating - need to make it so it locks on one ball
// // RobotIntakeDown,
// // new WaitCommand(1),
// // new InstantCommand(() -> System.out.println("Closest Ball: " + m_lidar.getClosestBall())),
// new AutoAlign(m_robotSwerveDrive, m_vision, m_lidar, true)
// // new RunCommand(
// // () -> m_robotSwerveDrive.driveWithInput(
// // m_lidar.getClosestBall(),
// // new Translation2d(m_lidar.getLatestBallAngle().getCos() * 0.1, 0),
// // false
// // ),
// // m_robotSwerveDrive
// // )
// // .withTimeout(5.0)
// // .andThen(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive))
// );
private Command RobotRev = new SequentialCommandGroup(
new InstantCommand(() -> m_robotShooter.spinUpShooting(), m_robotShooter),
IntakeExtended,
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.ExpelBalls), m_robotIntake)
);
private Command WaitIntakeReference =
new WaitUntilCommand(m_robotIntake::intakeAtReference);
private Command ZeroEncoder =
new InstantCommand(() -> m_robotIntake.io.fixEncoder(), m_robotIntake);
private Command RobotShootDriving = new SequentialCommandGroup(
new RunCommand(() ->
m_robotSwerveDrive.enableRotationOverride(FieldPositions.HUB_POSITION, ShooterConstants.AIM_LEAD_TIME.get(), FieldPositions.HUB_POSITION)
).withTimeout(20)
).finallyDo((interrupted) ->
m_robotSwerveDrive.disableRotationOverride()
);
private Command IntakeRetracted = new SequentialCommandGroup(
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.RectractTorque), m_robotIntake)
);
private Command RobotShoot = new SequentialCommandGroup(
// TEST NEW AUTO ALIGN
//new AutoAlign(m_robotSwerveDrive, m_vision, new Pose2d(FieldPositions.HUB_POSITION, new Rotation2d(0)), false),
new WaitUntilCommand(m_robotShooter::isShooterUpToSpeed),
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Idle), m_robotIntake),
new InstantCommand(()-> m_robotShooter.allowShooting(), m_robotShooter),
new WaitCommand(3.5),
IntakeRetracted,
new WaitCommand(5),
new InstantCommand(() -> m_robotShooter.denyShooting(), m_robotShooter),
new InstantCommand(()-> m_robotShooter.spinUpIdle(), m_robotShooter)
);
public RobotContainer() {
configureButtonBindings();
configureSINGLEBindings();
// Called on first robot enable
DeferredBlock.addBlock(() -> {
@@ -193,15 +124,6 @@ public class RobotContainer {
m_robotShooter.io.updateGains();
}, true);
NamedCommands.registerCommand("Robot Rev Up", RobotRev);
NamedCommands.registerCommand("Zero Encoder", ZeroEncoder);
NamedCommands.registerCommand("Intake Retracted", IntakeRetracted);
NamedCommands.registerCommand("Robot Shoot", RobotShoot);
// NamedCommands.registerCommand("Lidar Intake", LidarIntake);
NamedCommands.registerCommand("Intake Extended", IntakeExtended);
NamedCommands.registerCommand("Labubu Growl", LabubuGrowl);
NamedCommands.registerCommand("Robot Shoot Driving", RobotShootDriving);
NamedCommands.registerCommand("Intake Reference", WaitIntakeReference);
NamedCommands.registerCommand("WaitShooter", new WaitUntilCommand(m_robotShooter::isShooterUpToSpeed));
NamedCommands.registerCommand("AllowShooting", new InstantCommand(() -> m_robotShooter.allowShooting(), m_robotShooter));
NamedCommands.registerCommand("DenyShooting", new InstantCommand(() -> m_robotShooter.denyShooting(), m_robotShooter));
@@ -246,55 +168,12 @@ public class RobotContainer {
/**
* Use this method to define your button->command mappings. Buttons can be
* created by instantiating a {@link GenericHID} or one of its subclasses
* ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then
* passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
*/
private void configureButtonBindings() {
//Driver controls
new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro()));
new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp()));
new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
// manually shoot from climb post/ feed balls
new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON)
.onTrue(new InstantCommand(() -> {
m_robotShooter.spinUpFeeding();
m_robotIntake.rollerStop();
}))
.onFalse(new InstantCommand(() -> {
m_robotShooter.spinUpIdle();
}));
new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)
.onTrue(new InstantCommand(() -> {
m_robotIntake.setMode(IntakeMode.ExtendingRolling);
}));
new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON)
.onTrue(new InstantCommand(() -> {
m_robotIntake.setMode(IntakeMode.ArmIdleRollingNot);
}));
}
private void configureSINGLEBindings() {
String controllerInstructions = "Single Controller: \n- A: Reset Gyro \n- Right Bumper: Shift Up \n- Left Bumper: Shift Down \n- X Button: Roller On \n- Y Button: Roller Off \n- B Button: Labubu Growl \n- Back Button: Manual shoot \n- Menu Button: Expels balls";
SmartDashboard.putString("Controller Binds", controllerInstructions);
//Driver controls
new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro()));
@@ -304,9 +183,7 @@ public class RobotContainer {
new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
// manually shoot from climb post/ feed balls
new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON)
.onTrue(new InstantCommand(() -> {
m_robotShooter.spinUpFeeding();
m_robotIntake.rollerStop();
@@ -315,18 +192,27 @@ public class RobotContainer {
m_robotShooter.spinUpIdle();
}));
new JoystickButton(getDeadbandedDriverController(), XboxController.START_BUTTON)
.onTrue(new InstantCommand(() -> {
m_robotShooter.spinUpFeeding();
m_robotIntake.rollerStop();
}));
new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
.onTrue(new InstantCommand(() -> {
m_robotIntake.setMode(IntakeMode.ExtendingRolling);
m_robotIntake.setMode(IntakeMode.RollerOn);
}));
new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
.onTrue(new InstantCommand(() -> {
m_robotIntake.setMode(IntakeMode.ArmIdleRollingNot);
m_robotIntake.setMode(IntakeMode.RollerOff);
}));
new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
.onTrue(new InstantCommand(() -> {
m_robotIntake.setMode(IntakeMode.LabubuGrowl);
}));
}
//.onTrue(new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.SOLID_PINK_HOT)));
src/main/java/frc4388/robot/constants/BuildConstants.java
+6 -6
View File
@@ -7,12 +7,12 @@ public final class BuildConstants {
public static final String MAVEN_GROUP = "";
public static final String MAVEN_NAME = "2026KPopRobotHunters";
public static final String VERSION = "unspecified";
public static final int GIT_REVISION = 241;
public static final String GIT_SHA = "6316daedf7fea6d03ec620bc433364a6881e3316";
public static final String GIT_DATE = "2026-04-17 17:27:23 MDT";
public static final String GIT_BRANCH = "demo";
public static final String BUILD_DATE = "2026-04-17 18:31:24 MDT";
public static final long BUILD_UNIX_TIME = 1776472284468L;
public static final int GIT_REVISION = 242;
public static final String GIT_SHA = "a15a6d08bf31c3ec80ed3e4327cbf214cc9781f5";
public static final String GIT_DATE = "2026-04-17 18:42:57 MDT";
public static final String GIT_BRANCH = "DemoBranch-(No-Intake)";
public static final String BUILD_DATE = "2026-04-27 23:44:57 MDT";
public static final long BUILD_UNIX_TIME = 1777355097369L;
public static final int DIRTY = 1;
private BuildConstants(){}
src/main/java/frc4388/robot/subsystems/intake/Intake.java
+12 -99
View File
@@ -1,8 +1,5 @@
package frc4388.robot.subsystems.intake;
import static edu.wpi.first.units.Units.Amps;
import static edu.wpi.first.units.Units.Rotations;
import java.util.function.Supplier;
import org.littletonrobotics.junction.Logger;
@@ -10,7 +7,6 @@ import org.littletonrobotics.junction.Logger;
import com.ctre.phoenix6.Utils;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.subsystems.swerve.SwerveDrive;
@@ -31,19 +27,11 @@ public class Intake extends SubsystemBase {
}
public enum IntakeMode {
ExtendingIdle,
ExtendingRolling,
EncoderFix,
Retracting,
ArmIdleRollingNot,
Idle,
RectractTorque,
RectractAuto,
Bouncing,
RollerOn,
RollerOff,
ExpelBalls,
LabubuGrowl
LabubuGrowl,
Idle
}
private boolean overCompressed = false;
@@ -51,15 +39,6 @@ public class Intake extends SubsystemBase {
public void setMode(IntakeMode mode) {
this.mode = mode;
switch (mode) {
case Bouncing:
// When bounce is enabled: set the bounce timer
this.state.currentBounceTime = Utils.getSystemTimeSeconds() + IntakeConstants.INTAKE_BOUNCE_HALF_PERIOD.get();
break;
default:
break;
}
}
public IntakeMode getMode() {
@@ -119,91 +98,25 @@ public class Intake extends SubsystemBase {
// getCurrentTime
switch (mode) {
case ExtendingIdle:
// io.armOutput(IntakeConstants.ARM_EXTEND_PERCENT_OUTPUT.get());
io.setRollerOutput(state, 0);
case RollerOn:
io.setRollerOutput(state, IntakeConstants.ROLLER_PERCENT_OUTPUT.get());
break;
case ExtendingRolling:
// io.armOutput(IntakeConstants.ARM_EXTEND_PERCENT_OUTPUT.get());
io.setRollerOutput(state, IntakeConstants.ROLLER_PERCENT_OUTPUT.get()); //getTargetRollerSpeed(ChassisOverallSpeed));
break;
case EncoderFix:
// io.armFix(IntakeConstants.FIX_ARM_PERCENT_OUTPUT.get());
io.setRollerOutput(state, 0);
break;
case Retracting:
// io.armOutput(IntakeConstants.ARM_RETRACT_PERCENT_OUTPUT.get());
// if(state.intakeEncoder.in(Rotations) > IntakeConstants.ARM_REVERSE_ROLLER_RANGE.get()) {
io.setRollerOutput(state, IntakeConstants.ROLLER_RETRACT_PERCENT_OUTPUT.get());
// } else {
// io.setRollerOutput(state, 0);
// }
break;
case ArmIdleRollingNot:
// io.armOutput(0);
io.setRollerOutput(state, 0);
break;
case Bouncing:
// io.setRollerOutput(state, 0);
// if(
// state.armMotorCurrent.in(Amps) > IntakeConstants.INTAKE_BOUNCE_CURRENT_LIMIT.get()
// // Math.abs(state.armMotorVelocity.in(RotationsPerSecond)) < IntakeConstants.INTAKE_BOUNCE_VELOCITY_LIMIT.get()
// ) {
// this.state.currentBounceTime = Utils.getSystemTimeSeconds() + IntakeConstants.INTAKE_BOUNCE_HALF_PERIOD.get();
// }
// // Get the time delta from the last bounce time update
// double currentTime = Utils.getSystemTimeSeconds() - state.currentBounceTime;
// // Get the percentage through the bounce period (0 output means one half period has passed)
// double percentOutput = (currentTime / IntakeConstants.INTAKE_BOUNCE_HALF_PERIOD.get()) * IntakeConstants.INTAKE_BOUNCE_OUTPUT.get();
// // Clamp the output of the motor to some value
// percentOutput = -Math.max(Math.min(percentOutput, IntakeConstants.INTAKE_BOUNCE_MAX_OUTPUT.get()), -IntakeConstants.INTAKE_BOUNCE_MAX_OUTPUT.get());
// io.armOutput(percentOutput);
// if(percentOutput < 0) {
// io.setRollerOutput(state, IntakeConstants.ROLLER_RETRACT_PERCENT_OUTPUT.get());
// } else {
// io.setRollerOutput(state, 0);
// }
// break;
// case RectractTorque:
// io.setRollerOutput(state, IntakeConstants.ROLLER_RETRACT_PERCENT_OUTPUT.get());
// if (!overCompressed){
// io.armOutput(IntakeConstants.ARM_SQUEEZE_PERCENT_OUTPUT.get());
// } else {
// io.armOutput(IntakeConstants.ARM_REDUCED_SQUEEZE_PERCENT_OUTPUT.get());
// }
// if(state.intakeEncoder.in(Rotations) > IntakeConstants.ARM_REVERSE_ROLLER_RANGE.get()) {
// io.setRollerOutput(state, IntakeConstants.ROLLER_RETRACT_PERCENT_OUTPUT.get());
// } else {
// io.setRollerOutput(state, 0);
// }
break;
case RectractAuto:
io.setRollerOutput(state, IntakeConstants.ROLLER_RETRACT_PERCENT_OUTPUT.get());
// io.armOutput(IntakeConstants.ARM_AUTO_OUTPUT.get());
break;
case Idle:
// io.armOutput(0);
case RollerOff:
io.setRollerOutput(state, 0);
break;
case ExpelBalls:
// io.armOutput(0);
io.setRollerOutput(state, IntakeConstants.ROLLER_EJECT_PERCENT_OUTPUT.get());
break;
case LabubuGrowl:
// io.armOutput(IntakeConstants.ARM_EXTEND_PERCENT_OUTPUT.get());
io.setRollerOutput(state, IntakeConstants.ROLLER_LABUBU_GROWL_PERCENT_OUTPUT.get());
break;
case Idle:
io.armOutput(0);
io.setRollerOutput(state, 0);
break;
}
// if (state.retractedLimit){
// this.mode = IntakeMode.Retracted;
src/main/java/frc4388/robot/subsystems/shooter/Shooter.java
+2 -14
View File
@@ -1,8 +1,5 @@
package frc4388.robot.subsystems.shooter;
import static edu.wpi.first.units.Units.Amps;
import static edu.wpi.first.units.Units.Rotation;
import static edu.wpi.first.units.Units.Rotations;
import static edu.wpi.first.units.Units.RotationsPerSecond;
import org.littletonrobotics.junction.AutoLogOutput;
@@ -12,9 +9,6 @@ import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.units.measure.Current;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.constants.Constants;
import frc4388.robot.subsystems.intake.Intake;
@@ -162,12 +156,6 @@ public class Shooter extends SubsystemBase {
badShooterVelocity = Math.abs(shooterSpeed - shooterSpeedTarget) > ShooterConstants.SHOOTER_SPEED_TOLERANCE.get();
//revtime calculations
// double shooterAcceleration =
double shooterSpeedTargetPretend = ShooterConstants.getTargetShooterSpeed(distanceToHub, chassisXSpeed).in(RotationsPerSecond);
double revTime = (Math.abs(shooterSpeed - shooterSpeedTargetPretend)/((7 - shooterSpeedTargetPretend)/ShooterConstants.T_CONSTANT));
// double revTimeExp = ShooterConstants.T_CONSTANT * Math.log(1 - Math.abs(shooterSpeed/shooterSpeedTargetPretend));
Logger.recordOutput("Time to rev", revTime);
switch (mode) {
case Shooting:
@@ -211,9 +199,9 @@ public class Shooter extends SubsystemBase {
break;
case ManualShoot:
if(io.demoSpeed(state)){
io.setIndexerOutput(state, -0.5);
io.setIndexerOutput(state, ShooterConstants.INDEXER_FORWARD_OUTPUT.get());
}else{
io.setIndexerOutput(state, 0.2);
io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
}
io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_OVERRIDE_VELOCITY.get()));
m_robotLED.setMode(Constants.LEDConstants.OPREADY_FEED);
src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java
+1 -1
View File
@@ -54,7 +54,7 @@ public interface ShooterIO {
public default void motorStalled(ShooterState state, Intake m_Intake, LED m_robotLED) {}
public default boolean demoSpeed(ShooterState state) {
boolean demo = false;
if((Math.abs(Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond))-Math.abs(state.motor2TargetVelocity.in(RotationsPerSecond))) < 3)){
if((Math.abs(Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond))-Math.abs(state.motor1Velocity.in(RotationsPerSecond))) < 3)){
demo = true;
}
return demo;