diff --git a/src/main/deploy/pathplanner/paths/HP to 330 Right.path b/src/main/deploy/pathplanner/paths/HP to 330 Right.path index 6428ad8..d2a38f9 100644 --- a/src/main/deploy/pathplanner/paths/HP to 330 Right.path +++ b/src/main/deploy/pathplanner/paths/HP to 330 Right.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 3.6442622950819668, - "y": 5.031967213114754 + "x": 3.609900518622585, + "y": 5.005924534374863 }, "prevControl": { - "x": 3.328722911520323, - "y": 5.557866185717494 + "x": 3.2943611350609414, + "y": 5.531823506977602 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/L Backup.path b/src/main/deploy/pathplanner/paths/L Backup.path index 3cdebd2..88019ff 100644 --- a/src/main/deploy/pathplanner/paths/L Backup.path +++ b/src/main/deploy/pathplanner/paths/L Backup.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.6442622950819668, - "y": 5.031967213114754 + "x": 3.609900518622585, + "y": 5.005924534374863 }, "prevControl": null, "nextControl": { - "x": 3.5117625600811717, - "y": 5.243966789116026 + "x": 3.47740078362179, + "y": 5.217924110376135 }, "isLocked": false, "linkedName": "L" diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index e33e033..89af11d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -7,7 +7,7 @@ package frc.robot; import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.Logger; -//import org.littletonrobotics.junction.networktables.NT4Publisher; +import org.littletonrobotics.junction.networktables.NT4Publisher; import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index adb4b01..eaafb81 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -113,7 +113,7 @@ public class RobotContainer { drivetrain.drive( () -> driver.getLeftY() * Math.pow(Math.abs(driver.getLeftY()) + Math.abs(driver.getLeftX()), 3), () -> driver.getLeftX() * Math.pow(Math.abs(driver.getLeftY()) + Math.abs(driver.getLeftX()), 3), - driver::getRightX, //Math.signum(driver.getRightX()) * Math.pow(driver.getRightX(), 3) + () -> driver.getRightX(), () -> true ) ); diff --git a/src/main/java/frc/robot/constants/AutoConstants.java b/src/main/java/frc/robot/constants/AutoConstants.java index 0ba9841..d28a2c1 100644 --- a/src/main/java/frc/robot/constants/AutoConstants.java +++ b/src/main/java/frc/robot/constants/AutoConstants.java @@ -16,6 +16,8 @@ public class AutoConstants { public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI; public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI; + public static final double kMaxSpeedMetersPerSecondAutoAlign = 2.5; + public static final double kPXYController = 3.5; public static final double kPThetaController = 5; diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 6b6ff78..89adee5 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -362,8 +362,8 @@ public class Drivetrain extends SubsystemBase { ); double speed = Math.hypot(controlEffort.vxMetersPerSecond, controlEffort.vyMetersPerSecond); - if (speed > AutoConstants.kMaxSpeedMetersPerSecond) { - double mul = AutoConstants.kMaxSpeedMetersPerSecond / speed; + if (speed > AutoConstants.kMaxSpeedMetersPerSecondAutoAlign) { + double mul = AutoConstants.kMaxSpeedMetersPerSecondAutoAlign / speed; controlEffort.vxMetersPerSecond *= mul; controlEffort.vyMetersPerSecond *= mul; }