From 042a03a58f3eaff0217087ba04d95cb3ab5685bf Mon Sep 17 00:00:00 2001 From: nathanrsxtn Date: Thu, 24 Mar 2022 19:08:24 -0600 Subject: [PATCH] Add manual CSV tuning Remove free storage and RAM commands --- src/main/java/frc4388/robot/Robot.java | 62 +++---------- .../java/frc4388/robot/RobotContainer.java | 87 +++++++++++++------ .../commands/ShooterCommands/TrackTarget.java | 28 +++--- .../frc4388/robot/subsystems/BoomBoom.java | 16 ++-- 4 files changed, 95 insertions(+), 98 deletions(-) diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 036bfa9..4dba8bf 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -5,30 +5,20 @@ package frc4388.robot; import java.io.File; -import java.nio.file.Files; -import java.nio.file.Path; -import java.util.HashMap; -import java.util.Map; import java.util.logging.Level; import java.util.logging.Logger; import com.diffplug.common.base.Errors; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.drive.Vector2d; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -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.CommandScheduler; -import edu.wpi.first.wpilibj2.command.RunCommand; import frc4388.utility.RobotTime; -import frc4388.utility.VelocityCorrection; -import frc4388.utility.desmos.DesmosServer; +import frc4388.utility.Vector2D; /** * The VM is configured to automatically run this class, and to call the @@ -83,45 +73,6 @@ public class Robot extends TimedRobot { m_robotContainer = new RobotContainer(); // addPeriodic(m_robotContainer::recordPeriodic, kDefaultPeriod); SmartDashboard.putData(CommandScheduler.getInstance()); - SmartDashboard.putData("JVM Memory", new RunCommand(() -> { - }) { - @Override - public boolean runsWhenDisabled() { - return true; - } - - @Override - public String getName() { - if (isScheduled()) { - Runtime runtime = Runtime.getRuntime(); - long totalMemory = runtime.totalMemory() / 1_000_000; - long freeMemory = runtime.freeMemory() / 1_000_000; - long maxMemory = runtime.maxMemory() / 1_000_000; - return totalMemory - freeMemory + " MB / " + totalMemory + " MB / " + maxMemory + " MB"; - } - return "Not Running"; - } - }); - SmartDashboard.putData("Usable Deploy Space", new RunCommand(() -> { - }) { - @Override - public boolean runsWhenDisabled() { - return true; - } - - @Override - public String getName() { - if (isScheduled()) { - File deploy = Filesystem.getDeployDirectory(); - long usedSpace = Errors.suppress().getWithDefault( - () -> Files.walk(deploy.toPath()).map(Path::toFile).filter(File::isFile).mapToLong(File::length).sum(), - 0l) / 1_000_000; - long usableSpace = deploy.getUsableSpace() / 1_000_000; - return usedSpace + " MB / " + usableSpace + " MB"; - } - return "Not Running"; - } - }); // desmosServer.start(); m_robotContainer.m_robotVisionOdometry.setLEDs(true); @@ -140,6 +91,15 @@ public class Robot extends TimedRobot { @Override public void robotPeriodic() { m_robotTime.updateTimes(); + + Vector2D firstBallPosition = new Vector2D(15.56 - (82.83 / 2.00), 11.21 - 162.00); + Vector2D secondBallPosition = new Vector2D(-(40.44 * (Math.sqrt(2.00) / 2.00)) - ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (82.83 / 2.00), -(40.44 * (Math.sqrt(2.00) / 2.00)) + ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (219.25 / 2.00)); // * position of second ball, relative to hub. + Vector2D firstToSecond = Vector2D.subtract(secondBallPosition, firstBallPosition); + + System.out.println("First Ball (FEET): " + Vector2D.divide(firstBallPosition, 12).toString()); + System.out.println("Second Ball (FEET): " + Vector2D.divide(secondBallPosition, 12).toString()); + System.out.println("First To Second (FEET): " + Vector2D.divide(firstToSecond, 12).toString()); + // current = // m_robotContainer.m_robotBoomBoom.getCurrent() + // m_robotContainer.m_robotClimber.getCurrent(); //+ diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 4c1ddec..5758dd9 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -4,12 +4,15 @@ package frc4388.robot; +import java.io.File; +import java.nio.file.Files; +import java.nio.file.StandardOpenOption; import java.util.ArrayList; -import java.util.HashMap; -import java.util.Map; +import java.util.Arrays; import java.util.Objects; import java.util.logging.Logger; +import com.diffplug.common.base.Errors; import com.pathplanner.lib.PathPlanner; import com.pathplanner.lib.PathPlannerTrajectory; import com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState; @@ -19,14 +22,19 @@ 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.util.sendable.Sendable; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; 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.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.JoystickButton; @@ -37,7 +45,6 @@ import frc4388.robot.commands.PathRecorder; import frc4388.robot.commands.RunCommandForTime; import frc4388.robot.commands.DriveCommands.DriveWithInputForTime; import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup; -import frc4388.robot.commands.ShooterCommands.Shoot; import frc4388.robot.commands.ShooterCommands.TrackTarget; import frc4388.robot.subsystems.BoomBoom; import frc4388.robot.subsystems.Claws; @@ -129,8 +136,6 @@ public class RobotContainer { */ public RobotContainer() { - - // double turretDistanceFromFront = 10.0; // * distance of turret from the front of the robot in inches. might need to be somewhat accurate. // double distancePerSecond = 134.0; // * assuming emulated joystick input magnitude is 1.0 @@ -269,6 +274,39 @@ public class RobotContainer { new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.45), m_robotBoomBoom) ); + + if (m_robotBoomBoom.readManualData) { + var tab = Shuffleboard.getTab("Manual Shooter Data"); + var manual = tab.getLayout("Manual Shooter Data", BuiltInLayouts.kList).withSize(2, 3); + manual.add("Manual Shooter Data", new Sendable() { + private double manualHoodExt; + private double manualDrumVel; + + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("RobotPreferences"); + builder.addBooleanProperty("Disable CSV", () -> m_robotBoomBoom.readManualData, b -> m_robotBoomBoom.readManualData = b); + builder.addDoubleProperty("Drum Velocity", () -> manualDrumVel, d -> manualDrumVel = d); + builder.addDoubleProperty("Hood Extension", () -> manualHoodExt, d -> manualHoodExt = d); + builder.addDoubleProperty("Measured Distance", () -> SmartDashboard.getNumber("Distance to Target", -1), System.out::println); + } + }); + manual.add("Shooter Table Appender", new InstantCommand(() -> Errors.log().run(() -> Files.write(new File(Filesystem.getDeployDirectory(), "ShooterData.csv").toPath(), String.format("%s,%s,%s%n", SmartDashboard.getNumber("Manual Shooter Data/Distance to Target", -1), SmartDashboard.getNumber("Manual Shooter Data/Hood Extension", -1), SmartDashboard.getNumber("Manual Shooter Data/Drum Velocity", -1)).getBytes(), StandardOpenOption.WRITE, StandardOpenOption.APPEND))) { + @Override + public boolean runsWhenDisabled() { + return true; + } + }.withName("Append")); + var csv = tab.getLayout("Shooter Data", BuiltInLayouts.kList).withPosition(2,0).withSize(4, 3); + csv.add("Initial Shooter Data", builder -> Arrays.stream(m_robotBoomBoom.m_shooterTable).forEach(e -> builder.addDoubleArrayProperty(Double.toString(e.distance), () -> new double[] {e.hoodExt, e.drumVelocity}, a -> {}))); + csv.add("Reload Data", new InstantCommand(m_robotBoomBoom::updateShooterTable) { + @Override + public boolean runsWhenDisabled() { + return true; + } + }.withName("Reload")); + } + } /** @@ -460,6 +498,7 @@ public class RobotContainer { // * 2. try opposite joystick input: new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, -1.0, 0.0, 0.0, false), m_robotSwerveDrive); // * 3a. try permanently setting drive motors to brake, not coast, in RobotMap.java, and ask the driver how it feels. // * 3b. try to only set the drive motors to brake if in auto mode. + // * 4. try new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive); // ! 1.0 input, 1 second: 134 inches // ! 0.75 input, 1 second: 48 inches @@ -471,33 +510,29 @@ public class RobotContainer { double distancePerSecond = 134.0; // * assuming emulated joystick input magnitude is 1.0 double offset = 10.0; // * distance (in inches) from ball that we actually want to stop + // ! ball positions are unit tested Vector2D firstBallPosition = new Vector2D(15.56 - (82.83 / 2.00), 11.21 - 162.00); // * position of first ball, relative to hub. Vector2D secondBallPosition = new Vector2D(-(40.44 * (Math.sqrt(2.00) / 2.00)) - ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (82.83 / 2.00), -(40.44 * (Math.sqrt(2.00) / 2.00)) + ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (219.25 / 2.00)); // * position of second ball, relative to hub. Vector2D firstToSecond = Vector2D.subtract(secondBallPosition, firstBallPosition); // * vector from first ball to second ball, used to calculate emulated joystick inputs. - // SequentialCommandGroup weirdAutoShootingGroup = new SequentialCommandGroup(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true), - // new ParallelCommandGroup( - // new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true), - // new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 2.0) - // )); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead. + SequentialCommandGroup weirdAutoShootingGroup = new SequentialCommandGroup(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true), + new ParallelCommandGroup( + new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true), + new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 2.0) + )); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead. - // return new SequentialCommandGroup( new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2((219.25 / 2.00) - turretDistanceFromFront, (82.83 / 2.00) - 15.56)), m_robotTurret), 1.0, true), // * aim with turret to target - // weirdAutoShootingGroup, // * shoot - // new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); // * stop running storage - // ! SHOOT FIRST BALL, THEN DRIVE OFF LINE (HOPEFULLY) - return new SequentialCommandGroup( //new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2((219.25 / 2.00) - turretDistanceFromFront, (82.83 / 2.00) - 15.56)), m_robotTurret), 1.0, true), // * aim with turret to target - // weirdAutoShootingGroup, // * shoot - // new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), // * stop running storage + // ! DRIVE OFF LINE, THEN SHOOT BALL (HOPEFULLY) + return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive), // * reset gyro before moving + new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.0, 0.5, 0.0, 0.0}, 0.5), // * drive out of tarmac + new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive), // * brake - new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive), // * reset gyro before moving - new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.0, 0.6, 0.0, 0.0}, 0.5))); // * drive off line + new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID((180.0 / Math.PI) * Math.atan2(-(82.83 / 2.00) + 15.56, -(219.25 / 2.00) - 40.44 + 10.00)), m_robotTurret), 1.0, true), // * aim with turret to target + weirdAutoShootingGroup, // * shoot + new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage) // * stop running storage + ); // ! TWO BALL AUTO (HOPEFULLY) - // return new SequentialCommandGroup( new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2((219.25 / 2.00) - turretDistanceFromFront, (82.83 / 2.00) - 15.56)), m_robotTurret), 1.0, true), // * aim with turret to target - // weirdAutoShootingGroup, // * shoot - // new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), // * stop running storage - - // new ExtenderIntakeGroup(m_robotIntake, m_robotExtender), // * extend out, in preparation of running intake + // return new SequentialCommandGroup( new ExtenderIntakeGroup(m_robotIntake, m_robotExtender), // * extend out, in preparation of running intake // new ParallelCommandGroup( new RunCommand(() -> m_robotIntake.runAtOutput(1.0), m_robotIntake), // * run intake all throughout path @@ -506,7 +541,7 @@ public class RobotContainer { // // new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, -1.0, 0.0, 0.0, true)), // * brake (see line 376), // new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive), - // new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2(firstBallPosition.y, firstBallPosition.x)), m_robotTurret), 1.0, true), // * aim with turret to target + // new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID((180.0 / Math.PI) * Math.atan2(-(82.83 / 2.00) + 15.56, -(219.25 / 2.00) - 40.44 + 10.00)), m_robotTurret), 1.0, true), // * aim with turret to target // weirdAutoShootingGroup, // * shoot // new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)))); // * stop running storage diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java index 20aec65..97e1913 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java @@ -35,7 +35,7 @@ public class TrackTarget extends CommandBase { BoomBoom m_boomBoom; Hood m_hood; - // boolean isAuto; + boolean isAuto; static double velocity; static double hoodPosition; @@ -55,22 +55,22 @@ public class TrackTarget extends CommandBase { long startTime; private double timeTolerance; - public TrackTarget (Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry/*, boolean isAuto*/) { + public TrackTarget (Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry, boolean isAuto) { m_turret = turret; m_boomBoom = boomBoom; m_hood = hood; m_visionOdometry = visionOdometry; - // this.isAuto = isAuto; + this.isAuto = isAuto; this.timeTolerance = 1000; addRequirements(m_turret, m_boomBoom, m_hood, m_visionOdometry); SmartDashboard.putNumber("Distance Adjust", -35); } - // public TrackTarget(Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry) { - // this(turret, boomBoom, hood, visionOdometry, false); - // } + public TrackTarget(Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry) { + this(turret, boomBoom, hood, visionOdometry, false); + } // Called when the command is initially scheduled. @Override @@ -195,14 +195,14 @@ public class TrackTarget extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - // if (this.isAuto) { - // if (targetLocked && !timerStarted) { - // timerStarted = true; - // startTime = System.currentTimeMillis(); - // } - // return (targetLocked && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance)); - // } else { + if (this.isAuto) { + if (targetLocked && !timerStarted) { + timerStarted = true; + startTime = System.currentTimeMillis(); + } + return (targetLocked && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance)); + } else { return false; - // } + } } } diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index 51f675d..0978374 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -31,6 +31,7 @@ import frc4388.utility.Gains; import frc4388.utility.NumericData; public class BoomBoom extends SubsystemBase { + public boolean readManualData = true; private static final Logger LOGGER = Logger.getLogger(BoomBoom.class.getSimpleName()); public WPI_TalonFX m_shooterFalconLeft; public WPI_TalonFX m_shooterFalconRight; @@ -54,7 +55,7 @@ public class BoomBoom extends SubsystemBase { public Double distance, hoodExt, drumVelocity; } - private ShooterTableEntry[] m_shooterTable; + public ShooterTableEntry[] m_shooterTable; /** Creates a new BoomBoom, which has a drum shooter and angle adjuster. */ public BoomBoom(WPI_TalonFX shooterFalconLeft, WPI_TalonFX shooterFalconRight) { @@ -63,7 +64,7 @@ public class BoomBoom extends SubsystemBase { setShooterGains(); - m_shooterTable = readShooterTable(); + updateShooterTable(); // Run a helper method that logs the contents of the table on a new thread. new Thread(() -> LOGGER.fine(() -> CSV.ReflectionTable.create(m_shooterTable, RobotBase.isSimulation()))).start(); } @@ -76,7 +77,7 @@ public class BoomBoom extends SubsystemBase { * @return Drum Velocity in units per 100 ms */ public Double getVelocity(final Double distance) { - return NumericData.linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.drumVelocity).doubleValue(); + return readManualData ? SmartDashboard.getNumber("Manual Shooter Data/Drum Velocity", 0) : NumericData.linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.drumVelocity).doubleValue(); } /** @@ -87,7 +88,7 @@ public class BoomBoom extends SubsystemBase { * @return Hood extension in units */ public Double getHood(final Double distance) { - return NumericData.linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.hoodExt).doubleValue(); + return readManualData ? SmartDashboard.getNumber("Manual Shooter Data/Hood Extension", 0) : NumericData.linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.hoodExt).doubleValue(); } @Override @@ -139,7 +140,8 @@ public class BoomBoom extends SubsystemBase { // m_shooterFalconLeft.set(m_controller.calculate(m_shooterFalconLeft.get(), targetVel)); } - private static ShooterTableEntry[] readShooterTable() { + public void updateShooterTable() { + try { // This is a helper class that allows us to read a CSV file into a Java array. CSV csv = new CSV<>(ShooterTableEntry::new) { @@ -162,14 +164,14 @@ public class BoomBoom extends SubsystemBase { } }; // This is reading the CSV file into a Java array. - return csv.read(new File(Filesystem.getDeployDirectory(), "ShooterData.csv").toPath()); + m_shooterTable = csv.read(new File(Filesystem.getDeployDirectory(), "ShooterData.csv").toPath()); } catch (IOException exception) { ShooterTableEntry dummyEntry = new ShooterTableEntry(); dummyEntry.distance = 0.0; dummyEntry.hoodExt = 0.0; dummyEntry.drumVelocity = 0.0; LOGGER.log(Level.SEVERE, "Exception while reading shooter CSV table.", exception); - return new ShooterTableEntry[] { dummyEntry }; + m_shooterTable = new ShooterTableEntry[] { dummyEntry }; } }