astatin3/Vision-Minibot
1
0
Fork 0
mirror of https://github.com/Astatin3/Vision-Minibot.git synced 2026-06-08 16:18:02 -06:00
Code Issues Packages Projects Releases Wiki Activity

Actually make pathplanner work

Browse Source
This commit is contained in:
Michael Mikovsky
2024-12-15 23:39:59 -08:00
parent 219ab47f71
commit a9e45fcf9b
7 changed files with 133 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/deploy/pathplanner/paths/New Path.path
+25 -9
View File
@@ -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": [],
src/main/java/frc4388/robot/Constants.java
+45 -3
View File
@@ -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 {
src/main/java/frc4388/robot/RobotContainer.java
+2
View File
@@ -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);
}
/**
src/main/java/frc4388/robot/subsystems/RobotLocalizer.java
+15 -17
View File
@@ -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);
}
}
src/main/java/frc4388/robot/subsystems/TankDrive.java
+36 -13
View File
@@ -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);
}
}
src/main/java/frc4388/utility/RobotGyro.java
+4
View File
@@ -212,6 +212,10 @@ public class RobotGyro {
m_pigeon.getAccelerationZ().getValue().baseUnitMagnitude());
}
public double getAngularVelocity() {
return m_pigeon.getAngularVelocityYDevice().getValueAsDouble();
}
public double getAngle() {
// if (m_isGyroAPigeon) {
return getPigeonAngles()[2];
vendordeps/PathplannerLib2024.json
+6 -6
View File
@@ -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"