From a9e45fcf9ba356f63af7c567d51115d255bb269b Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Sun, 15 Dec 2024 23:39:59 -0800 Subject: [PATCH] Actually make pathplanner work --- .../deploy/pathplanner/paths/New Path.path | 34 +++++++++---- src/main/java/frc4388/robot/Constants.java | 48 ++++++++++++++++-- .../java/frc4388/robot/RobotContainer.java | 2 + .../robot/subsystems/RobotLocalizer.java | 32 ++++++------ .../frc4388/robot/subsystems/TankDrive.java | 49 ++++++++++++++----- src/main/java/frc4388/utility/RobotGyro.java | 4 ++ vendordeps/PathplannerLib2024.json | 12 ++--- 7 files changed, 133 insertions(+), 48 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path index 6441c8a..efc54d4 100644 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -3,29 +3,45 @@ "waypoints": [ { "anchor": { - "x": 13.882261436839258, - "y": 3.061093579814278 + "x": 13.16336824764045, + "y": 1.4493814298363055 }, "prevControl": null, "nextControl": { - "x": 14.693915037547589, - "y": 2.6668618308988026 + "x": 14.079377311296996, + "y": 1.379811121204163 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 14.728700191862979, - "y": 1.2522655553831117 + "x": 13.16336824764045, + "y": 2.6204816251440413 }, "prevControl": { - "x": 13.928641642594021, - "y": 1.3218358640107113 + "x": 14.160542671367828, + "y": 2.550911316511899 + }, + "nextControl": { + "x": 12.223879630793721, + "y": 2.686027342598464 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 13.16336824764045, + "y": 1.4493814298363055 + }, + "prevControl": { + "x": 12.200978978229143, + "y": 1.518951738468449 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "mid" } ], "rotationTargets": [], diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 56cdaf1..ee735f9 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -7,7 +7,15 @@ package frc4388.robot; -import com.pathplanner.lib.util.ReplanningConfig; +import com.pathplanner.lib.config.ModuleConfig; +import com.pathplanner.lib.config.RobotConfig; +import com.pathplanner.lib.controllers.PPLTVController; +import com.pathplanner.lib.controllers.PathFollowingController; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.system.plant.DCMotor; + +// import com.pathplanner.l; import edu.wpi.first.math.trajectory.TrapezoidProfile; import frc4388.utility.Gains; @@ -91,7 +99,41 @@ public final class Constants { public static final double PATH_MAX_VEL = 0.3; // TODO: find the actual value public static final double PATH_MAX_ACC = 0.3; // TODO: find the actual value - public static final ReplanningConfig replanningConfig = new ReplanningConfig(); + public static final double MASS = 10; // KG + public static final double MOI = 10; // Moment of inertia + + public static final double WHEEL_RADIUS = 0.05; // Meters + public static final double MAX_DRIVE_VELOCITY = 0.5; // Meters per second + public static final double MAX_ROT_SPEED = 0.2; // Rotations per second + + + public static final double COEFFICENT_OF_FRICTION = 1.0; // Between 0 and 1 + public static final DCMotor TALON_SRX_MOTOR = DCMotor.getVex775Pro(1); //TODO: Get actual motor constants + public static final double DRIVE_CURRENT_LIMIT = 100000; //TODO: Get actual value + + + public static final ModuleConfig MODULE_CONFIG = new ModuleConfig( + WHEEL_RADIUS, + MAX_DRIVE_VELOCITY, + COEFFICENT_OF_FRICTION, + TALON_SRX_MOTOR, + DRIVE_CURRENT_LIMIT, + 2); + + public static final RobotConfig PP_ROBOT_CONFIG = new RobotConfig( + MASS, + MOI, + MODULE_CONFIG, + new Translation2d[] { + new Translation2d(), + new Translation2d(), + new Translation2d(), + new Translation2d(), + }); + + + public static final double ROBOT_LOOP_TIME = 0.02; // Time it takes for the robot to run a loop + public static final PathFollowingController PP_PATH_FOLLOWING_CONTROLLER = new PPLTVController(ROBOT_LOOP_TIME); } public static final class Conversions { @@ -135,7 +177,7 @@ public final class Constants { } public static final class VisionConstants { - public static final String CAMERA_NAME = "photonvision"; + public static final String CAMERA_NAME = "Camera_Module_v1"; } public static final class DriveConstants { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 4ff2cfa..cf83f7f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -14,6 +14,7 @@ import com.pathplanner.lib.commands.PathPlannerAuto; import edu.wpi.first.wpilibj.DriverStation; 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; @@ -133,6 +134,7 @@ public class RobotContainer { autoChooser = AutoBuilder.buildAutoChooser(); + SmartDashboard.putData("Auto Chooser", autoChooser); } /** diff --git a/src/main/java/frc4388/robot/subsystems/RobotLocalizer.java b/src/main/java/frc4388/robot/subsystems/RobotLocalizer.java index 3a8ea82..7e884c2 100644 --- a/src/main/java/frc4388/robot/subsystems/RobotLocalizer.java +++ b/src/main/java/frc4388/robot/subsystems/RobotLocalizer.java @@ -4,6 +4,7 @@ import org.photonvision.PhotonCamera; import org.photonvision.targeting.PhotonTrackedTarget; import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.util.DriveFeedforwards; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation3d; @@ -11,6 +12,7 @@ import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.utility.RobotGyro; @@ -29,17 +31,17 @@ public class RobotLocalizer extends SubsystemBase { @Override public void periodic() { // time - // Translation3d accel = gyro.getAcceleration(); + Translation3d accel = gyro.getAcceleration3d(); - // SmartDashboard.putNumber("Accel X", accel.getX()); - // SmartDashboard.putNumber("Accel Y", accel.getY()); - // SmartDashboard.putNumber("Accel Z", accel.getZ()); + SmartDashboard.putNumber("Accel X", accel.getX()); + SmartDashboard.putNumber("Accel Y", accel.getY()); + SmartDashboard.putNumber("Accel Z", accel.getZ()); - // Rotation3d rot = gyro.getRotation3d(); + Rotation3d rot = gyro.getRotation3d(); - // SmartDashboard.putNumber("Rot X", rot.getX()); - // SmartDashboard.putNumber("Rot Y", rot.getY()); - // SmartDashboard.putNumber("Rot Z", rot.getZ()); + SmartDashboard.putNumber("Rot X", rot.getX()); + SmartDashboard.putNumber("Rot Y", rot.getY()); + SmartDashboard.putNumber("Rot Z", rot.getZ()); // boolean tagExists = SmartDashboard.getBoolean("photonvision/Camera_Module_v1/hasTarget", false); @@ -95,14 +97,10 @@ public class RobotLocalizer extends SubsystemBase { // PathPlanner public ChassisSpeeds getChassisSpeeds() { - return new ChassisSpeeds(); + return new ChassisSpeeds( + 0, + 0, + Units.rotationsToRadians(gyro.getAngularVelocity()) + ); } - - // PathPlanner - public void setChassisSpeeds(ChassisSpeeds target) { - System.out.println(target); - } - - - } diff --git a/src/main/java/frc4388/robot/subsystems/TankDrive.java b/src/main/java/frc4388/robot/subsystems/TankDrive.java index 069cba3..65efd06 100755 --- a/src/main/java/frc4388/robot/subsystems/TankDrive.java +++ b/src/main/java/frc4388/robot/subsystems/TankDrive.java @@ -8,8 +8,11 @@ import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.controllers.PPLTVController; import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.util.DriveFeedforwards; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -32,18 +35,23 @@ public class TankDrive extends SubsystemBase{ // Configure AutoBuilder last - AutoBuilder.configureLTV( + AutoBuilder.configure( robotLocalizer::getPose, robotLocalizer::resetPose, robotLocalizer::getChassisSpeeds, - robotLocalizer::setChassisSpeeds, - 0.02, // Default loop time - AutoConstants.replanningConfig, - new BooleanSupplier() { - @Override - public boolean getAsBoolean() { - return false; + this::setTargetChassisSpeeds, + AutoConstants.PP_PATH_FOLLOWING_CONTROLLER, + AutoConstants.PP_ROBOT_CONFIG, + () -> { + // Boolean supplier that controls when the path will be mirrored for the red alliance + // This will flip the path being followed to the red side of the field. + // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; } + return false; }, this ); @@ -51,14 +59,14 @@ public class TankDrive extends SubsystemBase{ private static final ControlMode mode = ControlMode.PercentOutput; - private static final double maxMoveSpeed = 0.6; - private static final double maxturnSpeed = 0.6; + private static final double maxMoveSpeed = 0.3; + private static final double maxturnSpeed = 0.3; private static final double[] turn = { //Right by default, motors are wired weirdly maxturnSpeed, //FR - maxturnSpeed, //FL - maxturnSpeed, //BL + maxturnSpeed, //FL + maxturnSpeed, //BL maxturnSpeed, //BR }; @@ -71,7 +79,7 @@ public class TankDrive extends SubsystemBase{ public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { - double percent_move = (1. - Math.abs(rightStick.getX())) * leftStick.getY(); + double percent_move = /* (1. - Math.abs(rightStick.getX())) */ leftStick.getY(); double FR_rot = move[0] * percent_move + turn[0] * rightStick.getX(); double FL_rot = move[1] * percent_move + turn[1] * rightStick.getX(); @@ -89,4 +97,19 @@ public class TankDrive extends SubsystemBase{ SmartDashboard.putNumber("BR", BR_rot); } + + private double clamp(double x, double min, double max){ + return Math.max(Math.min(x, max), min); + } + + private void setTargetChassisSpeeds(ChassisSpeeds target, DriveFeedforwards idk) { + double move = clamp(target.vxMetersPerSecond/AutoConstants.MAX_DRIVE_VELOCITY, -1, 1); + double rot = clamp(Units.radiansToRotations(target.omegaRadiansPerSecond)/AutoConstants.MAX_ROT_SPEED, -1, 1); + + driveWithInput( + new Translation2d(0, move), + new Translation2d(rot, 0), + false); + + } } diff --git a/src/main/java/frc4388/utility/RobotGyro.java b/src/main/java/frc4388/utility/RobotGyro.java index 87fa8aa..d0d65fd 100644 --- a/src/main/java/frc4388/utility/RobotGyro.java +++ b/src/main/java/frc4388/utility/RobotGyro.java @@ -211,6 +211,10 @@ public class RobotGyro { m_pigeon.getAccelerationY().getValue().baseUnitMagnitude(), m_pigeon.getAccelerationZ().getValue().baseUnitMagnitude()); } + + public double getAngularVelocity() { + return m_pigeon.getAngularVelocityYDevice().getValueAsDouble(); + } public double getAngle() { // if (m_isGyroAPigeon) { diff --git a/vendordeps/PathplannerLib2024.json b/vendordeps/PathplannerLib2024.json index f3dc687..e79fe1a 100644 --- a/vendordeps/PathplannerLib2024.json +++ b/vendordeps/PathplannerLib2024.json @@ -1,18 +1,18 @@ { - "fileName": "PathplannerLib.json", + "fileName": "PathplannerLib-beta.json", "name": "PathplannerLib", - "version": "2024.2.8", + "version": "2025.0.0-beta-5", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2025", "mavenUrls": [ "https://3015rangerrobotics.github.io/pathplannerlib/repo" ], - "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", + "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib-beta.json", "javaDependencies": [ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.2.8" + "version": "2025.0.0-beta-5" } ], "jniDependencies": [], @@ -20,15 +20,15 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.2.8", + "version": "2025.0.0-beta-5", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", - "linuxx86-64", "osxuniversal", + "linuxx86-64", "linuxathena", "linuxarm32", "linuxarm64"