diff --git a/simgui-ds.json b/simgui-ds.json index 73cc713..4a63cc1 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,4 +1,9 @@ { + "System Joysticks": { + "window": { + "enabled": false + } + }, "keyboardJoysticks": [ { "axisConfig": [ diff --git a/src/main/deploy/pathplanner/paths/TurnNinety.path b/src/main/deploy/pathplanner/paths/TurnNinety.path index 41bceba..b8ac2c4 100644 --- a/src/main/deploy/pathplanner/paths/TurnNinety.path +++ b/src/main/deploy/pathplanner/paths/TurnNinety.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.42775, - "y": 4.0055 + "x": 3.0, + "y": 4.0 }, "prevControl": null, "nextControl": { - "x": 3.4277499999999996, - "y": 4.0055 + "x": 4.0, + "y": 4.0 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.42775, - "y": 4.0055 + "x": 3.0, + "y": 4.0 }, "prevControl": { - "x": 1.42775, - "y": 4.0055 + "x": 2.0, + "y": 4.0 }, "nextControl": null, "isLocked": false, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": 90.0 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -90.48219994972354 + "rotation": 0.0 }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index f565921..2604350 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -10,7 +10,9 @@ package frc4388.robot; import java.io.File; import com.pathplanner.lib.commands.PathPlannerAuto; +import com.pathplanner.lib.trajectory.PathPlannerTrajectory; +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; @@ -353,16 +355,21 @@ public class RobotContainer { autoChooser.onChange((filename) -> { autoChooserUpdated = true; - if (filename.equals("Taxi")) { - autoCommand = new SequentialCommandGroup( - new MoveForTimeCommand(m_robotSwerveDrive, - new Translation2d(0, -1), - new Translation2d(), 1000, true - ), new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive)); - } else { + // if (filename.equals("Taxi")) { + // autoCommand = new SequentialCommandGroup( + // new MoveForTimeCommand(m_robotSwerveDrive, + // new Translation2d(0, -1), + // new Translation2d(), 1000, true + // ), new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive)); + // } else { autoCommand = new PathPlannerAuto(filename); - } + // } System.out.println("Robot Auto Changed " + filename); + + //---- + PathPlannerAuto auto = new PathPlannerAuto(filename); + m_robotSwerveDrive.setInitalPose(auto.getStartingPose()); + //----- }); SmartDashboard.putData(autoChooser); diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index 9e0b918..228429e 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -14,6 +14,7 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import com.pathplanner.lib.trajectory.PathPlannerTrajectory; import com.pathplanner.lib.util.PathPlannerLogging; import edu.wpi.first.math.geometry.Pose2d; @@ -21,6 +22,7 @@ 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.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.constants.Constants.AutoConstants; import frc4388.robot.subsystems.vision.Vision; @@ -50,7 +52,8 @@ public class SwerveDrive extends SubsystemBase implements Queryable { public double lastOdomSpeed; - public Pose2d initalPose2d = new Pose2d(); // Mira aqui + public Pose2d initalPose2d = new Pose2d(); + public double rotTarget = 0.0; @@ -136,7 +139,11 @@ public class SwerveDrive extends SubsystemBase implements Queryable { public void setOdoPose(Pose2d pose) { if (pose == null) return; initalPose2d = pose; - io.resetPose(pose); + io.resetPose(initalPose2d); + } + + public void setInitalPose(Pose2d startingAutoPose){ + initalPose2d = startingAutoPose; } // MIRA public void setOdoPose(Pose2d pose) { // if (pose == null) return; diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java index 05d9e8f..c4ab4d6 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java @@ -10,6 +10,7 @@ import com.ctre.phoenix6.swerve.SwerveDrivetrain; import com.ctre.phoenix6.swerve.SwerveModule; import com.ctre.phoenix6.swerve.SwerveRequest; +import edu.wpi.first.math.geometry.Pose2d; import frc4388.robot.subsystems.vision.Vision; import frc4388.robot.subsystems.vision.VisionIO.PoseObservation; @@ -40,6 +41,20 @@ public class SwerveReal implements SwerveIO { swerveDriveTrain.tareEverything(); } + @Override + public void resetPose(Pose2d pose) { + if (pose == null) return; + try { + // Preferred: ask the drivetrain to reset its odometry directly + System.out.println("!"+pose); + swerveDriveTrain.resetPose(pose); + } catch (NoSuchMethodError | RuntimeException e) { + // Fallback: tare sensors then add a timed vision measurement so odometry is seeded + swerveDriveTrain.tareEverything(); + swerveDriveTrain.addVisionMeasurement(pose, Utils.fpgaToCurrentTime(Vision.getTime())); + } + } + @Override public void setLimits(double limitInAmps) { for (SwerveModule module : swerveDriveTrain.getModules()) {