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

make those autos

Browse Source
This commit is contained in:
Michael Mikovsky
2025-01-23 19:32:12 -07:00
parent 52d7659f3e
commit 4a45243828
26 changed files with 1137 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+1 -1
View File
@@ -341,7 +341,7 @@ public final class Constants {
public static final class VisionConstants {
public static final String CAMERA_NAME = "Camera_Module_v1";
public static final Transform3d CAMERA_POS = new Transform3d(new Translation3d(-.3048, 0.2413*0, .2794), new Rotation3d(0,0.52333,Math.PI));
public static final Transform3d CAMERA_POS = new Transform3d(new Translation3d(-.3048, 0.2413, .2794), new Rotation3d(0,0.52333,Math.PI));
public static final double MIN_ESTIMATION_DISTANCE = 1; // Meters
src/main/java/frc4388/robot/Robot.java
+1
View File
@@ -148,6 +148,7 @@ public class Robot extends TimedRobot {
// continue until interrupted by another command, remove
// this line or comment it out.
if (m_autonomousCommand != null) {
CommandScheduler.getInstance().cancel(m_autonomousCommand);
m_autonomousCommand.cancel();
}
m_robotTime.startMatchTime();
src/main/java/frc4388/robot/RobotContainer.java
+17 -8
View File
@@ -16,6 +16,8 @@ import java.util.List;
import edu.wpi.first.cameraserver.CameraServer;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc4388.utility.controller.XboxController;
import frc4388.utility.controller.DeadbandedXboxController;
import frc4388.robot.Constants.OIConstants;
@@ -36,6 +38,8 @@ import frc4388.robot.commands.GotoPositionCommand;
import frc4388.robot.commands.Swerve.neoJoystickPlayback;
import frc4388.robot.commands.Swerve.neoJoystickRecorder;
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.auto.NamedCommands;
import com.pathplanner.lib.commands.PathPlannerAuto;
// Subsystems
@@ -89,6 +93,7 @@ public class RobotContainer {
// ! /* Autos */
private String lastAutoName = "defualt.auto";
private final SendableChooser<Command> autoChooser;
private ConfigurableString autoplaybackName = new ConfigurableString("Auto Playback Name", lastAutoName);
private neoJoystickPlayback autoPlayback = new neoJoystickPlayback(m_robotSwerveDrive,
() -> autoplaybackName.get(), // lastAutoName
@@ -120,6 +125,8 @@ public class RobotContainer {
.withName("SwerveDrive DefaultCommand"));
m_robotSwerveDrive.setToSlow();
autoChooser = AutoBuilder.buildAutoChooser();
SmartDashboard.putData("Auto Chooser", autoChooser);
// this.subsystems.add(m_robotSwerveDrive);
// this.subsystems.add(m_robotMap.leftFront);
// this.subsystems.add(m_robotMap.rightFront);
@@ -270,15 +277,17 @@ public class RobotContainer {
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
//return autoPlayback;
//return new GotoPositionCommand(m_robotSwerveDrive, m_vision);
try{
// Load the path you want to follow using its name in the GUI
return new PathPlannerAuto("New Auto");
} catch (Exception e) {
DriverStation.reportError("Path planner error: " + e.getMessage(), e.getStackTrace());
return Commands.none();
}
//return new GotoPositionCommand(m_robotSwerveDrive, m_vision)
return autoChooser.getSelected();
// try{
// // Load the path you want to follow using its name in the GUI
// return new PathPlannerAuto("New Auto");
// } catch (Exception e) {
// DriverStation.reportError("Path planner error: " + e.getMessage(), e.getStackTrace());
// return Commands.none();
// }
// zach told me to do the below comment
//return new GotoPositionCommand(m_robotSwerveDrive, m_vision);
// return new GotoPositionCommand(m_robotSwerveDrive, m_vision, AutoConstants.targetpos);
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+10 -4
View File
@@ -32,6 +32,7 @@ import frc4388.utility.Status;
import frc4388.utility.Subsystem;
import frc4388.utility.Status.ReportLevel;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import com.pathplanner.lib.controllers.PPHolonomicDriveController;
import com.pathplanner.lib.config.PIDConstants;
@@ -52,6 +53,8 @@ public class SwerveDrive extends Subsystem {
public double autoSpeedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to
// 25%
public Pose2d initalPose2d = null;
public double rotTarget = 0.0;
public Rotation2d orientRotTarget = new Rotation2d();
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
@@ -75,11 +78,9 @@ public class SwerveDrive extends Subsystem {
// DoubleSupplier a = () -> 1.d;
AutoBuilder.configure(
() -> {
var pose = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d());
// pose = new Pose2d(pose.getX(), pose.getY(), pose.getRotation().times(-1));
return pose;// getRotation().times(-1)
return swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(initalPose2d);
}, // Robot pose supplier
swerveDriveTrain::resetPose, // Method to reset odometry (will be called if your auto has a starting
this::setOdoPose, // Method to reset odometry (will be called if your auto has a starting
// pose)
() -> swerveDriveTrain.getState().Speeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
(speeds, feedforwards) -> swerveDriveTrain.setControl(new SwerveRequest.ApplyRobotSpeeds()
@@ -108,6 +109,11 @@ public class SwerveDrive extends Subsystem {
}
public void setOdoPose(Pose2d pose) {
initalPose2d = pose;
swerveDriveTrain.resetPose(pose);
}
// public void oneModuleTest(SwerveModule module, Translation2d leftStick,
// Translation2d rightStick){
// // double ang = Math.atan2(rightStick.getY(), rightStick.getX());