Team4388/2026KPopRobotHunters
1
0
Fork 0
mirror of https://github.com/Team4388/2026KPopRobotHunters.git synced 2026-06-09 00:38:03 -06:00
Code Issues Packages Projects Releases Wiki Activity

Make rotate code work

Browse Source
This commit is contained in:
C4llSqin
2026-01-13 18:17:37 -07:00
parent 8dbb9d5a90
commit 5e86771288
6 changed files with 181 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/deploy/pathplanner/navgrid.json
+1
View File
File diff suppressed because one or more lines are too long
src/main/deploy/pathplanner/paths/Example Path.path
+54
View File
@@ -0,0 +1,54 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 2.0,
"y": 7.0
},
"prevControl": null,
"nextControl": {
"x": 3.0,
"y": 7.0
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 4.0,
"y": 6.0
},
"prevControl": {
"x": 3.0,
"y": 6.0
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 0.0
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": 0.0
},
"useDefaultConstraints": true
}
src/main/deploy/pathplanner/settings.json
+32
View File
@@ -0,0 +1,32 @@
{
"robotWidth": 0.9,
"robotLength": 0.9,
"holonomicMode": true,
"pathFolders": [],
"autoFolders": [],
"defaultMaxVel": 3.0,
"defaultMaxAccel": 3.0,
"defaultMaxAngVel": 540.0,
"defaultMaxAngAccel": 720.0,
"defaultNominalVoltage": 12.0,
"robotMass": 74.088,
"robotMOI": 6.883,
"robotTrackwidth": 0.546,
"driveWheelRadius": 0.048,
"driveGearing": 5.143,
"maxDriveSpeed": 5.45,
"driveMotorType": "krakenX60",
"driveCurrentLimit": 60.0,
"wheelCOF": 1.2,
"flModuleX": 0.273,
"flModuleY": 0.273,
"frModuleX": 0.273,
"frModuleY": -0.273,
"blModuleX": -0.273,
"blModuleY": 0.273,
"brModuleX": -0.273,
"brModuleY": -0.273,
"bumperOffsetX": 0.0,
"bumperOffsetY": 0.0,
"robotFeatures": []
}
src/main/java/frc4388/robot/RobotContainer.java
+51 -51
View File
@@ -7,18 +7,14 @@
package frc4388.robot;
// Drive Systems
import edu.wpi.first.wpilibj.DriverStation;
import java.io.File;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc4388.utility.controller.XboxController;
import frc4388.utility.DeferredBlock;
import frc4388.utility.compute.TimesNegativeOne;
import frc4388.utility.controller.ButtonBox;
import frc4388.utility.controller.DeadbandedXboxController;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
@@ -27,43 +23,23 @@ import edu.wpi.first.wpilibj2.command.button.Trigger;
// Commands
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
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.Commands;
import edu.wpi.first.wpilibj2.command.ConditionalCommand;
// Autos
import frc4388.utility.controller.VirtualController;
import frc4388.utility.controller.XboxController;
import frc4388.robot.commands.MoveForTimeCommand;
import frc4388.robot.commands.MoveUntilSuply;
// import frc4388.robot.commands.alignment.DriveToReef;
import frc4388.robot.commands.alignment.DriveUntilLiDAR;
import frc4388.robot.commands.alignment.LidarAlign;
// import frc4388.robot.commands.wait.waitElevatorRefrence;
// import frc4388.robot.commands.wait.waitEndefectorRefrence;
// import frc4388.robot.commands.wait.waitFeedCoral;
import frc4388.robot.commands.wait.waitSupplier;
import frc4388.robot.commands.alignment.RotTo45;
import frc4388.robot.constants.Constants;
import frc4388.robot.constants.Constants.AutoConstants;
import frc4388.robot.constants.Constants.LiDARConstants;
import frc4388.robot.constants.Constants.OIConstants;
import frc4388.robot.constants.Constants.SimConstants.Mode;
import com.pathplanner.lib.auto.NamedCommands;
import com.pathplanner.lib.commands.PathPlannerAuto;
// Subsystems
import frc4388.robot.subsystems.LED;
// import frc4388.robot.subsystems.elevator.Elevator;
// import frc4388.robot.subsystems.elevator.Elevator.CoordinationState;
import frc4388.robot.subsystems.lidar.LiDAR;
import frc4388.robot.subsystems.swerve.SwerveDrive;
import frc4388.robot.subsystems.vision.Vision;
// Utilites
import frc4388.utility.DeferredBlock;
import frc4388.utility.compute.TimesNegativeOne;
// import frc4388.utility.compute.ReefPositionHelper.Side;
/**
* This class is where the bulk of the robot should be declared. Since
@@ -91,7 +67,7 @@ public class RobotContainer {
/* Controllers */
private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTONBOX_ID);
// private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTONBOX_ID);
// public List<Subsystem> subsystems = new ArrayList<>();
@@ -106,30 +82,54 @@ public class RobotContainer {
private SendableChooser<String> autoChooser;
private Command autoCommand;
public RobotContainer() {
configureButtonBindings();
DeferredBlock.addBlock(() -> { // Called on first robot enable
m_robotSwerveDrive.resetGyro();
}, false);
DeferredBlock.addBlock(() -> { // Called on every robot enable
TimesNegativeOne.update();
}, true);
DriverStation.silenceJoystickConnectionWarning(true);
m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(),
// m_robotSwerveDrive.driveWithInput(new Translation2d(.4, 0),
getDeadbandedDriverController().getRight(),
true);
}, m_robotSwerveDrive)
.withName("SwerveDrive DefaultCommand"));
m_robotSwerveDrive.setToSlow();
}
/**
* This method is used to replcate {@link Trigger Triggers} for {@link VirtualController Virtual Controllers}. <p/>
* Please use {@link RobotContainer#DualJoystickButton} in {@link RobotContainer#configureButtonBindings} for standard buttons.
* 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 configureVirtualButtonBindings() {
// ? /* Driver Buttons */
private void configureButtonBindings() {
/* Notice: the following buttons have not been replicated
* Swerve Drive Slow and Fast mode Gear Shifts : Fast mode is known to cause drift, so we disable that feature in Autoplayback
* Swerve Drive Rotation Gear Shifts : Same reason as Slow and Fast mode.
* Auto Recording controls : We don't want an Null Ouroboros for an auto.
*/
// ? /* Operator Buttons */
new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro()));
/* Notice: the following buttons have not been replicated
* Override Intake Position Encoder : It's an emergancy overide, for when the position of intake when the robot boots, the intake is not inside the robot.
* We don't need it in an auto.
* Climbing controls : We don't need to climb in auto.
*/
// ? Notice: the Programer Buttons are not to be replicated because they are designed for debuging the robot, and do not need to be replicated in auto.
new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
.onTrue(new RotTo45(m_robotSwerveDrive));
new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
.onTrue(new InstantCommand(() -> {m_robotSwerveDrive.softStop();}, m_robotSwerveDrive));
}
@@ -215,7 +215,7 @@ public class RobotContainer {
return this.m_operatorXbox;
}
public ButtonBox getButtonBox() {
return this.m_buttonBox;
}
// public ButtonBox getButtonBox() {
// return this.m_buttonBox;
// }
}
src/main/java/frc4388/robot/commands/alignment/RotTo45.java
+37
View File
@@ -0,0 +1,37 @@
package frc4388.robot.commands.alignment;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.subsystems.swerve.SwerveDrive;
public class RotTo45 extends Command {
SwerveDrive m_SwerveDrive;
Rotation2d targetAngle;
public RotTo45(SwerveDrive swerveDrive) {
m_SwerveDrive = swerveDrive;
addRequirements(swerveDrive);
}
@Override
public void initialize() {
targetAngle = new Rotation2d();
}
@Override
public void execute() {
m_SwerveDrive.driveRelativeAngle(new Translation2d(), targetAngle);
}
@Override
public boolean isFinished() {
Rotation2d curRot = m_SwerveDrive.getPose2d().getRotation();
// TODO: Tune
return Math.abs(curRot.getDegrees() - targetAngle.getDegrees()) < 5;
}
}
src/main/java/frc4388/robot/constants/BuildConstants.java
+6 -6
View File
@@ -5,14 +5,14 @@ package frc4388.robot.constants;
*/
public final class BuildConstants {
public static final String MAVEN_GROUP = "";
public static final String MAVEN_NAME = "2026KPopRobotHunters-new";
public static final String MAVEN_NAME = "2026KPopRobotHunters";
public static final String VERSION = "unspecified";
public static final int GIT_REVISION = 2;
public static final String GIT_SHA = "17c3ff1ec9ef6763ee1c736622be3ef0fcc30d10";
public static final String GIT_DATE = "2026-01-08 19:42:41 MST";
public static final int GIT_REVISION = 3;
public static final String GIT_SHA = "8dbb9d5a9099f417617ec2245e275a42b6788116";
public static final String GIT_DATE = "2026-01-10 16:52:43 MST";
public static final String GIT_BRANCH = "master";
public static final String BUILD_DATE = "2026-01-10 16:21:06 MST";
public static final long BUILD_UNIX_TIME = 1768087266722L;
public static final String BUILD_DATE = "2026-01-13 17:47:04 MST";
public static final long BUILD_UNIX_TIME = 1768351624667L;
public static final int DIRTY = 1;
private BuildConstants(){}