diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index c18f200..29a2125 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -47,18 +47,17 @@ public final class Constants { public static final class PIDConstants { public static final int SWERVE_SLOT_IDX = 0; public static final int SWERVE_PID_LOOP_IDX = 1; - public static final Gains SWERVE_GAINS = new Gains(1.0, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains SWERVE_GAINS = new Gains(0.5, 0.0, 0.0, 0.0, 0, 1.0); } public static final class AutoConstants { - public static final PIDController X_CONTROLLER = new PIDController(0.0, 0.0, 0.0); - public static final PIDController Y_CONTROLLER = new PIDController(0.0, 0.0, 0.0); - public static final ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController(0.0, 0.0, 0.0, - new TrapezoidProfile.Constraints(0.0, 0.0) - ); + public static final Gains X_CONTROLLER = new Gains(0.8, 0.0, 0.0); + public static final Gains Y_CONTROLLER = new Gains(0.8, 0.0, 0.0); + public static final Gains THETA_CONTROLLER = new Gains(-0.8, 0.0, 0.0); + public static final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints(Math.PI/2, Math.PI/2); // TODO: tune - public static final double PATH_MAX_VEL = -1; // TODO: find the actual value - public static final double PATH_MAX_ACC = -1; // TODO: find the actual value + 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 class Conversions { @@ -89,7 +88,8 @@ public final class Constants { public static final double NEUTRAL_DEADBAND = 0.04; // TODO: find the actual value } - public static final double MAX_SPEED_FEET_PER_SECOND = 10; // TODO: find the actual value + public static final double MAX_SPEED_FEET_PER_SECOND = 5; // TODO: find the actual value + public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; // TODO: find the actual value // dimensions public static final double WIDTH = 18.5; diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index d57fa12..54f68d3 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -10,8 +10,13 @@ package frc4388.robot; import java.lang.System; import java.lang.reflect.Array; import java.util.Arrays; +import java.io.File; +import java.io.IOException; +import java.io.PrintWriter; +import edu.wpi.first.wpilibj.DriverStation; 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.RobotTime; @@ -45,6 +50,8 @@ public class Robot extends TimedRobot { // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); + + SmartDashboard.putData("AutoPlayback Chooser", m_robotContainer.chooser); } /** @@ -94,6 +101,7 @@ public class Robot extends TimedRobot { */ @Override public void autonomousInit() { + m_robotContainer.m_robotSwerveDrive.resetGyro(); m_autonomousCommand = m_robotContainer.getAutonomousCommand(); // schedule the autonomous command (example) @@ -112,6 +120,8 @@ public class Robot extends TimedRobot { @Override public void teleopInit() { + m_robotContainer.m_robotSwerveDrive.resetGyro(); + // This makes sure that the autonomous stops running when // teleop starts running. If you want the autonomous to // continue until interrupted by another command, remove @@ -119,6 +129,7 @@ public class Robot extends TimedRobot { if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } + m_robotContainer.m_robotSwerveDrive.resetGyro(); m_robotTime.startMatchTime(); } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 031cd46..1d181cf 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -7,15 +7,42 @@ package frc4388.robot; +import java.util.ArrayList; +import java.util.List; + +import org.opencv.objdetect.HOGDescriptor; + +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.Trajectory.State; import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 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.SwerveDriveConstants.AutoConstants; +import frc4388.robot.Constants.SwerveDriveConstants.PIDConstants; import frc4388.robot.commands.AutoBalance; import frc4388.robot.commands.DriveWithInput; import frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.controller.IHandController; +import frc4388.robot.commands.JoystickPlayback; +import frc4388.robot.commands.JoystickRecorder; +import frc4388.robot.commands.PlaybackChooser; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.utility.controller.DeadbandedXboxController; import frc4388.utility.controller.XboxController; /** @@ -30,31 +57,60 @@ public class RobotContainer { public final RobotMap m_robotMap = new RobotMap(); /* Subsystems */ - public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.rightFront, m_robotMap.leftBack, m_robotMap.rightBack);//, m_robotMap.gyro); - // private final LED m_robotLED = new LED(m_robotMap.LEDController); - + public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, + m_robotMap.rightFront, + m_robotMap.leftBack, + m_robotMap.rightBack, + m_robotMap.gyro); /* Controllers */ - private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); - private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID); + private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); + private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); + /* Autos */ + public SendableChooser chooser = new SendableChooser<>(); + + private Command noAuto = new InstantCommand(); + + // private Command balance = new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive); + + // private Command blue1Path = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt"); + // private Command blue1PathWithBalance = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt").andThen(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); + + // private Command red1Path = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt", -1); + // private Command red1PathWithBalance = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt", -1).andThen(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); + + // private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"); + + private PlaybackChooser playbackChooser; /** * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { configureButtonBindings(); - /* Default Commands */ - - m_robotSwerveDrive.setDefaultCommand(new DriveWithInput(m_robotSwerveDrive, - () -> getDriverController().getLeftXAxis(), - () -> getDriverController().getLeftYAxis(), - () -> getDriverController().getRightXAxis(), - false)); + // * Default Commands + m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { + m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(), + getDeadbandedDriverController().getRight(), + true); + }, m_robotSwerveDrive) + .withName("SwerveDrive DefaultCommand")); - // m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); + // * Auto Commands + // chooser.setDefaultOption("NoAuto", noAuto); - + // chooser.addOption("Blue1Path", blue1Path); + // chooser.addOption("Blue1PathWithBalance", blue1PathWithBalance); + + // chooser.addOption("Red1Path", red1Path); + // chooser.addOption("Red1PathWithBalance", red1PathWithBalance); + + // chooser.addOption("Taxi", taxi); + + playbackChooser = new PlaybackChooser(m_robotSwerveDrive) + .addOption("Balance", new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)) + .buildDisplay(); } @@ -65,20 +121,27 @@ public class RobotContainer { * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { - /* Driver Buttons */ + // * Driver Buttons + new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); - new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) - .onTrue(new InstantCommand(() -> m_robotMap.gyro.reset())); - // .onFalse() - - /* Operator Buttons */ + // new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(), m_robotSwerveDrive)); + // // .onFalse() new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) .onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); - // interrupt button - new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) - .onTrue(new InstantCommand()); + new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) + .whileTrue(new JoystickRecorder(m_robotSwerveDrive, + () -> getDeadbandedDriverController().getLeftX(), + () -> getDeadbandedDriverController().getLeftY(), + () -> getDeadbandedDriverController().getRightX(), + () -> getDeadbandedDriverController().getRightY(), + "Blue1Path.txt")) + .onFalse(new InstantCommand()); + + // * Operator Buttons } /** @@ -87,35 +150,16 @@ public class RobotContainer { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - - return new InstantCommand(); + // return chooser.getSelected(); + return playbackChooser.getCommand(); } - /** - * Add your docs here. - */ - public IHandController getDriverController() { - return m_driverXbox; + public DeadbandedXboxController getDeadbandedDriverController() { + return this.m_driverXbox; } - /** - * Add your docs here. - */ - public IHandController getOperatorController() { - return m_operatorXbox; + public DeadbandedXboxController getDeadbandedOperatorController() { + return this.m_operatorXbox; } - /** - * Add your docs here. - */ - public Joystick getOperatorJoystick() { - return m_operatorXbox.getJoyStick(); - } - - /** - * Add your docs here. - */ - public Joystick getDriverJoystick() { - return m_driverXbox.getJoyStick(); - } } diff --git a/src/main/java/frc4388/robot/commands/AutoBalance.java b/src/main/java/frc4388/robot/commands/AutoBalance.java index 374de0a..58ee732 100644 --- a/src/main/java/frc4388/robot/commands/AutoBalance.java +++ b/src/main/java/frc4388/robot/commands/AutoBalance.java @@ -25,7 +25,7 @@ public class AutoBalance extends PelvicInflammatoryDisease { @Override public double getError() { - var pitch = gyro.getPitch(); + var pitch = gyro.getRoll(); SmartDashboard.putNumber("pitch", pitch); return pitch; } @@ -33,8 +33,9 @@ public class AutoBalance extends PelvicInflammatoryDisease { @Override public void runWithOutput(double output) { double out2 = MathUtil.clamp(output / 40, -.5, .5); - if (Math.abs(gyro.getPitch()) < 3) out2 = 0; - drive.drive(out2, 0, 0, false); + + if (Math.abs(getError()) < 3) out2 = 0; + drive.driveWithInput(new Translation2d(0, out2), new Translation2d(), false); } @Override diff --git a/src/main/java/frc4388/robot/commands/Blue1Path.txt b/src/main/java/frc4388/robot/commands/Blue1Path.txt new file mode 100644 index 0000000..70d9ee7 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Blue1Path.txt @@ -0,0 +1,150 @@ +0.0,0.0,0.0,0.0,0 +0.0,0.0,0.0,0.0,2 +0.0,0.0,0.0,0.0,18 +0.0,0.0,0.0,0.0,31 +0.0,0.0,0.0,0.0,61 +0.0,0.0,0.0,0.0,72 +0.0,0.0,0.0,0.0,85 +0.0,0.0,0.0,0.0,102 +0.0,0.0,0.0,0.0,116 +0.0,0.0,0.0,0.0,163 +0.0,0.0,0.0,0.0,175 +0.0,0.0,0.0,0.0,188 +0.0,0.0,0.0,0.0,205 +0.0,0.0,0.0,0.0,217 +0.0,0.0,0.0,0.0,229 +0.0,0.0,0.0,0.0,241 +0.0,0.0,0.0,0.0,261 +0.0,0.0,0.0,0.0,281 +0.0,0.0,0.0,0.0,302 +0.0,0.0,0.0,0.0,322 +0.0,0.0,0.0,0.0,341 +0.0,0.0,0.0,0.0,361 +0.0,-0.1328125,0.0,0.0,397 +0.0,-0.1328125,0.0,0.0,412 +0.0,-0.1484375,0.0,0.0,425 +0.0,-0.1796875,0.0,0.0,441 +0.0,-0.1875,0.0,0.0,461 +0.0,-0.1953125,0.0,0.0,481 +0.0,-0.234375,0.0,0.0,502 +0.0,-0.2890625,0.0,0.0,521 +0.0,-0.3125,0.0,0.0,541 +0.0,-0.328125,0.0,0.0,561 +0.0,-0.3671875,0.0,0.0,582 +0.0,-0.390625,0.0,0.0,601 +0.0,-0.4609375,0.0,0.0,642 +0.0,-0.4765625,0.0,0.0,653 +0.0,-0.4765625,0.0,0.0,666 +0.0,-0.4765625,0.0,0.0,681 +0.0,-0.4765625,0.0,0.0,702 +0.0,-0.4765625,0.0,0.0,721 +0.0,-0.4765625,0.0,0.0,741 +0.0,-0.4765625,0.0,0.0,762 +0.0,-0.4765625,0.0,0.0,782 +0.0,-0.484375,0.0,0.0,803 +0.0,-0.484375,0.0,0.0,821 +0.0,-0.4921875,0.0,0.0,842 +0.0,-0.5078125,0.0,0.0,878 +0.0,-0.5234375,0.0,0.0,893 +0.0,-0.5234375,0.0,0.0,906 +0.0,-0.53125,0.0,0.0,922 +0.0,-0.53125,0.0,0.0,942 +0.0,-0.53125,0.0,0.0,962 +0.0,-0.53125,0.0,0.0,982 +0.0,-0.5390625,0.0,0.0,1001 +0.0,-0.5390625,0.0,0.0,1022 +0.0,-0.546875,0.0,0.0,1042 +0.0,-0.546875,0.0,0.0,1061 +0.0,-0.546875,0.0,0.0,1082 +0.0,-0.546875,0.0,0.0,1164 +0.0,-0.546875,0.0,0.0,1176 +0.0,-0.546875,0.0,0.0,1190 +0.0,-0.546875,0.0,0.0,1202 +0.0,-0.546875,0.0,0.0,1219 +0.0,-0.546875,0.0,0.0,1230 +0.0,-0.546875,0.0,0.0,1244 +0.0,-0.546875,0.0,0.0,1258 +0.0,-0.546875,0.0,0.0,1268 +0.0,-0.546875,0.0,0.0,1281 +0.0,-0.546875,0.0,0.0,1301 +0.0,-0.546875,0.0,0.0,1321 +0.0,-0.546875,0.0,0.0,1363 +0.0,-0.546875,0.0,0.0,1376 +0.0,-0.546875,0.0,0.0,1388 +0.0,-0.546875,0.0,0.0,1402 +0.0,-0.546875,0.0,0.0,1422 +0.0,-0.546875,0.0,0.0,1441 +0.0,-0.546875,0.0,0.0,1461 +0.0,-0.546875,0.0,0.0,1482 +0.0,-0.546875,0.0,0.0,1502 +0.0,-0.546875,0.0,0.0,1521 +0.0,-0.546875,0.0,0.0,1542 +0.0,-0.546875,0.0,0.0,1562 +0.0,-0.546875,0.0,0.0,1598 +0.0,-0.546875,0.0,0.0,1609 +0.0,-0.546875,0.0,0.0,1621 +0.0,-0.546875,0.0,0.0,1642 +0.0,-0.546875,0.0,0.0,1661 +0.0,-0.546875,0.0,0.0,1682 +0.0,-0.546875,0.0,0.0,1702 +0.0,-0.546875,0.0,0.0,1722 +0.0,-0.546875,0.0,0.0,1742 +0.0,-0.546875,0.0,0.0,1762 +0.0,-0.546875,0.0,0.0,1781 +0.0,-0.5390625,0.0,0.0,1801 +0.0,-0.5390625,0.0,0.0,1835 +0.0,-0.5390625,0.0,0.0,1847 +0.0,-0.5390625,0.0,0.0,1861 +0.0,-0.5390625,0.0,0.0,1882 +0.0,-0.5390625,0.0,0.0,1901 +0.0,-0.5390625,0.0,0.0,1921 +0.0,-0.5390625,0.0,0.0,1941 +0.0,-0.5390625,0.0,0.0,1962 +0.0,-0.5390625,0.0,0.0,1983 +0.0,-0.5390625,0.0,0.0,2001 +0.0,-0.5390625,0.0,0.0,2022 +0.0,-0.5390625,0.0,0.0,2042 +0.0,-0.5390625,0.0,0.0,2085 +0.0,-0.5390625,0.0,0.0,2097 +0.0,-0.5390625,0.0,0.0,2113 +0.0,-0.5390625,0.0,0.0,2124 +0.0,-0.5390625,0.0,0.0,2141 +0.0,-0.5390625,0.0,0.0,2161 +0.0,-0.5390625,0.0,0.0,2181 +0.0,-0.5390625,0.0,0.0,2201 +0.0,-0.5390625,0.0,0.0,2221 +0.0,-0.5390625,0.0,0.0,2241 +0.0,-0.5390625,0.0,0.0,2262 +0.0,-0.5390625,0.0,0.0,2283 +0.0,-0.2265625,0.0,0.0,2377 +0.0,-0.2265625,0.0,0.0,2390 +0.0,0.0,0.0,0.0,2402 +0.0,0.0,0.0,0.0,2417 +0.0,0.0,0.0,0.0,2428 +0.0,0.0,0.0,0.0,2440 +0.0,0.0,0.0,0.0,2451 +0.0,0.0,0.0,0.0,2463 +0.0,0.0,0.0,0.0,2475 +0.0,0.0,0.0,0.0,2493 +0.0,0.0,0.0,0.0,2505 +0.0,0.0,0.0,0.0,2521 +0.0,0.0,0.0,0.0,2558 +0.0,0.0,0.0,0.0,2576 +0.0,0.0,0.0,0.0,2592 +0.0,0.0,0.0,0.0,2603 +0.0,0.0,0.0,0.0,2621 +0.0,0.0,0.0,0.0,2641 +0.0,0.0,0.0,0.0,2661 +0.0,0.0,0.0,0.0,2681 +0.0,0.0,0.0,0.0,2702 +0.0,0.0,0.0,0.0,2721 +0.0,0.0,0.0,0.0,2742 +0.0,0.0,0.0,0.0,2762 +0.0,0.0,0.0,0.0,2805 +0.0,0.0,0.0,0.0,2814 +0.0,0.0,0.0,0.0,2825 +0.0,0.0,0.0,0.0,2841 +0.0,0.0,0.0,0.0,2861 +0.0,0.0,0.0,0.0,2882 +0.0,0.0,0.0,0.0,2901 +0.0,0.0,0.0,0.0,2922 \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/JoystickPlayback.java b/src/main/java/frc4388/robot/commands/JoystickPlayback.java new file mode 100644 index 0000000..0f496a9 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/JoystickPlayback.java @@ -0,0 +1,141 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands; + +import java.io.File; +import java.io.FileNotFoundException; +import java.util.ArrayList; +import java.util.Scanner; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.utility.UtilityStructs.TimedOutput; + +public class JoystickPlayback extends CommandBase { + private final SwerveDrive swerve; + private String filename; + private int mult = 1; + private Scanner input; + private final ArrayList outputs = new ArrayList<>(); + private int counter = 0; + private long startTime = 0; + private long playbackTime = 0; + private int lastIndex; + private boolean m_finished = false; // ! find a better way + + /** Creates a new JoystickPlayback. */ + public JoystickPlayback(SwerveDrive swerve, String filename, int mult) { + this.swerve = swerve; + this.filename = filename; + this.mult = mult; + + addRequirements(this.swerve); + } + + /** Creates a new JoystickPlayback. */ + public JoystickPlayback(SwerveDrive swerve, String filename) { + this(swerve, filename, 1); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + outputs.clear(); + m_finished = false; + + startTime = System.currentTimeMillis(); + playbackTime = 0; + lastIndex = 0; + try { + input = new Scanner(new File("/home/lvuser/autos/" + filename)); + + String line = ""; + while (input.hasNextLine()) { + line = input.nextLine(); + + if (line.isEmpty() || line.isBlank() || line.equals("\n")) { + continue; + } + + String[] values = line.split(","); + System.out.println("values: " + values[0] + " " + values[1] + " " + values[2] + " " + values[3]); + + var out = new TimedOutput(); + out.leftX = Double.parseDouble(values[0]) * mult; + out.leftY = Double.parseDouble(values[1]); + out.rightX = Double.parseDouble(values[2]); + out.rightY = Double.parseDouble(values[3]); + + out.timedOffset = Long.parseLong(values[4]); + + outputs.add(out); + } + + input.close(); + } catch (FileNotFoundException e) { + e.printStackTrace(); + } + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if (counter == 0) { + startTime = System.currentTimeMillis(); + playbackTime = 0; + } else { + playbackTime = System.currentTimeMillis() - startTime; + } + + // skip to reasonable time frame + // too tired to write comment: ask daniel thomas; it goes to the thing until it's bigger than the other thing + { + int i = lastIndex == 0 ? 1 : lastIndex; + while (i < outputs.size() && outputs.get(i).timedOffset < playbackTime) { + i++; + } + + if (i >= outputs.size()) { + m_finished = true; // ! kind of a hack + return; + } + lastIndex = i; + } + + TimedOutput lastOut = outputs.get(lastIndex - 1); + TimedOutput out = outputs.get(lastIndex); + + double deltaTime = out.timedOffset - lastOut.timedOffset; + double playbackDelta = playbackTime - lastOut.timedOffset; + + double lerpLX = lastOut.leftX + (out.leftX - lastOut.leftX) * (playbackDelta / deltaTime); + double lerpLY = lastOut.leftY + (out.leftY - lastOut.leftY) * (playbackDelta / deltaTime); + double lerpRX = lastOut.rightX + (out.rightX - lastOut.rightX) * (playbackDelta / deltaTime); + double lerpRY = lastOut.rightY + (out.rightY - lastOut.rightY) * (playbackDelta / deltaTime); + + // this.swerve.driveWithInput(new Translation2d(out.leftX, out.leftY), + // new Translation2d(out.rightX, out.rightY), + // true); + + this.swerve.driveWithInput( new Translation2d(lerpLX, lerpLY), + new Translation2d(lerpRX, lerpRY), + true); + + counter++; + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + input.close(); + swerve.stopModules(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return m_finished; + } +} diff --git a/src/main/java/frc4388/robot/commands/JoystickRecorder.java b/src/main/java/frc4388/robot/commands/JoystickRecorder.java new file mode 100644 index 0000000..eea72b4 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/JoystickRecorder.java @@ -0,0 +1,97 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands; + +import java.io.File; +import java.io.IOException; +import java.io.PrintWriter; +import java.util.ArrayList; +import java.util.function.Supplier; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.utility.UtilityStructs.TimedOutput; + +public class JoystickRecorder extends CommandBase { + public final SwerveDrive swerve; + + public final Supplier leftX; + public final Supplier leftY; + public final Supplier rightX; + public final Supplier rightY; + private String filename; + public final ArrayList outputs = new ArrayList<>(); + private long startTime = -1; + + + /** Creates a new JoystickRecorder. */ + public JoystickRecorder(SwerveDrive swerve, Supplier leftX, Supplier leftY, + Supplier rightX, Supplier rightY, + String filename) + { + this.swerve = swerve; + this.leftX = leftX; + this.leftY = leftY; + this.rightX = rightX; + this.rightY = rightY; + this.filename = filename; + + addRequirements(this.swerve); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + outputs.clear(); + + this.startTime = System.currentTimeMillis(); + + outputs.add(new TimedOutput()); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + var inputs = new TimedOutput(); + inputs.leftX = leftX.get(); + inputs.leftY = leftY.get(); + inputs.rightX = rightX.get(); + inputs.rightY = rightY.get(); + inputs.timedOffset = System.currentTimeMillis() - startTime; + + outputs.add(inputs); + + swerve.driveWithInput(new Translation2d(inputs.leftX, inputs.leftY), + new Translation2d(inputs.rightX, inputs.rightY), + true); + + System.out.println("RECORDING"); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + File output = new File("/home/lvuser/autos/" + filename); + + try (PrintWriter writer = new PrintWriter(output)) { + for (var input : outputs) { + writer.println( input.leftX + "," + input.leftY + "," + + input.rightX + "," + input.rightY + "," + + input.timedOffset); + } + + writer.close(); + } catch (IOException e) { + e.printStackTrace(); + } + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc4388/robot/commands/PlaybackChooser.java b/src/main/java/frc4388/robot/commands/PlaybackChooser.java new file mode 100644 index 0000000..a86c38e --- /dev/null +++ b/src/main/java/frc4388/robot/commands/PlaybackChooser.java @@ -0,0 +1,80 @@ +package frc4388.robot.commands; + +import java.io.File; +import java.util.ArrayList; +import java.util.HashMap; + +import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.WidgetType; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc4388.robot.subsystems.SwerveDrive; + +public class PlaybackChooser { + private final ArrayList> m_choosers = new ArrayList<>(); + private SendableChooser m_playback = null; + private final HashMap m_commandPool = new HashMap<>(); + + private final File m_dir = new File("/home/lvuser/autos/"); + private final SwerveDrive m_swerve; + + // commands + private Command m_noAuto = new InstantCommand(); + + public PlaybackChooser(SwerveDrive swerve, Object... pool) { + m_swerve = swerve; + } + + public PlaybackChooser addOption(String name, Command option) { + m_commandPool.put(name, option); + return this; + } + + public PlaybackChooser buildDisplay() { + appendCommand(); + m_playback = m_choosers.get(0); + + Shuffleboard.getTab("Auto Chooser") + .add("Add Sequence", new InstantCommand(() -> appendCommand())) + .withPosition(4, 0); + return this; + } + + // This will be bound to a button for the time being + public void appendCommand() { + var chooser = new SendableChooser(); + + chooser.setDefaultOption("No Auto", m_noAuto); + for (String auto : m_dir.list()) { + m_playback.addOption(auto, new JoystickPlayback(m_swerve, auto)); + } + for (var cmd_name : m_commandPool.keySet()) { + chooser.addOption(cmd_name, m_commandPool.get(cmd_name)); + } + + m_choosers.add(chooser); + Shuffleboard.getTab("Auto Chooser") + .add("Command: " + m_choosers.size(), chooser) + .withSize(4, 1) + .withPosition(0, m_choosers.size() - 1) + .withWidget(BuiltInWidgets.kSplitButtonChooser); + } + + public Command getCommand() { + Command command = m_playback.getSelected(); + command = command == null ? m_noAuto : command.asProxy(); + + Command[] commands = new Command[m_choosers.size() - 1]; + for (int i = 0; i < m_choosers.size()-1; i++) { + Command command2 = m_choosers.get(i + 1).getSelected(); + command2 = command2 == null ? m_noAuto : command2.asProxy(); + + commands[i] = command2.asProxy(); + } + + return command.andThen(commands); + } +} diff --git a/src/main/java/frc4388/robot/commands/Taxi.txt b/src/main/java/frc4388/robot/commands/Taxi.txt new file mode 100644 index 0000000..c99ee2c --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Taxi.txt @@ -0,0 +1,225 @@ +0.0,0.0,0.0,0.0,0 +0.0,0.0,0.0,0.0,0 +0.0,0.0,0.0,0.0,12 +0.0,0.0,0.0,0.0,26 +0.0,0.0,0.0,0.0,37 +0.0,0.0,0.0,0.0,50 +0.0,0.0,0.0,0.0,62 +0.0,0.0,0.0,0.0,73 +0.0,0.0,0.0,0.0,88 +0.0,0.0,0.0,0.0,103 +0.0,0.0,0.0,0.0,116 +0.0,0.0,0.0,0.0,160 +0.0,0.0,0.0,0.0,173 +0.0,0.0,0.0,0.0,185 +0.0,0.0,0.0,0.0,200 +0.0,0.0,0.0,0.0,211 +0.0,0.0,0.0,0.0,223 +0.0,0.0,0.0,0.0,235 +0.0,0.0,0.0,0.0,247 +0.0,0.0,0.0,0.0,263 +0.0,0.0,0.0,0.0,283 +0.0,0.0,0.0,0.0,303 +0.0,-0.109375,0.0,0.0,323 +0.0,-0.1484375,0.0,0.0,343 +0.0,-0.2109375,0.0,0.0,363 +0.0,-0.3671875,0.0,0.0,398 +0.0,-0.4140625,0.0,0.0,411 +0.0,-0.4765625,0.0,0.0,425 +0.0,-0.5078125,0.0,0.0,443 +0.0,-0.5078125,0.0,0.0,463 +0.0,-0.53125,0.0,0.0,483 +0.0,-0.5546875,0.0,0.0,504 +0.0,-0.5625,0.0,0.0,523 +0.0,-0.5625,0.0,0.0,544 +0.0,-0.5703125,0.0,0.0,563 +0.0,-0.5859375,0.0,0.0,584 +0.0,-0.5859375,0.0,0.0,603 +0.0,-0.5859375,0.0,0.0,640 +0.0,-0.59375,0.0,0.0,657 +0.0,-0.6015625,0.0,0.0,672 +0.0,-0.6015625,0.0,0.0,685 +0.0,-0.6015625,0.0,0.0,703 +0.0,-0.6015625,0.0,0.0,723 +0.0,-0.6015625,0.0,0.0,743 +0.0,-0.6015625,0.0,0.0,763 +0.0,-0.6015625,0.0,0.0,783 +0.0,-0.6015625,0.0,0.0,803 +0.0,-0.6015625,0.0,0.0,823 +0.0,-0.6015625,0.0,0.0,844 +0.0,-0.6015625,0.0,0.0,878 +0.0,-0.6015625,0.0,0.0,893 +0.0,-0.6015625,0.0,0.0,907 +0.0,-0.6015625,0.0,0.0,924 +0.0,-0.609375,0.0,0.0,943 +0.0,-0.609375,0.0,0.0,963 +0.0,-0.609375,0.0,0.0,983 +0.0,-0.609375,0.0,0.0,1004 +0.0,-0.609375,0.0,0.0,1023 +0.0,-0.609375,0.0,0.0,1043 +0.0,-0.609375,0.0,0.0,1064 +0.0,-0.609375,0.0,0.0,1083 +0.0,-0.609375,0.0,0.0,1156 +0.0,-0.609375,0.0,0.0,1172 +0.0,-0.609375,0.0,0.0,1185 +0.0,-0.609375,0.0,0.0,1200 +0.0,-0.609375,0.0,0.0,1215 +0.0,-0.609375,0.0,0.0,1225 +0.0,-0.609375,0.0,0.0,1236 +0.0,-0.609375,0.0,0.0,1249 +0.0,-0.609375,0.0,0.0,1263 +0.0,-0.609375,0.0,0.0,1283 +0.0,-0.609375,0.0,0.0,1303 +0.0,-0.609375,0.0,0.0,1323 +0.0,-0.609375,0.0,0.0,1363 +0.0,-0.6015625,0.0,0.0,1376 +0.0,-0.6015625,0.0,0.0,1394 +0.0,-0.6015625,0.0,0.0,1405 +0.0,-0.6015625,0.0,0.0,1423 +0.0,-0.6015625,0.0,0.0,1443 +0.0,-0.6015625,0.0,0.0,1463 +0.0,-0.6015625,0.0,0.0,1483 +0.0,-0.6015625,0.0,0.0,1503 +0.0,-0.6015625,0.0,0.0,1523 +0.0,-0.6015625,0.0,0.0,1543 +0.0,-0.6015625,0.0,0.0,1563 +0.0,-0.6015625,0.0,0.0,1597 +0.0,-0.6015625,0.0,0.0,1608 +0.0,-0.6015625,0.0,0.0,1624 +0.0,-0.6015625,0.0,0.0,1643 +0.0,-0.6015625,0.0,0.0,1664 +0.0,-0.5859375,0.0,0.0,1683 +0.0,-0.5859375,0.0,0.0,1703 +0.0,-0.5625,0.0,0.0,1723 +0.0,-0.5625,0.0,0.0,1743 +0.0,-0.5625,0.0,0.0,1763 +0.0,-0.5625,0.0,0.0,1783 +0.0,-0.5625,0.0,0.0,1803 +0.0,-0.5625,0.0,0.0,1843 +0.0,-0.5625,0.0,0.0,1855 +0.0,-0.5625,0.0,0.0,1868 +0.0,-0.5625,0.0,0.0,1883 +0.0,-0.5625,0.0,0.0,1903 +0.0,-0.5625,0.0,0.0,1923 +0.0,-0.5625,0.0,0.0,1943 +0.0,-0.5625,0.0,0.0,1963 +0.0,-0.5625,0.0,0.0,1983 +0.0,-0.5625,0.0,0.0,2003 +0.0,-0.5625,0.0,0.0,2024 +0.0,-0.5625,0.0,0.0,2043 +0.0,-0.5625,0.0,0.0,2081 +0.0,-0.5625,0.0,0.0,2093 +0.0,-0.5625,0.0,0.0,2105 +0.0,-0.5625,0.0,0.0,2123 +0.0,-0.5625,0.0,0.0,2143 +0.0,-0.5625,0.0,0.0,2163 +0.0,-0.5625,0.0,0.0,2183 +0.0,-0.5625,0.0,0.0,2203 +0.0,-0.5625,0.0,0.0,2223 +0.0,-0.5625,0.0,0.0,2243 +0.0,-0.5625,0.0,0.0,2263 +0.0,-0.5625,0.0,0.0,2283 +0.0,-0.5625,0.0,0.0,2366 +0.0,-0.5625,0.0,0.0,2377 +0.0,-0.5625,0.0,0.0,2394 +0.0,-0.5703125,0.0,0.0,2405 +0.0,-0.5703125,0.0,0.0,2418 +0.0,-0.5703125,0.0,0.0,2431 +0.0,-0.5703125,0.0,0.0,2444 +0.0,-0.5703125,0.0,0.0,2458 +0.0,-0.5703125,0.0,0.0,2470 +0.0,-0.5703125,0.0,0.0,2485 +0.0,-0.5703125,0.0,0.0,2503 +0.0,-0.5703125,0.0,0.0,2523 +0.0,-0.5703125,0.0,0.0,2563 +0.0,-0.5703125,0.0,0.0,2577 +0.0,-0.5703125,0.0,0.0,2591 +0.0,-0.5703125,0.0,0.0,2608 +0.0,-0.5703125,0.0,0.0,2624 +0.0,-0.5703125,0.0,0.0,2643 +0.0,-0.5703125,0.0,0.0,2677 +0.0,-0.5703125,0.0,0.0,2698 +0.0,-0.5703125,0.0,0.0,2711 +0.0,-0.5703125,0.0,0.0,2725 +0.0,-0.5703125,0.0,0.0,2743 +0.0,-0.5703125,0.0,0.0,2764 +0.0,-0.5703125,0.0,0.0,2810 +0.0,-0.5703125,0.0,0.0,2820 +0.0,-0.5703125,0.0,0.0,2833 +0.0,-0.5703125,0.0,0.0,2845 +0.0,-0.5703125,0.0,0.0,2863 +0.0,-0.5703125,0.0,0.0,2883 +0.0,-0.5703125,0.0,0.0,2904 +0.0,-0.5703125,0.0,0.0,2924 +0.0,-0.5703125,0.0,0.0,2943 +0.0,-0.5703125,0.0,0.0,2963 +0.0,-0.5703125,0.0,0.0,2983 +0.0,-0.5703125,0.0,0.0,3003 +0.0,-0.5703125,0.0,0.0,3033 +0.0,-0.5703125,0.0,0.0,3050 +0.0,-0.5703125,0.0,0.0,3065 +0.0,-0.5703125,0.0,0.0,3083 +0.0,-0.5703125,0.0,0.0,3103 +0.0,-0.5703125,0.0,0.0,3123 +0.0,-0.5703125,0.0,0.0,3144 +0.0,-0.5703125,0.0,0.0,3164 +0.0,-0.5703125,0.0,0.0,3184 +0.0,-0.5703125,0.0,0.0,3203 +0.0,-0.5703125,0.0,0.0,3223 +0.0,-0.5703125,0.0,0.0,3243 +0.0,-0.5703125,0.0,0.0,3272 +0.0,-0.5703125,0.0,0.0,3289 +0.0,-0.5703125,0.0,0.0,3303 +0.0,-0.5703125,0.0,0.0,3323 +0.0,-0.5703125,0.0,0.0,3343 +0.0,-0.5703125,0.0,0.0,3363 +0.0,-0.5703125,0.0,0.0,3383 +0.0,-0.5703125,0.0,0.0,3403 +0.0,-0.5703125,0.0,0.0,3423 +0.0,-0.5703125,0.0,0.0,3443 +0.0,-0.5703125,0.0,0.0,3463 +0.0,-0.5703125,0.0,0.0,3483 +0.0,-0.5703125,0.0,0.0,3566 +0.0,-0.5703125,0.0,0.0,3578 +0.0,-0.5703125,0.0,0.0,3596 +0.0,-0.5703125,0.0,0.0,3610 +0.0,-0.5703125,0.0,0.0,3623 +0.0,-0.5703125,0.0,0.0,3640 +0.0,-0.5703125,0.0,0.0,3651 +0.0,-0.5703125,0.0,0.0,3663 +0.0,-0.5703125,0.0,0.0,3678 +0.0,-0.5703125,0.0,0.0,3691 +0.0,-0.5703125,0.0,0.0,3706 +0.0,-0.5703125,0.0,0.0,3723 +0.0,-0.5703125,0.0,0.0,3766 +0.0,-0.5703125,0.0,0.0,3778 +0.0,-0.5703125,0.0,0.0,3792 +0.0,-0.5703125,0.0,0.0,3807 +0.0,-0.5703125,0.0,0.0,3823 +0.0,-0.5703125,0.0,0.0,3843 +0.0,-0.53125,0.0,0.0,3863 +0.0,-0.53125,0.0,0.0,3884 +0.0,-0.421875,0.0,0.0,3904 +0.0,0.0,0.0,0.0,3924 +0.0,0.0,0.0,0.0,3944 +0.0,0.0,0.0,0.0,3963 +0.0,0.0,0.0,0.0,3999 +0.0,0.0,0.0,0.0,4010 +0.0,0.0,0.0,0.0,4025 +0.0,0.0,0.0,0.0,4043 +0.0,0.0,0.0,0.0,4063 +0.0,0.0,0.0,0.0,4083 +0.0,0.0,0.0,0.0,4103 +0.0,0.0,0.0,0.0,4123 +0.0,0.0,0.0,0.0,4143 +0.0,0.0,0.0,0.0,4163 +0.0,0.0,0.0,0.0,4183 +0.0,0.0,0.0,0.0,4203 +0.0,0.0,0.0,0.0,4236 +0.0,0.0,0.0,0.0,4247 +0.0,0.0,0.0,0.0,4264 +0.0,0.0,0.0,0.0,4284 +0.0,0.0,0.0,0.0,4304 +0.0,0.0,0.0,0.0,4324 +0.0,0.0,0.0,0.0,4343 +0.0,0.0,0.0,0.0,4363 \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index e9ef898..9f29094 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -4,6 +4,9 @@ package frc4388.robot.subsystems; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +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.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; @@ -26,18 +29,13 @@ public class SwerveDrive extends SubsystemBase { private Translation2d leftBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); private Translation2d rightBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); - private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation); + private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation); - // private SwerveDriveOdometry odometry = new SwerveDrive( - // kinematics, - // gyro.getRotation2d(), - // new SwerveModulePosition[] { - // leftFront.getPosition(), - // rightFront.getPosition(), - // leftBack.getPosition(), - // rightBack.getPosition() - // } - // ); + private RobotGyro gyro; + + // private SwerveDriveOdometry odometry; + + private SwerveDrivePoseEstimator poseEstimator; public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default @@ -47,18 +45,55 @@ public class SwerveDrive extends SubsystemBase { this.rightFront = rightFront; this.leftBack = leftBack; this.rightBack = rightBack; + + this.gyro = gyro; + + // this.odometry = new SwerveDriveOdometry( + // kinematics, + // gyro.getRotation2d(), + // new SwerveModulePosition[] { + // leftFront.getPosition(), + // rightFront.getPosition(), + // leftBack.getPosition(), + // rightBack.getPosition() + // }, + // getOdometry() + // ); + + this.poseEstimator = new SwerveDrivePoseEstimator( + kinematics, + gyro.getRotation2d(), + new SwerveModulePosition[] { + leftFront.getPosition(), + rightFront.getPosition(), + leftBack.getPosition(), + rightBack.getPosition() + }, + new Pose2d(0,0, new Rotation2d(0)) + ); this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack}; } - // WPILib swerve drive example - public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) { - // SwerveModuleState[] states = kinematics.toSwerveModuleStates( - // fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, gyro.getRotation2d()) - // : new ChassisSpeeds(xSpeed, ySpeed, rot) - // ); - SwerveModuleState[] states = kinematics.toSwerveModuleStates(new ChassisSpeeds(xSpeed, ySpeed, rot * SwerveDriveConstants.ROTATION_SPEED)); - setModuleStates(states); + public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { + if (fieldRelative) { + if (rightStick.getNorm() > 0.1) { + rotTarget = new Rotation2d(rightStick.getX(), -rightStick.getY()).minus(new Rotation2d(0, 1)); + } + + double rot = rotTarget.minus(gyro.getRotation2d()).getRadians(); + + // Use the left joystick to set speed. Apply a quadratic curve and the set max speed. + Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); + + // Convert field-relative speeds to robot-relative speeds. + chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), speed.getY(), rot * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1)); + + } else { + // Create robot-relative speeds. + chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED); + } + setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds)); } /** @@ -74,6 +109,16 @@ public class SwerveDrive extends SubsystemBase { } } + public double getGyroAngle() { + return gyro.getAngle(); + } + + public void resetGyro() { + gyro.reset(); + // setOdometry(getOdometry()); + rotTarget = new Rotation2d(0); + } + /** * Updates the odometry of the SwerveDrive. */ @@ -89,6 +134,18 @@ public class SwerveDrive extends SubsystemBase { // ); // } + public void updatePoseEstimator() { + poseEstimator.update( + gyro.getRotation2d(), + new SwerveModulePosition[] { + leftFront.getPosition(), + rightFront.getPosition(), + leftBack.getPosition(), + rightBack.getPosition() + } + ); + } + /** * Gets the odometry of the SwerveDrive. * @return The odometry of the SwerveDrive as a Pose2d object (xMeters, yMeters, theta). @@ -97,6 +154,10 @@ public class SwerveDrive extends SubsystemBase { // return odometry.getPoseMeters(); // } + public Pose2d getPoseEstimator() { + return poseEstimator.getEstimatedPosition(); + } + /** * Sets the odometry of the SwerveDrive. * @param pose Pose to set the odometry to. @@ -114,21 +175,35 @@ public class SwerveDrive extends SubsystemBase { // ); // } + public void setPoseEstimator(Pose2d pose) { + poseEstimator.resetPosition( + gyro.getRotation2d(), + new SwerveModulePosition[] { + leftFront.getPosition(), + rightFront.getPosition(), + leftBack.getPosition(), + rightBack.getPosition() + }, + pose + ); + } + + public void resetPoseEstimator() { + setPoseEstimator(new Pose2d()); + } + + public void stopModules() { + for (SwerveModule module : this.modules) { + module.stop(); + } + } + /** * Resets the odometry of the SwerveDrive to 0. * *NOTE: If you reset your gyroscope or wheel encoders, this method MUST be called with the new gyro angle and wheel encoder positions. */ // public void resetOdometry() { - // odometry.resetPosition( - // gyro.getRotation2d(), - // new SwerveModulePosition[] { - // leftFront.getPosition(), - // rightFront.getPosition(), - // leftBack.getPosition(), - // rightBack.getPosition() - // }, - // new Pose2d() - // ); + // setOdometry(new Pose2d()); // } public SwerveDriveKinematics getKinematics() { @@ -138,7 +213,16 @@ public class SwerveDrive extends SubsystemBase { @Override public void periodic() { // This method will be called once per scheduler run + // updateOdometry(); + updatePoseEstimator(); + + // SmartDashboard.putNumber("Odo X (ft)", Units.metersToFeet(this.getOdometry().getX())); + // SmartDashboard.putNumber("Odo Y (ft)", Units.metersToFeet(this.getOdometry().getY())); + // SmartDashboard.putNumber("Odo Theta", this.getOdometry().getRotation().getDegrees()); + + // SmartDashboard.putNumber("Gyro Angle", getGyroAngle()); + // SmartDashboard.putNumber("rotTarget", this.rotTarget.getDegrees()); } /** diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 4bbbefb..2ed5df8 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -124,7 +124,10 @@ public class SwerveModule extends SubsystemBase { // convert the CANCoder from its position reading to ticks double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient(); - angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks); + + if (!ignoreAngle) { + angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks); + } double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond); diff --git a/src/main/java/frc4388/utility/AprilTag.java b/src/main/java/frc4388/utility/AprilTag.java new file mode 100644 index 0000000..c2ad269 --- /dev/null +++ b/src/main/java/frc4388/utility/AprilTag.java @@ -0,0 +1,13 @@ +package frc4388.utility; + +// This is a seperate class in case I want to encode rotation or other +// information about the tag +public class AprilTag { + public final double x, y, z; + + public AprilTag(double _x, double _y, double _z) { + x = _x; + y = _y; + z = _z; + } +} diff --git a/src/main/java/frc4388/utility/Gains.java b/src/main/java/frc4388/utility/Gains.java index 7cda1e0..7a3a026 100644 --- a/src/main/java/frc4388/utility/Gains.java +++ b/src/main/java/frc4388/utility/Gains.java @@ -54,6 +54,12 @@ public class Gains { this.kMinOutput = -1.0; } + public Gains(double kP, double kI, double kD) { + this.kP = kP; + this.kI = kI; + this.kD = kD; + } + /** * Creates Gains object for PIDs * @param kP The P value. diff --git a/src/main/java/frc4388/utility/UtilityStructs.java b/src/main/java/frc4388/utility/UtilityStructs.java new file mode 100644 index 0000000..e8b10cc --- /dev/null +++ b/src/main/java/frc4388/utility/UtilityStructs.java @@ -0,0 +1,12 @@ +package frc4388.utility; + +public class UtilityStructs { + public static class TimedOutput { + public double leftX = 0.0; + public double leftY = 0.0; + public double rightX = 0.0; + public double rightY = 0.0; + + public long timedOffset = 0; + } +}