Rotate and Autos

This commit is contained in:
Shikhar
2026-01-19 13:42:16 -07:00
parent c68329c9ca
commit f600317435
14 changed files with 258 additions and 183 deletions
+90
View File
@@ -0,0 +1,90 @@
{
"System Joysticks": {
"window": {
"enabled": false
}
},
"keyboardJoysticks": [
{
"axisConfig": [
{
"decKey": 65,
"incKey": 68
},
{
"decKey": 87,
"incKey": 83
},
{
"decKey": 69,
"decayRate": 0.0,
"incKey": 82,
"keyRate": 0.009999999776482582
}
],
"axisCount": 3,
"buttonCount": 4,
"buttonKeys": [
90,
88,
67,
86
],
"povConfig": [
{
"key0": 328,
"key135": 323,
"key180": 322,
"key225": 321,
"key270": 324,
"key315": 327,
"key45": 329,
"key90": 326
}
],
"povCount": 1
},
{
"axisConfig": [
{
"decKey": 74,
"incKey": 76
},
{
"decKey": 73,
"incKey": 75
}
],
"axisCount": 2,
"buttonCount": 4,
"buttonKeys": [
77,
44,
46,
47
]
},
{
"axisConfig": [
{
"decKey": 263,
"incKey": 262
},
{
"decKey": 265,
"incKey": 264
}
],
"axisCount": 2,
"buttonCount": 6,
"buttonKeys": [
260,
268,
266,
261,
269,
267
]
}
]
}
@@ -0,0 +1,19 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "TurnNinety"
}
}
]
}
},
"resetOdom": true,
"folder": null,
"choreoAuto": false
}
@@ -16,12 +16,12 @@
},
{
"anchor": {
"x": 4.0,
"y": 6.0
"x": 2.0,
"y": 7.0
},
"prevControl": {
"x": 3.0,
"y": 6.0
"x": 1.0,
"y": 7.0
},
"nextControl": null,
"isLocked": false,
@@ -48,7 +48,7 @@
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": 0.0
"rotation": -90.48219994972354
},
"useDefaultConstraints": true
}
+16 -26
View File
@@ -9,47 +9,37 @@ package frc4388.robot;
import java.io.File;
import edu.wpi.first.math.geometry.Rotation2d;
import com.pathplanner.lib.commands.PathPlannerAuto;
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.SendableChooser;
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;
import edu.wpi.first.wpilibj2.command.button.Trigger;
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.RunCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
// Autos
import frc4388.utility.controller.VirtualController;
import frc4388.utility.controller.XboxController;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc4388.robot.commands.MoveForTimeCommand;
import frc4388.robot.commands.alignment.RotTo45;
import frc4388.robot.commands.MoveUntilSuply;
// import frc4388.robot.commands.alignment.DriveToReef;
// 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.Autos.neoPlaybackChooser;
import frc4388.robot.constants.Constants;
import frc4388.robot.constants.FieldConstants;
import frc4388.robot.constants.Constants.AutoConstants;
import frc4388.robot.constants.Constants.OIConstants;
import frc4388.robot.constants.Constants.SimConstants.Mode;
import com.pathplanner.lib.commands.PathPlannerAuto;
import frc4388.robot.constants.FieldConstants;
// Subsystems
import frc4388.robot.subsystems.LED;
import frc4388.robot.subsystems.swerve.SwerveDrive;
import frc4388.robot.subsystems.vision.Vision;
import frc4388.utility.DeferredBlock;
import frc4388.utility.compute.TimesNegativeOne;
import frc4388.utility.controller.DeadbandedXboxController;
// Autos
import frc4388.utility.controller.VirtualController;
import frc4388.utility.controller.XboxController;
/**
* This class is where the bulk of the robot should be declared. Since
@@ -65,7 +55,7 @@ public class RobotContainer {
/* Subsystems */
public final LED m_robotLED = new LED();
public final Vision m_vision = new Vision();
public final Vision m_vision = new Vision(m_robotMap.rightCamera, m_robotMap.leftCamera);
// public final Elevator m_robotElevator = new Elevator(m_robotMap.elevatorIO, m_robotLED);
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision);
// public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain);
@@ -91,7 +81,7 @@ public class RobotContainer {
// ! /* Autos */
private SendableChooser<String> autoChooser;
private Command autoCommand;
private neoPlaybackChooser m_autoChooser= new neoPlaybackChooser(m_robotSwerveDrive, null);
public RobotContainer() {
@@ -224,7 +214,7 @@ public class RobotContainer {
}
System.out.println("Robot Auto Changed " + filename);
});
// SmartDashboard.putData(autoChooser);
SmartDashboard.putData(autoChooser);
}
+11 -11
View File
@@ -40,8 +40,8 @@ public class RobotMap {
// private Pigeon2 m_pigeon2 = new Pigeon2(SwerveDriveConstants.IDs.DRIVE_PIGEON.id);
// public RobotGyro gyro = new RobotGyro(m_pigeon2);
// public final VisionIO leftCamera;
// public final VisionIO rightCamera;
public final VisionIO leftCamera;
public final VisionIO rightCamera;
// public final LiDAR lidar = new
@@ -61,15 +61,15 @@ public class RobotMap {
public RobotMap(SimConstants.Mode mode) {
switch (mode) {
case REAL:
// // Configure cameras
// PhotonCamera leftCameraReal = new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME);
// PhotonCamera rightCameraReal = new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME);
// Configure cameras
PhotonCamera leftCameraReal = new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME);
PhotonCamera rightCameraReal = new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME);
// leftCamera = new VisionReal(leftCameraReal, VisionConstants.LEFT_CAMERA_POS); ;
// rightCamera = new VisionReal(rightCameraReal, VisionConstants.RIGHT_CAMERA_POS);
leftCamera = new VisionReal(leftCameraReal, VisionConstants.RIGHT_CAMERA_POS); ;
rightCamera = new VisionReal(rightCameraReal, VisionConstants.LEFT_CAMERA_POS);
// FaultPhotonCamera.addDevice(leftCameraReal, "Left Camera");
// FaultPhotonCamera.addDevice(rightCameraReal , "Right Camera");
FaultPhotonCamera.addDevice(leftCameraReal, "Left Camera");
FaultPhotonCamera.addDevice(rightCameraReal , "Right Camera");
// // Configure LiDAR
// reefLidar = new LidarReal(LiDARConstants.REEF_LIDAR_DIO_CHANNEL);
@@ -121,8 +121,8 @@ public class RobotMap {
// case SIM:
// break;
default:
// leftCamera = new VisionIO() {};
// rightCamera = new VisionIO() {};
leftCamera = new VisionIO() {};
rightCamera = new VisionIO() {};
// reefLidar = new LidarIO() {};
// reverseLidar = new LidarIO() {};
swerveDrivetrain = new SwerveIO() {};
@@ -1,108 +1,107 @@
package frc4388.robot.commands.Autos;
// package frc4388.robot.commands.Autos;
// import java.io.File;
// import java.util.ArrayList;
// import java.util.HashMap;
import java.util.ArrayList;
// import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
// import edu.wpi.first.wpilibj.shuffleboard.ComplexWidget;
// import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
// import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
// import edu.wpi.first.wpilibj2.command.Command;
// import edu.wpi.first.wpilibj2.command.InstantCommand;
// import frc4388.robot.commands.Swerve.JoystickPlayback;
// import frc4388.robot.commands.Swerve.neoJoystickPlayback;
// import frc4388.robot.subsystems.SwerveDrive;
// import frc4388.utility.controller.VirtualController;
import edu.wpi.first.wpilibj.shuffleboard.ComplexWidget;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import frc4388.robot.commands.Swerve.neoJoystickPlayback;
import frc4388.robot.subsystems.swerve.SwerveDrive;
import frc4388.utility.controller.VirtualController;
// public class neoPlaybackChooser {
// private final SendableChooser<String> m_teamChosser = new SendableChooser<String>();
// private final SendableChooser<String> m_possitionChosser = new SendableChooser<String>();
// private final SendableChooser<String> m_autoNameChosser = new SendableChooser<String>();
public class neoPlaybackChooser {
private final SendableChooser<String> m_teamChosser = new SendableChooser<String>();
private final SendableChooser<String> m_possitionChosser = new SendableChooser<String>();
private final SendableChooser<String> m_autoNameChosser = new SendableChooser<String>();
// private final SwerveDrive m_swerve;
// private final VirtualController[] m_controllers;
// // private final ArrayList<SendableChooser<Command>> m_choosers = new ArrayList<>();
// // private SendableChooser<Command> m_playback = null;
// private final ArrayList<ComplexWidget> m_widgets = new ArrayList<>();
// // private final HashMap<String, Command> m_commandPool = new HashMap<>();
private final SwerveDrive m_swerve;
private final VirtualController[] m_controllers;
// private final ArrayList<SendableChooser<Command>> m_choosers = new ArrayList<>();
// private SendableChooser<Command> m_playback = null;
private final ArrayList<ComplexWidget> m_widgets = new ArrayList<>();
// private final HashMap<String, Command> m_commandPool = new HashMap<>();
// // private final File m_dir = new File("/home/lvuser/autos/");
// // private int m_cmdNum = 0;
// private final File m_dir = new File("/home/lvuser/autos/");
// private int m_cmdNum = 0;
// // commands
// private Command m_noAuto = new InstantCommand();
// commands
private Command m_noAuto = new InstantCommand();
// public neoPlaybackChooser(SwerveDrive swerve, VirtualController[] controllers) {
// m_teamChosser.addOption("Red", "red");
// m_teamChosser.setDefaultOption("Blue", "blue");
// m_teamChosser.addOption("Nuetral", "nuetral");
// m_possitionChosser.addOption("AMP", "amp");
// m_possitionChosser.setDefaultOption("Center", "center");
// m_possitionChosser.addOption("Source", "source");
// m_swerve = swerve;
// m_controllers = controllers;
// }
public neoPlaybackChooser(SwerveDrive swerve, VirtualController[] controllers) {
m_teamChosser.addOption("Red", "red");
m_teamChosser.setDefaultOption("Blue", "blue");
m_teamChosser.addOption("Nuetral", "nuetral");
m_possitionChosser.addOption("AMP", "amp");
m_possitionChosser.setDefaultOption("Center", "center");
m_possitionChosser.addOption("Source", "source");
m_swerve = swerve;
m_controllers = controllers;
SmartDashboard.putData("Team Chooser", m_teamChosser);
SmartDashboard.putData("Position Chooser", m_possitionChosser);
SmartDashboard.putData("Auto Name Chooser", m_autoNameChosser);
}
// public neoPlaybackChooser addOption(String name, String option) {
// m_autoNameChosser.addOption(name, option);
// return this;
// }
public neoPlaybackChooser addOption(String name, String option) {
m_autoNameChosser.addOption(name, option);
return this;
}
// // public PlaybackChooser buildDisplay() {
// // for (int i = 0; i < 10; i++) {
// // appendCommand();
// // }
// // m_playback = m_choosers.get(0);
// // nextChooser();
// public PlaybackChooser buildDisplay() {
// for (int i = 0; i < 10; i++) {
// appendCommand();
// }
// m_playback = m_choosers.get(0);
// nextChooser();
// // // ! This does not work, why?
// // Shuffleboard.getTab("Auto Chooser")
// // .add("Add Sequence", new InstantCommand(() -> nextChooser()))
// // .withPosition(4, 0);
// // return this;
// // }
// // ! This does not work, why?
// Shuffleboard.getTab("Auto Chooser")
// .add("Add Sequence", new InstantCommand(() -> nextChooser()))
// .withPosition(4, 0);
// return this;
// }
// // This will be bound to a button for the time being
// public void render() {
// // var chooser = new SendableChooser<Command>();
// // // chooser.setDefaultOption("No Auto", m_noAuto);
// This will be bound to a button for the time being
// public void render() {
// // var chooser = new SendableChooser<Command>();
// // // chooser.setDefaultOption("No Auto", m_noAuto);
// // m_choosers.add(chooser);
// ComplexWidget widget = Shuffleboard.getTab("Neo Auto Chooser")
// .add("Command: " + m_choosers.size(), chooser)
// .withSize(4, 1)
// .withPosition(0, m_choosers.size() - 1)
// .withWidget(BuiltInWidgets.kSplitButtonChooser)
// .withWidget(BuiltInWidgets.kComboBoxChooser);
// // m_choosers.add(chooser);
// ComplexWidget widget = Shuffleboard.getTab("Neo Auto Chooser")
// .add("Command: " + m_choosers.size(), chooser)
// .withSize(4, 1)
// .withPosition(0, m_choosers.size() - 1)
// .withWidget(BuiltInWidgets.kSplitButtonChooser)
// .withWidget(BuiltInWidgets.kComboBoxChooser);
// m_widgets.add(widget);
// }
// m_widgets.add(widget);
// }
// // public void nextChooser() {
// // SendableChooser<Command> chooser = m_choosers.get(m_cmdNum++);
// public void nextChooser() {
// SendableChooser<Command> chooser = m_choosers.get(m_cmdNum++);
// // String[] dirs = m_dir.list();
// String[] dirs = m_dir.list();
// // if(dirs != null){ // Fix funny error
// // for (String auto : dirs) {
// // chooser.addOption(auto, new JoystickPlayback(m_swerve, auto));
// // }
// // }
// if(dirs != null){ // Fix funny error
// for (String auto : dirs) {
// chooser.addOption(auto, new JoystickPlayback(m_swerve, auto));
// }
// }
// // for (var cmd_name : m_commandPool.keySet()) {
// // chooser.addOption(cmd_name, m_commandPool.get(cmd_name));
// // }
// // }
// for (var cmd_name : m_commandPool.keySet()) {
// chooser.addOption(cmd_name, m_commandPool.get(cmd_name));
// }
// }
// public String autoName() {
// return m_teamChosser.getSelected() + "_" + m_possitionChosser.getSelected() + "_" + m_autoNameChosser.getSelected() + ".auto";
// }
public String autoName() {
return m_teamChosser.getSelected() + "_" + m_possitionChosser.getSelected() + "_" + m_autoNameChosser.getSelected() + ".auto";
}
// public Command getCommand() {
// return new neoJoystickPlayback(m_swerve, autoName(), m_controllers, true, true);
// }
// }
public Command getCommand() {
return new neoJoystickPlayback(m_swerve, autoName(), m_controllers, true, true);
}
}
@@ -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 = 7;
public static final String GIT_SHA = "24fdd610c9256b4599b7d29ebb1a3acd14cc38b7";
public static final String GIT_DATE = "2026-01-13 18:19:47 MST";
public static final String GIT_BRANCH = "master";
public static final String BUILD_DATE = "2026-01-13 18:58:23 MST";
public static final long BUILD_UNIX_TIME = 1768355903336L;
public static final int GIT_REVISION = 9;
public static final String GIT_SHA = "c68329c9cac4515e1f14398d9ddab5a08b903db5";
public static final String GIT_DATE = "2026-01-18 12:53:07 MST";
public static final String GIT_BRANCH = "Autorotate";
public static final String BUILD_DATE = "2026-01-19 13:36:57 MST";
public static final long BUILD_UNIX_TIME = 1768855017879L;
public static final int DIRTY = 1;
private BuildConstants(){}
@@ -1,14 +1,27 @@
package frc4388.robot.constants;
import java.util.Arrays;
import edu.wpi.first.apriltag.AprilTag;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
public final class FieldConstants {
public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded);
// public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded);
public static final Translation2d BLUE_HUB_POS = new Translation2d();
public static final Translation2d RED_HUB_POS = new Translation2d();
// Test april tag field layout
public static final AprilTagFieldLayout kTagLayout = new AprilTagFieldLayout(
Arrays.asList(new AprilTag[] {
new AprilTag(0, new Pose3d(
new Translation3d(0.,0.,0.26035), new Rotation3d(0.,0.,0.)
)),
}), 100, 100);
}
@@ -289,7 +289,8 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
Translation2d fieldPosLead = robotSpeed.times(SwerveDriveConstants.AIM_LEAD_TIME.get()).plus(fieldPos);
// Calculate the angle between the current position and the lead position
Rotation2d ang = getPose2d().getTranslation().minus(fieldPosLead).getAngle();
Rotation2d ang = fieldPosLead.minus(getPose2d().getTranslation()).getAngle();
driveFieldAngle(leftStick, ang);
}
@@ -164,7 +164,7 @@ public final class SwerveDriveConstants {
// TODO: Replace this with a static constant
public static final ConfigurableDouble AIM_kP = new ConfigurableDouble("Aim kP", 2.5);
public static final ConfigurableDouble AIM_kP = new ConfigurableDouble("Aim kP", 15);
public static final ConfigurableDouble AIM_kI = new ConfigurableDouble("Aim kI", 0);
public static final ConfigurableDouble AIM_kD = new ConfigurableDouble("Aim kD", 0.1);
// public static final Gains AIM_GAINS = new Gains(2.5, 0, 0.1);
@@ -43,6 +43,7 @@ public class Vision extends SubsystemBase implements Queryable {
io[i].updateInputs(state[i]);
Logger.processInputs("Vision/Camera" + i , state[i]);
}
Logger.recordOutput("Vision/isTagDectected", isTag());
}
public List<PoseObservation> getPosesToAdd(){
@@ -45,12 +45,12 @@ public class VisionReal implements VisionIO {
var result = results.get(results.size()-1);
state.isTagDetected = state.isTagDetected | result.hasTargets();
// If there are no tags
if(!result.hasTargets())
return;
state.isTagDetected = state.isTagDetected | result.hasTargets();
Optional<EstimatedRobotPose> estimatedRobotPose = getEstimatedGlobalPose(result, estimator);
// If the tag was failed to be processed
-38
View File
@@ -1,38 +0,0 @@
{
"fileName": "PathplannerLib-2025.2.7.json",
"name": "PathplannerLib",
"version": "2025.2.7",
"uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
"frcYear": "2026",
"mavenUrls": [
"https://3015rangerrobotics.github.io/pathplannerlib/repo"
],
"jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json",
"javaDependencies": [
{
"groupId": "com.pathplanner.lib",
"artifactId": "PathplannerLib-java",
"version": "2025.2.7"
}
],
"jniDependencies": [],
"cppDependencies": [
{
"groupId": "com.pathplanner.lib",
"artifactId": "PathplannerLib-cpp",
"version": "2025.2.7",
"libName": "PathplannerLib",
"headerClassifier": "headers",
"sharedLibrary": false,
"skipInvalidPlatforms": true,
"binaryPlatforms": [
"windowsx86-64",
"linuxx86-64",
"osxuniversal",
"linuxathena",
"linuxarm32",
"linuxarm64"
]
}
]
}
+6 -6
View File
@@ -1,7 +1,7 @@
{
"fileName": "photonlib.json",
"name": "photonlib",
"version": "v2026.0.1-beta",
"version": "v2025.3.1",
"uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004",
"frcYear": "2026",
"mavenUrls": [
@@ -13,7 +13,7 @@
{
"groupId": "org.photonvision",
"artifactId": "photontargeting-cpp",
"version": "v2026.0.1-beta",
"version": "v2025.3.1",
"skipInvalidPlatforms": true,
"isJar": false,
"validPlatforms": [
@@ -28,7 +28,7 @@
{
"groupId": "org.photonvision",
"artifactId": "photonlib-cpp",
"version": "v2026.0.1-beta",
"version": "v2025.3.1",
"libName": "photonlib",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -43,7 +43,7 @@
{
"groupId": "org.photonvision",
"artifactId": "photontargeting-cpp",
"version": "v2026.0.1-beta",
"version": "v2025.3.1",
"libName": "photontargeting",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -60,12 +60,12 @@
{
"groupId": "org.photonvision",
"artifactId": "photonlib-java",
"version": "v2026.0.1-beta"
"version": "v2025.3.1"
},
{
"groupId": "org.photonvision",
"artifactId": "photontargeting-java",
"version": "v2026.0.1-beta"
"version": "v2025.3.1"
}
]
}