mirror of
https://github.com/Team4388/2026KPopRobotHunters.git
synced 2026-06-09 00:38:03 -06:00
Rotate and Autos
This commit is contained in:
Shikhar
@@ -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);
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user