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

Implement working PathPlanner I/O

Browse Source
This commit is contained in:
nathanrsxtn
2022-02-15 11:05:59 -07:00
parent 2dba3604e9
commit 3cb4a13bb1
9 changed files with 94 additions and 297 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Main.java
-1
View File
@@ -11,7 +11,6 @@ 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;
src/main/java/frc4388/robot/Robot.java
+15 -8
View File
@@ -4,11 +4,13 @@
package frc4388.robot;
import java.io.File;
import java.io.IOException;
import java.util.logging.Level;
import java.util.logging.Logger;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj.TimedRobot;
@@ -18,6 +20,7 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler;
import frc4388.utility.PathPlannerUtil;
import frc4388.utility.RobotLogger;
import frc4388.utility.RobotTime;
import frc4388.utility.PathPlannerUtil.Path;
/**
* The VM is configured to automatically run this class, and to call the
@@ -50,9 +53,11 @@ public class Robot extends TimedRobot {
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);
// var path = PathPlannerUtil.Path.read(Filesystem.getDeployDirectory().toPath().resolve("pathplanner").resolve("Move Forward.path").toFile());
// LOGGER.finest(path::toString);
LOGGER.fine("robotInit()");
LOGGER.fine("Sssssssssh.");
DriverStation.silenceJoystickConnectionWarning(true);
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
// autonomous chooser on the dashboard.
m_robotContainer = new RobotContainer();
@@ -88,19 +93,24 @@ public class Robot extends TimedRobot {
public void disabledInit() {
LOGGER.fine("disabledInit()");
m_robotTime.endMatchTime();
RobotLogger.getInstance().setEnabled(false);
if (isTest()) {
try {
String p = RobotLogger.getInstance().exportPath();
// IMPORTANT: Had to chown the pathplanner folder in order to save autos.
Path path = RobotLogger.getInstance().createPath(0.1, 0.1, false);
String pathPath = "recording." + System.currentTimeMillis() + ".path";
File outputFile = Filesystem.getDeployDirectory().toPath().resolve("pathplanner").resolve(pathPath).toFile();
outputFile.createNewFile();
path.write(outputFile);
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, "Recorded path to {0} in the deploy directory on the RoboRIO", pathPath);
LOGGER.log(Level.WARNING, "----------------------------------------------------------------------");
LOGGER.log(Level.WARNING, "----------------------------------------------------------------------");
LOGGER.log(Level.WARNING, "----------------------------------------------------------------------");
LOGGER.log(Level.WARNING, "----------------------------------------------------------------------");
// LOGGER.finest(path::toString);
} catch (IOException e) {
e.printStackTrace();
}
@@ -132,7 +142,6 @@ public class Robot extends TimedRobot {
m_autonomousCommand.schedule();
}
m_robotTime.startMatchTime();
RobotLogger.getInstance().setEnabled(false);
}
/**
@@ -153,7 +162,6 @@ public class Robot extends TimedRobot {
m_autonomousCommand.cancel();
}
m_robotTime.startMatchTime();
RobotLogger.getInstance().setEnabled(true);
DriverStation.silenceJoystickConnectionWarning(true);
}
@@ -168,7 +176,6 @@ public class Robot extends TimedRobot {
@Override
public void testInit() {
RobotLogger.getInstance().setEnabled(false);
}
/**
src/main/java/frc4388/robot/RobotContainer.java
+25 -23
View File
@@ -4,8 +4,13 @@
package frc4388.robot;
import java.io.File;
import java.io.IOException;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Comparator;
import java.util.Optional;
import java.util.logging.Logger;
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
import com.ctre.phoenix.motorcontrol.can.TalonFX;
@@ -18,11 +23,12 @@ 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.wpilibj.Filesystem;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.command.PrintCommand;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
@@ -113,8 +119,7 @@ public class RobotContainer {
}
public Command[] buildAuto(double maxVel, double maxAccel, Object... inputs) {
ArrayList<Command> commands = new ArrayList<Command>();
ArrayList<Command> commands = new ArrayList<>();
commands.add(new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset()));
PIDController xController = SwerveDriveConstants.X_CONTROLLER;
@@ -123,30 +128,18 @@ public class RobotContainer {
thetaController.enableContinuousInput(-Math.PI, Math.PI);
// parse input
for (int i=0; i<inputs.length; i++) {
for (int i = 0; i < inputs.length; i++) {
if (inputs[i] instanceof String) {
PathPlannerTrajectory traj = PathPlanner.loadPath(inputs[i].toString(), maxVel, maxAccel);
PathPlannerState initState = (PathPlannerState) traj.sample(0);
commands.add(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(new Pose2d(initState.poseMeters.getTranslation(), initState.holonomicRotation))));
commands.add(new PPSwerveControllerCommand(
traj,
m_robotSwerveDrive::getOdometry,
m_robotSwerveDrive.m_kinematics,
xController,
yController,
thetaController,
m_robotSwerveDrive::setModuleStates,
m_robotSwerveDrive));
}
if (inputs[i] instanceof Command) {
commands.add(new PPSwerveControllerCommand(traj, m_robotSwerveDrive::getOdometry, m_robotSwerveDrive.m_kinematics, xController, yController, thetaController, m_robotSwerveDrive::setModuleStates, m_robotSwerveDrive));
} else if (inputs[i] instanceof Command)
commands.add((Command) inputs[i]);
}
}
commands.add(new InstantCommand(() -> m_robotSwerveDrive.stopModules()));
commands.add(new InstantCommand(m_robotSwerveDrive::stopModules));
Command[] ret = new Command[commands.size()];
ret = commands.toArray(ret);
return ret;
@@ -161,9 +154,18 @@ public class RobotContainer {
public Command getAutonomousCommand() throws IOException {
// https://github.com/mjansen4857/pathplanner/wiki <-- Pathplanner Wiki
Command[] trajCommands = buildAuto(0.5, 0.5, "Move Forward", "Move Down");
SequentialCommandGroup ret = new SequentialCommandGroup(trajCommands);
return (new ParallelCommandGroup(buildAuto(0.1, 0.1, ret, new InstantCommand(() -> m_testMotor.set(TalonFXControlMode.PercentOutput, 0.2)))));
// Command[] trajCommands = buildAuto(0.5, 0.5, "Move Forward", "Move Down");
// SequentialCommandGroup ret = new SequentialCommandGroup(trajCommands);
// return (new ParallelCommandGroup(buildAuto(0.1, 0.1, ret, new InstantCommand(() -> m_testMotor.set(TalonFXControlMode.PercentOutput, 0.2)))));
Optional<File> recordedAuto = Arrays.stream(Filesystem.getDeployDirectory().toPath().resolve("pathplanner").toFile().listFiles()).filter(f -> f.getName().startsWith("recording")).max(Comparator.comparingLong(File::lastModified));
if (recordedAuto.isPresent()) {
String name = recordedAuto.orElseThrow().getName().replace(".path", "");
Logger.getLogger(this.getClass().getName()).fine(name);
return new SequentialCommandGroup(buildAuto(1.0, 1.0, name));
}
Logger.getLogger(this.getClass().getName()).severe("No recorded autos found.");
return new InstantCommand();
// return new PrintCommand("No recorded autos found.");
}
/**
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+15 -16
View File
@@ -24,6 +24,7 @@ import frc4388.robot.Constants.OIConstants;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.utility.Gains;
import frc4388.utility.RobotLogger;
import frc4388.utility.PathPlannerUtil.Path.Waypoint.Point;
public class SwerveDrive extends SubsystemBase {
@@ -156,28 +157,26 @@ public class SwerveDrive extends SubsystemBase {
module.setDesiredState(state, false);
}
}
@Override
public void periodic() {
// System.err.println(m_gyro.getFusedHeading() +" aaa");
updateOdometry();
// m_gyro.setFusedHeadingToCompass();
// m_gyro.setYawToCompass();
{
// 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());
}
Pose2d pos = m_poseEstimator.getEstimatedPosition();
RobotLogger.getInstance().put(
/* anchorPointX */ pos.getX(),
/* anchorPointY */ pos.getY(),
/* prevControlX */ pos.getX(),
/* prevControlY */ pos.getY(),
/* nextControlX */ pos.getX(),
/* nextControlY */ pos.getY(),
/* holonomicAngle */ m_gyro.getRotation2d().getDegrees(),
/* isReversal */ false,
/* velOverride */ null,
/* isLocked */ false
);
SmartDashboard.putNumber("Pigeon Fused Heading", m_gyro.getFusedHeading(fstatus));
SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw());
SmartDashboard.putNumber("Pigeon Get Angle", m_gyro.getAngle());