Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 08:48:07 -06:00
Code Issues Packages Projects Releases Wiki Activity

Testing PathPlanner read/write

Browse Source
This commit is contained in:
nathanrsxtn
2022-02-11 18:53:13 -07:00
parent 19d4b2accc
commit a0e7165c25
9 changed files with 389 additions and 138 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Main.java
+8
View File
@@ -5,6 +5,13 @@
package frc4388.robot;
import java.util.logging.Level;
import java.util.logging.Logger;
import com.diffplug.common.base.DurianPlugins;
import com.diffplug.common.base.Errors;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.RobotBase;
import frc4388.utility.AnsiLogging;
@@ -24,6 +31,7 @@ public final class Main {
*/
public static void main(String... args) {
AnsiLogging.systemInstall();
DurianPlugins.register(Errors.Plugins.Log.class, e -> Logger.getLogger(e.getStackTrace()[0].getClassName()).log(Level.SEVERE, e, e::getLocalizedMessage));
RobotBase.startRobot(Robot::new);
}
}
src/main/java/frc4388/robot/Robot.java
+29 -8
View File
@@ -10,10 +10,12 @@ import java.util.logging.Level;
import java.util.logging.Logger;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import frc4388.utility.PathPlannerUtil;
import frc4388.utility.RobotLogger;
import frc4388.utility.RobotTime;
@@ -37,7 +39,7 @@ public class Robot extends TimedRobot {
*/
@Override
public void robotInit() {
if (org.fusesource.jansi.Ansi.isEnabled()) {
// if (org.fusesource.jansi.Ansi.isEnabled()) {
LOGGER.log(Level.ALL, "Logging Test 1/8");
LOGGER.log(Level.SEVERE, "Logging Test 2/8");
LOGGER.log(Level.WARNING, "Logging Test 3/8");
@@ -46,7 +48,10 @@ public class Robot extends TimedRobot {
LOGGER.log(Level.FINE, "Logging Test 6/8");
LOGGER.log(Level.FINER, "Logging Test 7/8");
LOGGER.log(Level.FINEST, "Logging Test 8/8");
}
// }
var path = PathPlannerUtil.Path.read(Filesystem.getDeployDirectory().toPath().resolve("pathplanner").resolve("Move Forward.path").toFile());
LOGGER.finest(path::toString);
LOGGER.fine("robotInit()");
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
// autonomous chooser on the dashboard.
@@ -84,6 +89,22 @@ public class Robot extends TimedRobot {
LOGGER.fine("disabledInit()");
m_robotTime.endMatchTime();
RobotLogger.getInstance().setEnabled(false);
if (isTest()) {
try {
String p = RobotLogger.getInstance().exportPath();
LOGGER.log(Level.WARNING, "----------------------------------------------------------------------");
LOGGER.log(Level.WARNING, "----------------------------------------------------------------------");
LOGGER.log(Level.WARNING, "----------------------------------------------------------------------");
LOGGER.log(Level.WARNING, "----------------------------------------------------------------------");
LOGGER.log(Level.WARNING, "Recorded path to {0} in the deploy directory on the RoboRIO", p);
LOGGER.log(Level.WARNING, "----------------------------------------------------------------------");
LOGGER.log(Level.WARNING, "----------------------------------------------------------------------");
LOGGER.log(Level.WARNING, "----------------------------------------------------------------------");
LOGGER.log(Level.WARNING, "----------------------------------------------------------------------");
} catch (IOException e) {
e.printStackTrace();
}
}
}
@Override
@@ -99,7 +120,12 @@ public class Robot extends TimedRobot {
@Override
public void autonomousInit() {
LOGGER.fine("autonomousInit()");
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
try {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
} catch (IOException e) {
// TODO Auto-generated catch block
e.printStackTrace();
}
/*String autoSelected = SmartDashboard.getString("Auto Selector", "Default");
switch (autoSelected) {
@@ -154,11 +180,6 @@ public class Robot extends TimedRobot {
@Override
public void testInit() {
RobotLogger.getInstance().setEnabled(false);
try {
RobotLogger.getInstance().exportPath();
} catch (IOException e) {
e.printStackTrace();
}
}
/**
src/main/java/frc4388/robot/RobotContainer.java
+56 -96
View File
@@ -4,36 +4,33 @@
package frc4388.robot;
import java.io.File;
import java.io.IOException;
import java.util.Arrays;
import java.util.Comparator;
import java.util.List;
import com.pathplanner.lib.PathPlanner;
import com.pathplanner.lib.PathPlannerTrajectory;
import com.pathplanner.lib.commands.PPSwerveControllerCommand;
import edu.wpi.first.math.controller.HolonomicDriveController;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.ProfiledPIDController;
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.math.trajectory.Trajectory;
import edu.wpi.first.math.trajectory.TrajectoryConfig;
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint;
import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.SwerveControllerCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import frc4388.robot.Constants.*;
import frc4388.robot.Constants.LEDConstants;
import frc4388.robot.Constants.OIConstants;
import frc4388.robot.subsystems.LED;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.utility.LEDPatterns;
import frc4388.utility.PathPlannerTrajectoryUtil;
import frc4388.utility.controller.DeadbandedXboxController;
/**
@@ -49,7 +46,7 @@ public class RobotContainer {
/* Subsystems */
private final SwerveDrive m_robotSwerveDrive = new SwerveDrive(
m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro);
m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro);
private final LED m_robotLED = new LED(m_robotMap.LEDController);
@@ -66,12 +63,12 @@ public class RobotContainer {
// drives the swerve drive with a two-axis input from the driver controller
m_robotSwerveDrive.setDefaultCommand(
new RunCommand(() -> m_robotSwerveDrive.driveWithInput(
getDriverController().getLeftX(),
getDriverController().getLeftY(),
getDriverController().getRightX(),
getDriverController().getRightY(),
true),
m_robotSwerveDrive));
getDriverController().getLeftX(),
getDriverController().getLeftY(),
getDriverController().getRightX(),
getDriverController().getRightY(),
true),
m_robotSwerveDrive));
// continually sends updates to the Blinkin LED controller to keep the lights on
m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED));
@@ -86,107 +83,69 @@ public class RobotContainer {
private void configureButtonBindings() {
/* Driver Buttons */
new JoystickButton(getDriverController(), XboxController.Button.kY.value)
// new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.Y_BUTTON)
.whenPressed(m_robotSwerveDrive.m_gyro::reset);
// new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.Y_BUTTON)
.whenPressed(m_robotSwerveDrive.m_gyro::reset);
new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value)
// new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.LEFT_BUMPER_BUTTON)
.whenPressed(() -> m_robotSwerveDrive.highSpeed(false));
// new XboxControllerRawButton(m_driverXbox,
// XboxControllerRaw.LEFT_BUMPER_BUTTON)
.whenPressed(() -> m_robotSwerveDrive.highSpeed(false));
new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value)
// new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.RIGHT_BUMPER_BUTTON)
.whenPressed(() -> m_robotSwerveDrive.highSpeed(true));
// new XboxControllerRawButton(m_driverXbox,
// XboxControllerRaw.RIGHT_BUMPER_BUTTON)
.whenPressed(() -> m_robotSwerveDrive.highSpeed(true));
new JoystickButton(getDriverController(), XboxController.Button.kA.value)
.whenPressed(() -> zeroOdometry(new Pose2d(0, 0, new Rotation2d(0))));
//.whenPressed(this::resetOdometry);
.whenPressed(() -> zeroOdometry(new Pose2d(0, 0, new Rotation2d(0))));
// .whenPressed(this::resetOdometry);
/* Operator Buttons */
// activates "Lit Mode"
new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
// new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.A_BUTTON)
// new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.A_BUTTON)
.whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
.whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
}
public void configAuto(boolean pathplanner) {
}
public void configAuto() {
configAuto(true);
}
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
* @return the command to run in autonomous
* @throws IOException
*/
public Command getAutonomousCommand() {
public Command getAutonomousCommand() throws IOException {
// https://github.com/mjansen4857/pathplanner/wiki <-- Pathplanner Wiki
TrajectoryConfig config = new TrajectoryConfig(1.0, 1.0)
.setKinematics(m_robotSwerveDrive.m_kinematics);
Trajectory exTraj = TrajectoryGenerator.generateTrajectory(
new Pose2d(0, 0, new Rotation2d(0)),
List.of(new Translation2d(0, 0)),
new Pose2d(1, 0, new Rotation2d(0)),
config);
try (
PIDController xController = new PIDController(10.0, 0.0, 0.0);
PIDController yController = new PIDController(1.3, 0.0, 0.0)) {
ProfiledPIDController thetaController = new ProfiledPIDController(
10.0, 0.0, 0.0, new TrapezoidProfile.Constraints(2 * Math.PI, Math.PI));
thetaController.enableContinuousInput(-Math.PI, Math.PI);
PathPlannerTrajectory ppRecorded = PathPlannerTrajectoryUtil
.fromPathweaverJson(Arrays.stream(Filesystem.getDeployDirectory().listFiles())
.max(Comparator.comparingLong(File::lastModified)).orElseThrow().toPath(), 1.0, 1.0);
Trajectory firstTestPath = PathPlanner.loadPath("First Test Path", 1.0, 1.0);
Trajectory moveForward = PathPlanner.loadPath("Move Forward", 1.0, 1.0);
Trajectory rotate = PathPlanner.loadPath("Rotate", 1.0, 1.0);
PathPlannerTrajectory ppCurrentPath = ppRecorded; // change this to change auto
Trajectory currentPath = moveForward; // change this to change auto
PPSwerveControllerCommand ppSwerveControllerCommand = new PPSwerveControllerCommand(
ppCurrentPath,
m_robotSwerveDrive::getOdometry,
m_robotSwerveDrive.m_kinematics,
xController,
yController,
thetaController,
m_robotSwerveDrive::setModuleStates,
m_robotSwerveDrive);
PIDController xController = new PIDController(10.0, 0.0, 0.0);
PIDController yController = new PIDController(1.3, 0.0, 0.0);
ProfiledPIDController thetaController = new ProfiledPIDController(
10.0, 0.0, 0.0, new TrapezoidProfile.Constraints(2 * Math.PI, Math.PI));
thetaController.enableContinuousInput(-Math.PI, Math.PI);
SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand(
currentPath,
m_robotSwerveDrive::getOdometry,
m_robotSwerveDrive.m_kinematics,
xController,
yController,
thetaController,
m_robotSwerveDrive::setModuleStates,
m_robotSwerveDrive
);
PathPlannerTrajectory ppfirstTestPath = PathPlanner.loadPath("First Test Path", 4.0, 4.0);
PathPlannerTrajectory ppMoveForward = PathPlanner.loadPath("Move Forward", 1.0, 1.0);
PathPlannerTrajectory ppRotate = PathPlanner.loadPath("Rotate", 1.0, 1.0);
PathPlannerTrajectory ppCurrentPath = ppfirstTestPath; // change this to change auto
PPSwerveControllerCommand ppSwerveControllerCommand = new PPSwerveControllerCommand(
ppCurrentPath,
m_robotSwerveDrive::getOdometry,
m_robotSwerveDrive.m_kinematics,
xController,
yController,
thetaController,
m_robotSwerveDrive::setModuleStates,
m_robotSwerveDrive
);
// return new SequentialCommandGroup(
// new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(currentPath.getInitialPose())),
// swerveControllerCommand,
// new InstantCommand(() -> m_robotSwerveDrive.stopModules())
// );
return new SequentialCommandGroup(
new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset()),
new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(ppCurrentPath.getInitialPose())),
ppSwerveControllerCommand,
new InstantCommand(() -> m_robotSwerveDrive.stopModules())
);
return new SequentialCommandGroup(
new InstantCommand(m_robotSwerveDrive.m_gyro::reset),
new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(ppCurrentPath.getInitialPose())),
ppSwerveControllerCommand,
new InstantCommand(m_robotSwerveDrive::stopModules));
}
}
/**
@@ -203,6 +162,7 @@ public class RobotContainer {
public void zeroOdometry(Pose2d pose) {
m_robotSwerveDrive.resetOdometry(pose);
}
/**
* Add your docs here.
*/
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+20 -4
View File
@@ -122,6 +122,7 @@ public class SwerveDrive extends SubsystemBase {
setModuleStates(states);
}
private Rotation2d rotTarget = new Rotation2d();
private ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative)
{
ignoreAngles = leftX == 0 && leftY == 0 && rightX == 0 && rightY == 0;
@@ -132,11 +133,12 @@ public class SwerveDrive extends SubsystemBase {
double rot = rotTarget.minus(m_gyro.getRotation2d()).getRadians();
double xSpeedMetersPerSecond = -speed.getX();
double ySpeedMetersPerSecond = speed.getY();
chassisSpeeds = fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d())
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED);
SwerveModuleState[] states =
m_kinematics.toSwerveModuleStates(
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d())
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED));
chassisSpeeds);
setModuleStates(states);
}
@@ -155,7 +157,21 @@ public class SwerveDrive extends SubsystemBase {
updateOdometry();
// m_gyro.setFusedHeadingToCompass();
// m_gyro.setYawToCompass();
RobotLogger.getInstance().put("poseMeters", m_poseEstimator.getEstimatedPosition());
{
// double velocityMetersPerSecond = new Translation2d(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond).getNorm();
double velocityMetersPerSecond = chassisSpeeds.vxMetersPerSecond;
Pose2d poseMeters = m_poseEstimator.getEstimatedPosition();
double curvatureRadPerMeter = 0;
RobotLogger.getInstance().put(velocityMetersPerSecond, poseMeters, curvatureRadPerMeter, 0, new Rotation2d(), new Rotation2d(), new Rotation2d());
}
SmartDashboard.putNumber("Pigeon Fused Heading", m_gyro.getFusedHeading(fstatus));
SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw());
SmartDashboard.putNumber("Pigeon Get Angle", m_gyro.getAngle());