From 049eeeb30be8f4f6b023f2d81105ccb71810a7fd Mon Sep 17 00:00:00 2001 From: Caleb Distel Date: Mon, 31 Oct 2022 15:36:33 -0700 Subject: [PATCH 1/4] Added controller recording and following --- .../org/team498/C2022/RobotContainer.java | 10 +- .../java/org/team498/C2022/ShooterTable.java | 19 --- src/main/java/org/team498/C2022/Tables.java | 26 ++++ .../C2022/commands/auto/TableFollower.java | 62 +++++++++ .../org/team498/C2022/subsystems/Hood.java | 5 +- .../org/team498/C2022/subsystems/Shooter.java | 4 +- .../java/org/team498/app/ControllerApp.java | 65 +++++++++ .../team498/lib/util/DriveInterpolator.java | 128 ++++++++++++++++++ 8 files changed, 294 insertions(+), 25 deletions(-) delete mode 100644 src/main/java/org/team498/C2022/ShooterTable.java create mode 100644 src/main/java/org/team498/C2022/Tables.java create mode 100644 src/main/java/org/team498/C2022/commands/auto/TableFollower.java create mode 100644 src/main/java/org/team498/app/ControllerApp.java create mode 100644 src/main/java/org/team498/lib/util/DriveInterpolator.java diff --git a/src/main/java/org/team498/C2022/RobotContainer.java b/src/main/java/org/team498/C2022/RobotContainer.java index 2d1fc74..d15d018 100644 --- a/src/main/java/org/team498/C2022/RobotContainer.java +++ b/src/main/java/org/team498/C2022/RobotContainer.java @@ -6,12 +6,14 @@ import static org.team498.C2022.Constants.OIConstants.kDriverControllerID; import static org.team498.C2022.Constants.OIConstants.kOperatorControllerID; +import org.team498.C2022.Tables.DriveTables; import org.team498.C2022.commands.CalibrateGyro; import org.team498.C2022.commands.LimelightTestingSetup; import org.team498.C2022.commands.ResetGyro; import org.team498.C2022.commands.RumbleControllerLeft; import org.team498.C2022.commands.RumbleControllerRight; import org.team498.C2022.commands.auto.StateChampsTwoBall; +import org.team498.C2022.commands.auto.TableFollower; import org.team498.C2022.commands.centerer.ToggleCenterer; import org.team498.C2022.commands.climber.SetClimber; import org.team498.C2022.commands.climber.TuneClimber; @@ -40,6 +42,7 @@ import org.team498.C2022.subsystems.Shooter.ShooterSetting; import org.team498.C2022.subsystems.Wrist; import org.team498.C2022.subsystems.Wrist.WristState; +import org.team498.app.ControllerApp; import org.team498.lib.drivers.Limelight; import edu.wpi.first.wpilibj.XboxController; @@ -185,6 +188,9 @@ private void configureButtonBindings() { .whileActiveOnce(new ToggleHopper(hopper, HopperSetting.OUTTAKETOP)) .whenInactive(new SetShooter(shooter, 0)); + new JoystickButton(operatorController, Button.kStart.value) + .toggleWhenActive(new ControllerApp(driverController, "trajectory1.csv")); + //TODO: Test the input recorder new JoystickButton(driverController, Button.kRightBumper.value) // .and(flywheelAtSpeed) .whileActiveContinuous(new ToggleHopper(hopper, HopperSetting.LOAD)); @@ -234,7 +240,9 @@ private void configureButtonBindings() { } public Command getAutoCommand() { - return new StateChampsTwoBall(drivetrain, hood, shooter, hopper, intake, wrist, centerer, limelight); + return + new TableFollower(drivetrain, DriveTables.trajectory1); + //new StateChampsTwoBall(drivetrain, hood, shooter, hopper, intake, wrist, centerer, limelight); } public Command getRobotInitCommand() { diff --git a/src/main/java/org/team498/C2022/ShooterTable.java b/src/main/java/org/team498/C2022/ShooterTable.java deleted file mode 100644 index ac9620f..0000000 --- a/src/main/java/org/team498/C2022/ShooterTable.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team498.C2022; - -public class ShooterTable { - public static final double[][] shooterRPMTable = { - { 62, 1500 }, - { 93, 1500 }, - { 120, 2000 }, - { 138, 2200 }, - { 148, 2500 } - }; - - public static final double[][] hoodAngleTable = { - { 62, 0.2 }, - { 93, 0.4 }, - { 120, 0.4 }, - { 138, 0.5 }, - { 148, 0.6 } - }; -} diff --git a/src/main/java/org/team498/C2022/Tables.java b/src/main/java/org/team498/C2022/Tables.java new file mode 100644 index 0000000..9072c62 --- /dev/null +++ b/src/main/java/org/team498/C2022/Tables.java @@ -0,0 +1,26 @@ +package org.team498.C2022; + +public class Tables { + public static class ShooterTable { + public static final double[][] shooterRPMTable = { + { 62, 1500 }, + { 93, 1500 }, + { 120, 2000 }, + { 138, 2200 }, + { 148, 2500 } + }; + + public static final double[][] hoodAngleTable = { + { 62, 0.2 }, + { 93, 0.4 }, + { 120, 0.4 }, + { 138, 0.5 }, + { 148, 0.6 } + }; + } + public static class DriveTables { + public static final double[][] trajectory1 = { + + }; + } +} diff --git a/src/main/java/org/team498/C2022/commands/auto/TableFollower.java b/src/main/java/org/team498/C2022/commands/auto/TableFollower.java new file mode 100644 index 0000000..f8be36d --- /dev/null +++ b/src/main/java/org/team498/C2022/commands/auto/TableFollower.java @@ -0,0 +1,62 @@ +package org.team498.C2022.commands.auto; + +import org.team498.C2022.Tables.DriveTables; +import org.team498.C2022.subsystems.Drivetrain; +import org.team498.lib.util.DriveInterpolator; + +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class TableFollower extends CommandBase { + private Drivetrain drivetrain; + private Timer timer = new Timer(); + private DriveInterpolator interpolator; + private double time; + + private static double deadzone = 0.1; + private double driveSpeed = 2; + + public TableFollower(Drivetrain drivetrain, double[][] trajectory) { + time = 0; + this.drivetrain = drivetrain; + interpolator = new DriveInterpolator(DriveTables.trajectory1); + addRequirements(drivetrain); + } + @Override + public void initialize() { + timer.start(); + } + + @Override + public void execute() { + time = timer.get(); + double[] state = getState(time); + drivetrain.drive( + ChassisSpeeds.fromFieldRelativeSpeeds( + deadzone(((state[0] * driveSpeed) * (state[0] * driveSpeed)) * state[0], deadzone), + deadzone(((state[1] * driveSpeed) * (state[1] * driveSpeed)) * state[1], deadzone), + deadzone(((state[2] * driveSpeed) * (state[2] * driveSpeed)) * state[2], deadzone), + drivetrain.getGyroAngle())); + drivetrain.drive(new ChassisSpeeds(state[0], state[1], state[2])); + } + @Override + public void end(boolean interrupted) { + drivetrain.drive(new ChassisSpeeds()); + timer.stop(); + timer.reset(); + } + @Override + public boolean isFinished() { + return interpolator.isFinished(time); + } + + private double[] getState(double time) { + return interpolator.getInterpolatedValue(time); + } + private double deadzone(double input, double deadzone) { + if (Math.abs(input) > deadzone) + return input; + return 0; + } +} diff --git a/src/main/java/org/team498/C2022/subsystems/Hood.java b/src/main/java/org/team498/C2022/subsystems/Hood.java index b81410b..58bd3e7 100644 --- a/src/main/java/org/team498/C2022/subsystems/Hood.java +++ b/src/main/java/org/team498/C2022/subsystems/Hood.java @@ -1,8 +1,7 @@ package org.team498.C2022.subsystems; import static org.team498.C2022.Constants.HoodConstants.kHoodLimitDIO; -import static org.team498.C2022.ShooterTable.hoodAngleTable; - +import org.team498.C2022.Tables.ShooterTable; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMax.SoftLimitDirection; @@ -26,7 +25,7 @@ public class Hood extends SubsystemBase { // private final SparkMaxPIDController PID1; private ControlMode currentControlMode = ControlMode.PID; private final Limelight limelight; - private LinearInterpolator interpolator = new LinearInterpolator(hoodAngleTable); + private LinearInterpolator interpolator = new LinearInterpolator(ShooterTable.hoodAngleTable); public Hood(Limelight limelight) { motor = new CANSparkMax(36, MotorType.kBrushless); diff --git a/src/main/java/org/team498/C2022/subsystems/Shooter.java b/src/main/java/org/team498/C2022/subsystems/Shooter.java index 0bd63f6..a86d78c 100644 --- a/src/main/java/org/team498/C2022/subsystems/Shooter.java +++ b/src/main/java/org/team498/C2022/subsystems/Shooter.java @@ -2,8 +2,8 @@ import static org.team498.C2022.Constants.ShooterConstants.kBackShooterID; import static org.team498.C2022.Constants.ShooterConstants.kFrontShooterID; -import static org.team498.C2022.ShooterTable.shooterRPMTable; +import org.team498.C2022.Tables.ShooterTable; import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.FollowerType; @@ -19,7 +19,7 @@ public class Shooter extends SubsystemBase { private final TalonFX leftMotor = new TalonFX(kFrontShooterID); private final TalonFX rightMotor = new TalonFX(kBackShooterID); - private final LinearInterpolator interpolator = new LinearInterpolator(shooterRPMTable); + private final LinearInterpolator interpolator = new LinearInterpolator(ShooterTable.shooterRPMTable); public final Limelight limelight; public enum ShooterSetting { diff --git a/src/main/java/org/team498/app/ControllerApp.java b/src/main/java/org/team498/app/ControllerApp.java new file mode 100644 index 0000000..19a1c89 --- /dev/null +++ b/src/main/java/org/team498/app/ControllerApp.java @@ -0,0 +1,65 @@ +package org.team498.app; + +import java.io.FileNotFoundException; +import java.io.FileOutputStream; +import java.io.IOException; +import java.io.PrintWriter; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class ControllerApp extends CommandBase{ + //normally, I would make this a separate thing, but we want to run it at the same time as the robot + //also this is the easiest way to access the controllers lol + private FileOutputStream outFS; + private PrintWriter printer; + private XboxController controller; + private String fileName; + private Timer timer = new Timer(); + public ControllerApp(XboxController controller, String fileName) { + this.controller = controller; + this.fileName = fileName; + openFile(); + } + @Override + public void initialize() { + //printer.print("{\n"); + timer.start(); + } + @Override + public void execute() { + double timeStamp = timer.get(); + double dx = controller.getLeftY(); + double dy = controller.getLeftX(); + double dr = controller.getRightX(); + printInfo(timeStamp, dx, dy, dr); + } + @Override + public void end(boolean interrupted) { + //printer.print("\n}"); + closeFile(); + timer.stop(); + timer.reset(); + } + + private void printInfo(double timeStamp, double dx, double dy, double dr) { + printer.printf("{ %.3f, %.4f, %.4f, %.4f },\n", timeStamp, dx, dy, dr); + } + + private void openFile() { + try { + outFS = new FileOutputStream(fileName); + } catch (FileNotFoundException e) { + e.printStackTrace(); + } + printer = new PrintWriter(outFS); + } + private void closeFile() { + try { + outFS.close(); + } catch (IOException e) { + e.printStackTrace(); + } + } +} diff --git a/src/main/java/org/team498/lib/util/DriveInterpolator.java b/src/main/java/org/team498/lib/util/DriveInterpolator.java new file mode 100644 index 0000000..17667f9 --- /dev/null +++ b/src/main/java/org/team498/lib/util/DriveInterpolator.java @@ -0,0 +1,128 @@ +package org.team498.lib.util; + +import java.util.Arrays; + +/* + * linearInterpolator - given a table of x and y values, will interpolate values + * of y between known values of x using linear interpolation. + * + * Usage: private double[][] data = { {1.0, 10.0}, {3.0, 31.0}, {10,100} }; + * private linearInterpolator lt = new linearInterpolator(data); + * + * double y = lt.getInterpolatedValue(1.5); // returns 15.25 + * + * Credit: FRC team 5013 + */ +public class DriveInterpolator { + + // https://therevisionist.org/software-engineering/java/tutorials/passing-2d-arrays/ + private double[][] table; + private boolean initialized = false; + private int flexIndex = 0; + + /** + * create linearInterpolator class + * + * @param data, a table of x -> y mappings to be interpolated + */ + public DriveInterpolator(double[][] data) { + build_table(data); + } + + public boolean isInitialized() { + return initialized; + } + + /** + * Build the internal representation of the table data. + * + * @param data a table of data to be interpolated + */ + private void build_table(double[][] data) { + int rows = data.length; + int cols = data[0].length; + + table = new double[rows][cols]; + for (int x = 0; x < data.length; x++) { + for (int y = 0; y < data[x].length; y++) { + table[x][y] = data[x][y]; + } + } + Arrays.sort(table, (a, b) -> Double.compare(a[0], b[0])); + initialized = true; + } + + /** + * getInterpolatedValue() - return the interpolated value of y given x. + * + * If the value of x is in the table, that value is returned. + * + * If the value of x is not in the table, the closest two values of x are chosen + * and the value of y returned is interpolated between the corresponding y + * values. + * + * If the value of x is greater than max x value, the corresponding value of y + * for max x is returned. If the value of x is less than the min x value, the + * corresponding value of y for the min x is returned. + * + * @param x, the value of x to get an interpolated y value for + * @return the linear interpolated value y + */ + public boolean isFinished(double time) { + if (flexIndex >= table.length) { + return true; + } + return false; + } + public double[] getInterpolatedValue(double x) { + + if (!initialized) { + System.out.println("ERROR: Not Initialized"); + return new double[0]; + } + + int index = getIndex(x); + + // System.out.println("index of " + x + " is " + index); + + if (index >= table.length) { + return table[table.length - 1]; + } + double high_x = table[index][1]; + double high_y = table[index][2]; + double high_r = table[index][3]; + double high_t = table[index][0]; + if ((high_t == x) || (index == 0)) { + return new double[] {high_t, high_x, high_y, high_r}; + } + double low_x = table[index - 1][1]; + double low_y = table[index - 1][2]; + double low_r = table[index - 1][3]; + double low_t = table[index - 1][0]; + + return new double[] { + (low_x + (x - low_t) * (high_x - low_x) / (high_t - low_t)), + (low_y + (x - low_t) * (high_y - low_y) / (high_t - low_t)), + (low_r + (x - low_t) * (high_r - low_r) / (high_t - low_t)) + }; + } + //changed from linear search to binary -- since we use massive tables, we want it to be faster + public int getIndex(double time) { + int min = 0; + int max = table.length - 1; + int mid = (max + min) / 2; + while (time <= table[mid][0] - 0.1 || time >= table[mid][0] + 0.1) { + if (time > table[mid][0]) { + min = mid; + } else if (time < table[mid][0]) { + max = mid; + } + mid = min + max / 2; + } + flexIndex = mid; + return mid; + } + public int getFlexIndex() { + return flexIndex; + } +} \ No newline at end of file From 8f8334790efa8fee96b5a77b7052dcb12cb8893b Mon Sep 17 00:00:00 2001 From: Caleb Distel Date: Mon, 31 Oct 2022 22:26:17 -0700 Subject: [PATCH 2/4] Update RobotContainer.java --- src/main/java/org/team498/C2022/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/team498/C2022/RobotContainer.java b/src/main/java/org/team498/C2022/RobotContainer.java index d15d018..115e2ae 100644 --- a/src/main/java/org/team498/C2022/RobotContainer.java +++ b/src/main/java/org/team498/C2022/RobotContainer.java @@ -189,7 +189,7 @@ private void configureButtonBindings() { .whenInactive(new SetShooter(shooter, 0)); new JoystickButton(operatorController, Button.kStart.value) - .toggleWhenActive(new ControllerApp(driverController, "trajectory1.csv")); + .toggleWhenActive(new ControllerApp(driverController, "trajectory1.txt")); //TODO: Test the input recorder new JoystickButton(driverController, Button.kRightBumper.value) // .and(flywheelAtSpeed) From 8ee5262cff119eaf64f6087bd68b8f66c5c1fe65 Mon Sep 17 00:00:00 2001 From: Nsl106 <87742096+Nsl106@users.noreply.github.com> Date: Tue, 1 Nov 2022 13:55:01 -0700 Subject: [PATCH 3/4] --- .../org/team498/C2022/RobotContainer.java | 9 +- src/main/java/org/team498/C2022/Tables.java | 494 ++++++++++++++++++ .../C2022/commands/auto/TableFollower.java | 6 +- .../java/org/team498/app/ControllerApp.java | 39 +- 4 files changed, 531 insertions(+), 17 deletions(-) diff --git a/src/main/java/org/team498/C2022/RobotContainer.java b/src/main/java/org/team498/C2022/RobotContainer.java index 115e2ae..206f9ca 100644 --- a/src/main/java/org/team498/C2022/RobotContainer.java +++ b/src/main/java/org/team498/C2022/RobotContainer.java @@ -12,7 +12,6 @@ import org.team498.C2022.commands.ResetGyro; import org.team498.C2022.commands.RumbleControllerLeft; import org.team498.C2022.commands.RumbleControllerRight; -import org.team498.C2022.commands.auto.StateChampsTwoBall; import org.team498.C2022.commands.auto.TableFollower; import org.team498.C2022.commands.centerer.ToggleCenterer; import org.team498.C2022.commands.climber.SetClimber; @@ -112,13 +111,13 @@ private void configureButtonBindings() { new JoystickButton(operatorController, Button.kY.value).whileHeld(new LimelightAlign(drivetrain, limelight)); - new JoystickButton(driverController, Button.kBack.value).whenPressed(new SequentialCommandGroup( + new JoystickButton(driverController, Button.kBack.value).whenPressed( // new ParabolicTrajectory(drivetrain, 1, 1, 0, 0, 1); // new LinearTrajectory(drivetrain, 2, 2, 0, 1), // new LinearTrajectory(drivetrain, 2, -2, 0, 2), // new LinearTrajectory(drivetrain, 0, 2, 0, 2) - - new StateChampsTwoBall(drivetrain, hood, shooter, hopper, intake, wrist, centerer, limelight))); + new TableFollower(drivetrain, Tables.DriveTables.trajectory1)); + //new StateChampsTwoBall(drivetrain, hood, shooter, hopper, intake, wrist, centerer, limelight))); // new RotatingLinearTrajectory(drivetrain, 0, 0, 90, 10))); new JoystickButton(driverController, Button.kStart.value).toggleWhenActive(new SequentialCommandGroup( @@ -188,7 +187,7 @@ private void configureButtonBindings() { .whileActiveOnce(new ToggleHopper(hopper, HopperSetting.OUTTAKETOP)) .whenInactive(new SetShooter(shooter, 0)); - new JoystickButton(operatorController, Button.kStart.value) + new JoystickButton(operatorController, Button.kBack.value) .toggleWhenActive(new ControllerApp(driverController, "trajectory1.txt")); //TODO: Test the input recorder new JoystickButton(driverController, Button.kRightBumper.value) diff --git a/src/main/java/org/team498/C2022/Tables.java b/src/main/java/org/team498/C2022/Tables.java index 9072c62..f138763 100644 --- a/src/main/java/org/team498/C2022/Tables.java +++ b/src/main/java/org/team498/C2022/Tables.java @@ -18,8 +18,502 @@ public static class ShooterTable { { 148, 0.6 } }; } + public static class DriveTables { public static final double[][] trajectory1 = { + { 0.001, -0.0781, 0.0236, 0.1181 }, + { 0.021, -0.0859, 0.0315, 0.1181 }, + { 0.040, -0.0859, 0.0315, 0.1181 }, + { 0.061, -0.0859, 0.0315, 0.1181 }, + { 0.080, -0.0859, 0.0315, 0.1181 }, + { 0.100, -0.0859, 0.0315, 0.1181 }, + { 0.120, -0.0859, 0.0315, 0.1181 }, + { 0.140, -0.0859, 0.0315, 0.1181 }, + { 0.161, -0.0859, 0.0236, 0.1181 }, + { 0.181, -0.0859, -0.0078, 0.1181 }, + { 0.200, -0.0781, -0.0313, 0.1181 }, + { 0.220, -0.0547, -0.0703, 0.1181 }, + { 0.240, -0.0469, -0.0859, 0.1181 }, + { 0.261, -0.0313, -0.1094, 0.1181 }, + { 0.281, -0.0156, -0.1328, 0.1181 }, + { 0.301, -0.0078, -0.1641, 0.1181 }, + { 0.320, -0.0078, -0.1641, 0.1181 }, + { 0.340, -0.0078, -0.1875, 0.1181 }, + { 0.363, -0.0078, -0.1953, 0.1181 }, + { 0.381, -0.0078, -0.1953, 0.1181 }, + { 0.401, -0.0078, -0.1953, 0.1181 }, + { 0.420, -0.0078, -0.1953, 0.1181 }, + { 0.441, -0.0078, -0.2031, 0.1181 }, + { 0.462, -0.0078, -0.2031, 0.1181 }, + { 0.485, -0.0078, -0.2031, 0.1181 }, + { 0.559, 0.0000, -0.2656, 0.1260 }, + { 0.571, 0.0000, -0.2891, 0.1260 }, + { 0.585, 0.0079, -0.3359, 0.1260 }, + { 0.596, 0.0079, -0.3359, 0.1260 }, + { 0.607, 0.0079, -0.3359, 0.1260 }, + { 0.620, 0.0079, -0.3359, 0.1260 }, + { 0.632, 0.0079, -0.3672, 0.1260 }, + { 0.644, 0.0079, -0.3672, 0.1260 }, + { 0.660, 0.0079, -0.3828, 0.1260 }, + { 0.680, 0.0157, -0.3828, 0.1260 }, + { 0.700, 0.0157, -0.3828, 0.1260 }, + { 0.721, 0.0157, -0.3828, 0.1260 }, + { 0.741, 0.0157, -0.3906, 0.1260 }, + { 0.761, 0.0157, -0.3906, 0.1260 }, + { 0.785, 0.0157, -0.3906, 0.1260 }, + { 0.800, 0.0157, -0.3906, 0.1260 }, + { 0.820, 0.0157, -0.3906, 0.1260 }, + { 0.841, 0.0157, -0.3906, 0.1260 }, + { 0.861, 0.0157, -0.3906, 0.1260 }, + { 0.881, 0.0157, -0.3906, 0.1260 }, + { 0.900, 0.0236, -0.3906, 0.1260 }, + { 0.920, 0.0236, -0.3906, 0.1260 }, + { 0.941, 0.0236, -0.3984, 0.1260 }, + { 0.961, 0.0236, -0.3984, 0.1260 }, + { 0.981, 0.0394, -0.4141, 0.1260 }, + { 1.001, 0.0472, -0.4219, 0.1260 }, + { 1.021, 0.0472, -0.4297, 0.1260 }, + { 1.041, 0.0472, -0.4297, 0.1260 }, + { 1.061, 0.0551, -0.4297, 0.1260 }, + { 1.082, 0.0551, -0.4297, 0.1260 }, + { 1.102, 0.0551, -0.4297, 0.1260 }, + { 1.120, 0.0551, -0.4375, 0.1260 }, + { 1.141, 0.0630, -0.4453, 0.1260 }, + { 1.160, 0.0630, -0.4453, 0.1260 }, + { 1.180, 0.0630, -0.4531, 0.1260 }, + { 1.201, 0.0630, -0.4609, 0.1260 }, + { 1.221, 0.0709, -0.4844, 0.1260 }, + { 1.241, 0.0866, -0.5156, 0.1181 }, + { 1.260, 0.0945, -0.5469, 0.1181 }, + { 1.280, 0.0945, -0.5625, 0.1339 }, + { 1.303, 0.0945, -0.5781, 0.1181 }, + { 1.321, 0.0945, -0.5781, 0.1181 }, + { 1.341, 0.0945, -0.5781, 0.1181 }, + { 1.361, 0.0945, -0.5781, 0.1181 }, + { 1.381, 0.0945, -0.5938, 0.1181 }, + { 1.401, 0.0945, -0.5938, 0.1181 }, + { 1.421, 0.0945, -0.5938, 0.1181 }, + { 1.441, 0.0945, -0.5938, 0.1181 }, + { 1.461, 0.0945, -0.5938, 0.1181 }, + { 1.481, 0.0945, -0.5938, 0.1181 }, + { 1.502, 0.0945, -0.5938, 0.1181 }, + { 1.521, 0.0945, -0.5938, 0.1181 }, + { 1.541, 0.0945, -0.5938, 0.1181 }, + { 1.562, 0.0945, -0.5938, 0.1181 }, + { 1.581, 0.0945, -0.5938, 0.1181 }, + { 1.600, 0.0945, -0.5938, 0.1181 }, + { 1.621, 0.0945, -0.5938, 0.1181 }, + { 1.640, 0.0945, -0.5938, 0.1181 }, + { 1.662, 0.0945, -0.5938, 0.1181 }, + { 1.680, 0.0945, -0.5938, 0.1181 }, + { 1.700, 0.0945, -0.5938, 0.1181 }, + { 1.720, 0.0945, -0.5938, 0.1181 }, + { 1.740, 0.0945, -0.5938, 0.1181 }, + { 1.760, 0.0945, -0.5938, 0.1181 }, + { 1.781, 0.0866, -0.5938, 0.1181 }, + { 1.801, 0.0866, -0.5859, 0.1181 }, + { 1.821, 0.0866, -0.5859, 0.1181 }, + { 1.840, 0.0787, -0.5703, 0.1181 }, + { 1.861, 0.0787, -0.5703, 0.1181 }, + { 1.881, 0.0787, -0.5703, 0.1181 }, + { 1.901, 0.0709, -0.5625, 0.1181 }, + { 1.921, 0.0709, -0.5625, 0.1181 }, + { 1.941, 0.0630, -0.5625, 0.1181 }, + { 1.960, 0.0630, -0.5625, 0.1181 }, + { 1.980, 0.0551, -0.5625, 0.1181 }, + { 2.001, 0.0472, -0.5625, 0.1181 }, + { 2.022, 0.0472, -0.5625, 0.1181 }, + { 2.041, 0.0394, -0.5625, 0.1181 }, + { 2.060, 0.0394, -0.5625, 0.1181 }, + { 2.081, 0.0394, -0.5625, 0.1181 }, + { 2.101, 0.0394, -0.5625, 0.1181 }, + { 2.123, 0.0394, -0.5625, 0.1181 }, + { 2.141, 0.0394, -0.5625, 0.1181 }, + { 2.160, 0.0394, -0.5625, 0.1181 }, + { 2.181, 0.0394, -0.5625, 0.1181 }, + { 2.200, 0.0394, -0.5625, 0.1181 }, + { 2.223, 0.0394, -0.5625, 0.1181 }, + { 2.240, 0.0394, -0.5625, 0.1181 }, + { 2.268, 0.0394, -0.5625, 0.1181 }, + { 2.281, 0.0394, -0.5625, 0.1181 }, + { 2.301, 0.0394, -0.5625, 0.1181 }, + { 2.321, 0.0394, -0.5625, 0.1181 }, + { 2.340, 0.0394, -0.5625, 0.1181 }, + { 2.361, 0.0394, -0.5625, 0.1181 }, + { 2.381, 0.0472, -0.5625, 0.1181 }, + { 2.400, 0.0472, -0.5625, 0.1181 }, + { 2.422, 0.0472, -0.5625, 0.1181 }, + { 2.440, 0.0472, -0.5625, 0.1181 }, + { 2.460, 0.0472, -0.5625, 0.1181 }, + { 2.480, 0.0630, -0.5625, 0.1181 }, + { 2.501, 0.0630, -0.5625, 0.1181 }, + { 2.521, 0.0630, -0.5625, 0.1181 }, + { 2.540, 0.0630, -0.5625, 0.1181 }, + { 2.560, 0.0630, -0.5625, 0.1181 }, + { 2.580, 0.0709, -0.5625, 0.1181 }, + { 2.600, 0.0709, -0.5625, 0.1181 }, + { 2.622, 0.0709, -0.5625, 0.1181 }, + { 2.640, 0.0709, -0.5625, 0.1181 }, + { 2.661, 0.0709, -0.5625, 0.1181 }, + { 2.681, 0.0709, -0.5625, 0.1181 }, + { 2.701, 0.0709, -0.5625, 0.1181 }, + { 2.722, 0.0787, -0.5625, 0.1181 }, + { 2.740, 0.0787, -0.5625, 0.1181 }, + { 2.761, 0.0787, -0.5625, 0.1181 }, + { 2.780, 0.0866, -0.5625, 0.1181 }, + { 2.800, 0.0866, -0.5625, 0.1181 }, + { 2.821, 0.0866, -0.5625, 0.1181 }, + { 2.841, 0.0945, -0.5625, 0.1181 }, + { 2.861, 0.1024, -0.5625, 0.1181 }, + { 2.881, 0.1024, -0.5625, 0.1181 }, + { 2.900, 0.1024, -0.5625, 0.1181 }, + { 2.920, 0.1024, -0.5625, 0.1181 }, + { 2.941, 0.1024, -0.5625, 0.1181 }, + { 2.960, 0.1024, -0.5625, 0.1181 }, + { 2.981, 0.1024, -0.5625, 0.1181 }, + { 3.001, 0.1024, -0.5625, 0.1181 }, + { 3.020, 0.1024, -0.5625, 0.1181 }, + { 3.041, 0.1024, -0.5625, 0.1181 }, + { 3.061, 0.1024, -0.5625, 0.1181 }, + { 3.081, 0.1024, -0.5625, 0.1181 }, + { 3.102, 0.1024, -0.5625, 0.1181 }, + { 3.120, 0.1024, -0.5625, 0.1181 }, + { 3.140, 0.1024, -0.5625, 0.1181 }, + { 3.160, 0.1024, -0.5625, 0.1181 }, + { 3.180, 0.1024, -0.5625, 0.1181 }, + { 3.201, 0.1024, -0.5625, 0.1181 }, + { 3.222, 0.1024, -0.5625, 0.1181 }, + { 3.241, 0.1024, -0.5625, 0.1181 }, + { 3.261, 0.1024, -0.5625, 0.1181 }, + { 3.281, 0.1024, -0.5625, 0.1181 }, + { 3.301, 0.1024, -0.5625, 0.1181 }, + { 3.321, 0.1024, -0.5625, 0.1181 }, + { 3.343, 0.1024, -0.5625, 0.1181 }, + { 3.360, 0.1024, -0.5625, 0.1181 }, + { 3.381, 0.1024, -0.5625, 0.1181 }, + { 3.400, 0.1024, -0.5625, 0.1181 }, + { 3.421, 0.1024, -0.5625, 0.1181 }, + { 3.441, 0.1024, -0.5625, 0.1181 }, + { 3.461, 0.1024, -0.5625, 0.1181 }, + { 3.480, 0.1024, -0.5625, 0.1181 }, + { 3.501, 0.1024, -0.5625, 0.1181 }, + { 3.520, 0.1024, -0.5625, 0.1181 }, + { 3.542, 0.1024, -0.5625, 0.1181 }, + { 3.561, 0.0945, -0.5000, 0.1181 }, + { 3.581, 0.0236, -0.4141, 0.1181 }, + { 3.601, -0.0078, -0.2734, 0.1181 }, + { 3.620, -0.0234, -0.1406, 0.1181 }, + { 3.640, -0.0313, -0.0781, 0.1181 }, + { 3.661, -0.0313, -0.0781, 0.1181 }, + { 3.680, -0.0313, -0.0703, 0.1181 }, + { 3.700, -0.0313, -0.0625, 0.1181 }, + { 3.720, -0.0391, -0.0469, 0.1181 }, + { 3.747, -0.0391, -0.0234, 0.1181 }, + { 3.760, -0.0391, -0.0234, 0.1181 }, + { 3.781, -0.0391, -0.0234, 0.1181 }, + { 3.801, -0.0391, -0.0234, 0.1181 }, + { 3.821, -0.0313, -0.0234, 0.1181 }, + { 3.841, -0.0313, -0.0234, 0.1181 }, + { 3.860, -0.0313, -0.0234, 0.1181 }, + { 3.881, -0.0313, -0.0234, 0.1181 }, + { 3.901, -0.0313, -0.0234, 0.1181 }, + { 3.921, -0.0313, -0.0234, 0.1181 }, + { 3.941, -0.0313, -0.0234, 0.1181 }, + { 3.961, -0.0313, -0.0234, 0.1181 }, + { 3.980, -0.0313, -0.0234, 0.1181 }, + { 4.000, -0.0313, -0.0234, 0.1181 }, + { 4.020, -0.0313, -0.0234, 0.1181 }, + { 4.041, -0.0313, -0.0234, 0.1181 }, + { 4.062, -0.0313, -0.0234, 0.1181 }, + { 4.080, -0.0313, -0.0234, 0.1181 }, + { 4.100, -0.0313, -0.0234, 0.1181 }, + { 4.120, -0.0313, -0.0234, 0.1181 }, + { 4.140, -0.0313, -0.0234, 0.1181 }, + { 4.162, -0.0313, -0.0234, 0.1181 }, + { 4.181, -0.0313, -0.0234, 0.1181 }, + { 4.201, -0.0313, -0.0234, 0.1181 }, + { 4.220, -0.0313, -0.0234, 0.1181 }, + { 4.241, -0.0313, -0.0234, 0.1181 }, + { 4.260, -0.0313, -0.0234, 0.1181 }, + { 4.280, -0.0313, -0.0234, 0.1181 }, + { 4.302, -0.0313, -0.0234, 0.1181 }, + { 4.321, -0.0313, -0.0234, 0.1181 }, + { 4.341, -0.0313, -0.0234, 0.1181 }, + { 4.361, -0.0313, -0.0234, 0.1181 }, + { 4.381, -0.0313, -0.0234, 0.1181 }, + { 4.401, -0.0313, -0.0234, 0.1181 }, + { 4.421, -0.0313, -0.0234, 0.1181 }, + { 4.441, -0.0313, -0.0234, 0.1181 }, + { 4.461, -0.0313, -0.0234, 0.1181 }, + { 4.481, -0.0313, -0.0234, 0.1181 }, + { 4.500, -0.0313, -0.0234, 0.1181 }, + { 4.521, -0.0313, -0.0234, 0.1181 }, + { 4.541, -0.0313, -0.0234, 0.1181 }, + { 4.560, -0.0313, -0.0234, 0.1181 }, + { 4.580, -0.0313, -0.0234, 0.1181 }, + { 4.600, -0.0313, -0.0234, 0.1181 }, + { 4.620, -0.0313, -0.0234, 0.1181 }, + { 4.641, 0.0000, 0.0236, 0.1181 }, + { 4.661, 0.0157, 0.0315, 0.1181 }, + { 4.681, 0.0236, 0.0472, 0.1181 }, + { 4.700, 0.0315, 0.0551, 0.1181 }, + { 4.720, 0.0315, 0.0709, 0.1181 }, + { 4.741, 0.0394, 0.0709, 0.1181 }, + { 4.760, 0.0394, 0.0787, 0.1181 }, + { 4.781, 0.0394, 0.1102, 0.1181 }, + { 4.800, 0.0472, 0.1260, 0.1181 }, + { 4.820, 0.0472, 0.1417, 0.1181 }, + { 4.842, 0.0630, 0.1654, 0.1181 }, + { 4.861, 0.0630, 0.1811, 0.1181 }, + { 4.880, 0.0709, 0.2047, 0.1181 }, + { 4.929, 0.0945, 0.2677, 0.1181 }, + { 4.949, 0.0945, 0.3150, 0.1181 }, + { 4.960, 0.0945, 0.3150, 0.1181 }, + { 4.971, 0.0945, 0.3307, 0.1181 }, + { 4.984, 0.0945, 0.3307, 0.1181 }, + { 5.001, 0.0945, 0.3386, 0.1181 }, + { 5.020, 0.1024, 0.3465, 0.1181 }, + { 5.040, 0.1024, 0.3780, 0.1181 }, + { 5.061, 0.1102, 0.3858, 0.1181 }, + { 5.082, 0.1102, 0.3937, 0.1181 }, + { 5.101, 0.1102, 0.3937, 0.1181 }, + { 5.121, 0.1102, 0.3937, 0.1181 }, + { 5.141, 0.1102, 0.4016, 0.1181 }, + { 5.161, 0.1102, 0.4094, 0.1181 }, + { 5.180, 0.1102, 0.4173, 0.1181 }, + { 5.206, 0.1102, 0.4331, 0.1181 }, + { 5.221, 0.1102, 0.4331, 0.1181 }, + { 5.241, 0.1102, 0.4409, 0.1181 }, + { 5.261, 0.1102, 0.4567, 0.1181 }, + { 5.280, 0.1102, 0.4724, 0.1181 }, + { 5.300, 0.1102, 0.4724, 0.1181 }, + { 5.321, 0.1102, 0.4803, 0.1181 }, + { 5.341, 0.1102, 0.4882, 0.1181 }, + { 5.361, 0.1102, 0.4882, 0.1181 }, + { 5.380, 0.1102, 0.4882, 0.1181 }, + { 5.401, 0.1102, 0.4961, 0.1181 }, + { 5.421, 0.1102, 0.5039, 0.1181 }, + { 5.453, 0.1102, 0.5118, 0.1181 }, + { 5.466, 0.1102, 0.5276, 0.1181 }, + { 5.483, 0.1102, 0.5276, 0.1181 }, + { 5.507, 0.1181, 0.5276, 0.1181 }, + { 5.520, 0.1181, 0.5276, 0.1181 }, + { 5.541, 0.1181, 0.5433, 0.1181 }, + { 5.561, 0.1260, 0.5591, 0.1181 }, + { 5.581, 0.1260, 0.5748, 0.1181 }, + { 5.603, 0.1260, 0.5906, 0.1181 }, + { 5.621, 0.1260, 0.5906, 0.1181 }, + { 5.641, 0.1260, 0.6142, 0.1181 }, + { 5.661, 0.1260, 0.6142, 0.1181 }, + { 5.680, 0.1260, 0.6142, 0.1181 }, + { 5.700, 0.1260, 0.6142, 0.1181 }, + { 5.721, 0.1260, 0.6142, 0.1181 }, + { 5.740, 0.1260, 0.6142, 0.1181 }, + { 5.760, 0.1260, 0.6142, 0.1181 }, + { 5.781, 0.1260, 0.6063, 0.1181 }, + { 5.800, 0.1260, 0.6063, 0.1181 }, + { 5.821, 0.1260, 0.6063, 0.1181 }, + { 5.841, 0.1260, 0.6063, 0.1181 }, + { 5.861, 0.1260, 0.6142, 0.1181 }, + { 5.881, 0.1260, 0.6142, 0.1181 }, + { 5.901, 0.1260, 0.6142, 0.1181 }, + { 5.920, 0.1260, 0.6142, 0.1181 }, + { 5.941, 0.1260, 0.6142, 0.1181 }, + { 5.961, 0.1260, 0.6142, 0.1181 }, + { 5.980, 0.1260, 0.6142, 0.1181 }, + { 6.001, 0.1260, 0.6142, 0.1181 }, + { 6.021, 0.1260, 0.6142, 0.1181 }, + { 6.040, 0.1260, 0.6142, 0.1181 }, + { 6.060, 0.1260, 0.6142, 0.1181 }, + { 6.081, 0.1260, 0.6063, 0.1181 }, + { 6.100, 0.1260, 0.6142, 0.1181 }, + { 6.121, 0.1260, 0.6142, 0.1181 }, + { 6.140, 0.1260, 0.6142, 0.1181 }, + { 6.162, 0.1260, 0.6142, 0.1181 }, + { 6.181, 0.1260, 0.6142, 0.1181 }, + { 6.201, 0.1260, 0.6142, 0.1181 }, + { 6.220, 0.1260, 0.6142, 0.1181 }, + { 6.240, 0.1260, 0.6142, 0.1181 }, + { 6.261, 0.1260, 0.6142, 0.1181 }, + { 6.281, 0.1260, 0.6142, 0.1181 }, + { 6.300, 0.1260, 0.6142, 0.1181 }, + { 6.322, 0.1260, 0.6142, 0.1181 }, + { 6.341, 0.1260, 0.6142, 0.1181 }, + { 6.360, 0.1260, 0.6142, 0.1181 }, + { 6.381, 0.1260, 0.6142, 0.1181 }, + { 6.401, 0.1260, 0.6142, 0.1181 }, + { 6.422, 0.1260, 0.6142, 0.1181 }, + { 6.440, 0.1260, 0.6142, 0.1181 }, + { 6.461, 0.1260, 0.6142, 0.1181 }, + { 6.480, 0.1260, 0.6142, 0.1181 }, + { 6.501, 0.1339, 0.6142, 0.1181 }, + { 6.523, 0.1339, 0.6142, 0.1181 }, + { 6.541, 0.1339, 0.6142, 0.1181 }, + { 6.560, 0.1339, 0.6142, 0.1181 }, + { 6.581, 0.1339, 0.6142, 0.1181 }, + { 6.600, 0.1339, 0.6142, 0.1181 }, + { 6.620, 0.1339, 0.6142, 0.1181 }, + { 6.641, 0.1339, 0.6142, 0.1181 }, + { 6.661, 0.1417, 0.6142, 0.1181 }, + { 6.683, 0.1417, 0.6142, 0.1181 }, + { 6.701, 0.1417, 0.6142, 0.1181 }, + { 6.720, 0.1417, 0.6142, 0.1181 }, + { 6.740, 0.1417, 0.6142, 0.1181 }, + { 6.760, 0.1417, 0.6142, 0.1181 }, + { 6.781, 0.1417, 0.6142, 0.1181 }, + { 6.811, 0.1496, 0.6142, 0.1181 }, + { 6.862, 0.1496, 0.6142, 0.1181 }, + { 6.873, 0.1575, 0.6142, 0.1181 }, + { 6.884, 0.1575, 0.6142, 0.1181 }, + { 6.894, 0.1575, 0.6142, 0.1181 }, + { 6.904, 0.1575, 0.6142, 0.1181 }, + { 6.921, 0.1575, 0.6142, 0.1181 }, + { 6.941, 0.1575, 0.6142, 0.1181 }, + { 6.961, 0.1575, 0.6142, 0.1181 }, + { 6.981, 0.1575, 0.6142, 0.1181 }, + { 7.001, 0.1654, 0.6142, 0.1181 }, + { 7.021, 0.1654, 0.6142, 0.1181 }, + { 7.042, 0.1654, 0.6142, 0.1181 }, + { 7.060, 0.1654, 0.6142, 0.1181 }, + { 7.081, 0.1654, 0.6142, 0.1181 }, + { 7.101, 0.1654, 0.6142, 0.1181 }, + { 7.123, 0.1654, 0.6142, 0.1181 }, + { 7.148, 0.1654, 0.6142, 0.1181 }, + { 7.177, 0.1654, 0.6142, 0.1181 }, + { 7.208, 0.1654, 0.6142, 0.1181 }, + { 7.262, 0.1654, 0.6063, 0.1181 }, + { 7.285, 0.1654, 0.6063, 0.1181 }, + { 7.296, 0.1654, 0.6063, 0.1181 }, + { 7.308, 0.1654, 0.6063, 0.1181 }, + { 7.318, 0.1654, 0.6063, 0.1181 }, + { 7.328, 0.1654, 0.6142, 0.1181 }, + { 7.337, 0.1654, 0.6142, 0.1181 }, + { 7.350, 0.1654, 0.6063, 0.1181 }, + { 7.361, 0.1654, 0.6063, 0.1181 }, + { 7.381, 0.1654, 0.6063, 0.1181 }, + { 7.401, 0.1654, 0.6063, 0.1181 }, + { 7.420, 0.1654, 0.6063, 0.1181 }, + { 7.440, 0.1654, 0.6063, 0.1181 }, + { 7.460, 0.1654, 0.6063, 0.1181 }, + { 7.481, 0.1654, 0.6063, 0.1181 }, + { 7.501, 0.1654, 0.6063, 0.1181 }, + { 7.520, 0.1654, 0.6063, 0.1181 }, + { 7.541, 0.1654, 0.6063, 0.1181 }, + { 7.560, 0.1654, 0.6063, 0.1181 }, + { 7.581, 0.1654, 0.6063, 0.1181 }, + { 7.602, 0.1654, 0.6063, 0.1181 }, + { 7.621, 0.1654, 0.6063, 0.1181 }, + { 7.640, 0.1654, 0.6063, 0.1181 }, + { 7.661, 0.1654, 0.6063, 0.1181 }, + { 7.681, 0.1654, 0.6063, 0.1181 }, + { 7.701, 0.1654, 0.6063, 0.1181 }, + { 7.720, 0.1654, 0.5827, 0.1181 }, + { 7.740, 0.1654, 0.5669, 0.1181 }, + { 7.762, 0.1654, 0.5276, 0.1181 }, + { 7.780, 0.1260, 0.4331, 0.1181 }, + { 7.801, 0.1024, 0.3937, 0.1181 }, + { 7.820, 0.0945, 0.3622, 0.1181 }, + { 7.840, 0.0866, 0.3228, 0.1181 }, + { 7.862, 0.0866, 0.3228, 0.1181 }, + { 7.881, 0.0709, 0.3071, 0.1181 }, + { 7.900, 0.0630, 0.2756, 0.1181 }, + { 7.920, -0.0156, 0.1417, 0.1181 }, + { 7.941, -0.0391, 0.0945, 0.1181 }, + { 7.962, -0.0391, 0.0787, 0.1181 }, + { 7.981, -0.0391, 0.0787, 0.1181 }, + { 8.001, -0.0391, 0.0787, 0.1181 }, + { 8.020, -0.0391, 0.0787, 0.1181 }, + { 8.040, -0.0391, 0.0787, 0.1181 }, + { 8.060, -0.0391, 0.0787, 0.1181 }, + { 8.080, -0.0391, 0.0787, 0.1181 }, + { 8.101, -0.0391, 0.0787, 0.1181 }, + { 8.123, -0.0391, 0.0787, 0.1181 }, + { 8.141, -0.0391, 0.0787, 0.1181 }, + { 8.160, -0.0391, 0.0787, 0.1181 }, + { 8.180, -0.0391, 0.0787, 0.1181 }, + { 8.200, -0.0391, 0.0787, 0.1181 }, + { 8.220, -0.0391, 0.0787, 0.1181 }, + { 8.240, -0.0391, 0.0787, 0.1181 }, + { 8.261, -0.0391, 0.0787, 0.1181 }, + { 8.280, -0.0391, 0.0787, 0.1181 }, + { 8.300, -0.0391, 0.0787, 0.1181 }, + { 8.321, -0.0391, 0.0787, 0.1181 }, + { 8.342, -0.0391, 0.0787, 0.1181 }, + { 8.361, -0.0391, 0.0787, 0.1181 }, + { 8.381, -0.0391, 0.0787, 0.1181 }, + { 8.400, -0.0391, 0.0787, 0.1181 }, + { 8.421, -0.0391, 0.0787, 0.1181 }, + { 8.441, -0.0391, 0.0787, 0.1181 }, + { 8.461, -0.0391, 0.0787, 0.1181 }, + { 8.481, -0.0391, 0.0787, 0.1181 }, + { 8.500, -0.0391, 0.0787, 0.1181 }, + { 8.520, -0.0391, 0.0787, 0.1181 }, + { 8.541, -0.0391, 0.0787, 0.1181 }, + { 8.561, -0.0391, 0.0787, 0.1181 }, + { 8.581, -0.0391, 0.0787, 0.1181 }, + { 8.600, -0.0391, 0.0787, 0.1181 }, + { 8.620, -0.0391, 0.0787, 0.1181 }, + { 8.640, -0.0391, 0.0787, 0.1181 }, + { 8.660, -0.0391, 0.0787, 0.1181 }, + { 8.682, -0.0391, 0.0787, 0.1181 }, + { 8.700, -0.0391, 0.0787, 0.1181 }, + { 8.720, -0.0391, 0.0787, 0.1181 }, + { 8.740, -0.0391, 0.0787, 0.1181 }, + { 8.760, -0.0391, 0.0787, 0.1181 }, + { 8.781, -0.0391, 0.0787, 0.1181 }, + { 8.801, -0.0391, 0.0787, 0.1181 }, + { 8.821, -0.0391, 0.0787, 0.1181 }, + { 8.840, -0.0391, 0.0787, 0.1181 }, + { 8.860, -0.0313, 0.0787, 0.1181 }, + { 8.880, -0.0313, 0.0787, 0.1181 }, + { 8.901, -0.0313, 0.0787, 0.1181 }, + { 8.920, -0.0313, 0.0787, 0.1181 }, + { 8.941, -0.0313, 0.0787, 0.1181 }, + { 8.960, -0.0313, 0.0787, 0.1181 }, + { 8.980, -0.0313, 0.0787, 0.1181 }, + { 9.000, -0.0313, 0.0787, 0.1181 }, + { 9.021, -0.0313, 0.0787, 0.1181 }, + { 9.040, -0.0313, 0.0787, 0.1181 }, + { 9.061, -0.0313, 0.0787, 0.1181 }, + { 9.081, -0.0313, 0.0787, 0.1181 }, + { 9.101, -0.0313, 0.0787, 0.1181 }, + { 9.122, -0.0313, 0.0787, 0.1181 }, + { 9.141, -0.0313, 0.0787, 0.1181 }, + { 9.160, -0.0313, 0.0787, 0.1181 }, + { 9.181, -0.0313, 0.0787, 0.1181 }, + { 9.202, -0.0391, 0.0787, 0.1181 }, + { 9.220, -0.0391, 0.0787, 0.1181 }, + { 9.241, -0.0391, 0.0787, 0.1181 }, + { 9.260, -0.0391, 0.0787, 0.1181 }, + { 9.281, -0.0391, 0.0787, 0.1181 }, + { 9.301, -0.0391, 0.0787, 0.1181 }, + { 9.321, -0.0391, 0.0787, 0.1181 }, + { 9.341, -0.0391, 0.0787, 0.1181 }, + { 9.360, -0.0391, 0.0787, 0.1181 }, + { 9.381, -0.0391, 0.0787, 0.1181 }, + { 9.402, -0.0391, 0.0787, 0.1181 }, + { 9.420, -0.0391, 0.0787, 0.1181 }, + { 9.440, -0.0391, 0.0787, 0.1181 }, + { 9.460, -0.0391, 0.0787, 0.1181 }, + { 9.481, -0.0391, 0.0787, 0.1181 }, + { 9.503, -0.0391, 0.0787, 0.1181 }, + { 9.521, -0.0391, 0.0787, 0.1181 }, + { 9.540, -0.0391, 0.0787, 0.1181 }, + { 9.560, -0.0391, 0.0787, 0.1181 }, + { 9.580, -0.0391, 0.0787, 0.1181 }, + { 9.602, -0.0313, 0.0787, 0.1181 }, + { 9.621, -0.0313, 0.0787, 0.1181 }, + { 9.647, -0.0313, 0.0787, 0.1181 }, + { 9.661, -0.0313, 0.0787, 0.1181 }, + { 9.681, -0.0313, 0.0787, 0.1181 }, + { 9.701, -0.0313, 0.0787, 0.1181 }, + { 9.721, -0.0313, 0.0787, 0.1181 }, + { 9.741, -0.0313, 0.0787, 0.1181 }, + { 9.760, -0.0391, 0.0787, 0.1181 }, + { 9.781, -0.0391, 0.0787, 0.1181 }, + { 9.801, -0.0391, 0.0787, 0.1181 }, + { 9.820, -0.0391, 0.0787, 0.1181 }, + { 9.840, -0.0391, 0.0787, 0.1181 } }; } diff --git a/src/main/java/org/team498/C2022/commands/auto/TableFollower.java b/src/main/java/org/team498/C2022/commands/auto/TableFollower.java index f8be36d..41b0725 100644 --- a/src/main/java/org/team498/C2022/commands/auto/TableFollower.java +++ b/src/main/java/org/team498/C2022/commands/auto/TableFollower.java @@ -6,6 +6,7 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; public class TableFollower extends CommandBase { @@ -23,6 +24,7 @@ public TableFollower(Drivetrain drivetrain, double[][] trajectory) { interpolator = new DriveInterpolator(DriveTables.trajectory1); addRequirements(drivetrain); } + @Override public void initialize() { timer.start(); @@ -32,6 +34,7 @@ public void initialize() { public void execute() { time = timer.get(); double[] state = getState(time); + SmartDashboard.putNumber("timer", time); drivetrain.drive( ChassisSpeeds.fromFieldRelativeSpeeds( deadzone(((state[0] * driveSpeed) * (state[0] * driveSpeed)) * state[0], deadzone), @@ -42,10 +45,11 @@ public void execute() { } @Override public void end(boolean interrupted) { - drivetrain.drive(new ChassisSpeeds()); + drivetrain.drive(new ChassisSpeeds(0,0,0)); timer.stop(); timer.reset(); } + @Override public boolean isFinished() { return interpolator.isFinished(time); diff --git a/src/main/java/org/team498/app/ControllerApp.java b/src/main/java/org/team498/app/ControllerApp.java index 19a1c89..cf5520d 100644 --- a/src/main/java/org/team498/app/ControllerApp.java +++ b/src/main/java/org/team498/app/ControllerApp.java @@ -1,7 +1,9 @@ package org.team498.app; +import java.io.File; import java.io.FileNotFoundException; import java.io.FileOutputStream; +import java.io.FileWriter; import java.io.IOException; import java.io.PrintWriter; @@ -12,14 +14,15 @@ public class ControllerApp extends CommandBase{ //normally, I would make this a separate thing, but we want to run it at the same time as the robot //also this is the easiest way to access the controllers lol - private FileOutputStream outFS; - private PrintWriter printer; + private FileWriter printer; private XboxController controller; - private String fileName; private Timer timer = new Timer(); + private File file; public ControllerApp(XboxController controller, String fileName) { this.controller = controller; - this.fileName = fileName; + + File filepath = new File("/home/lvuser/paths".replace('/', File.separatorChar)); + file = new File(filepath, fileName); openFile(); } @Override @@ -34,6 +37,8 @@ public void execute() { double dy = controller.getLeftX(); double dr = controller.getRightX(); printInfo(timeStamp, dx, dy, dr); + + System.out.println("running"); } @Override public void end(boolean interrupted) { @@ -44,20 +49,32 @@ public void end(boolean interrupted) { } private void printInfo(double timeStamp, double dx, double dy, double dr) { - printer.printf("{ %.3f, %.4f, %.4f, %.4f },\n", timeStamp, dx, dy, dr); + try { + printer.write(String.format("{ %.3f, %.4f, %.4f, %.4f },\n", timeStamp, dx, dy, dr)); + } catch (IOException e) { + e.printStackTrace(); + } + // printer.printf("{ %.3f, %.4f, %.4f, %.4f },\n", timeStamp, dx, dy, dr); } private void openFile() { + try { + file.createNewFile(); + } catch (IOException e1) { + e1.printStackTrace(); + } + + try { - outFS = new FileOutputStream(fileName); - } catch (FileNotFoundException e) { - e.printStackTrace(); - } - printer = new PrintWriter(outFS); + printer = new FileWriter(file, true); + } catch (IOException e) { + e.printStackTrace(); + } } + private void closeFile() { try { - outFS.close(); + printer.close(); } catch (IOException e) { e.printStackTrace(); } From a45511fc7d019544cd830f3fcc839ffe2ed9bd4a Mon Sep 17 00:00:00 2001 From: Nsl106 <87742096+Nsl106@users.noreply.github.com> Date: Tue, 1 Nov 2022 17:58:05 -0700 Subject: [PATCH 4/4] Update DriveInterpolator --- .../org/team498/C2022/RobotContainer.java | 2 +- .../C2022/commands/auto/TableFollower.java | 87 +++++++------- .../java/org/team498/app/ControllerApp.java | 106 +++++++++--------- .../team498/lib/util/DriveInterpolator.java | 66 +++++------ 4 files changed, 128 insertions(+), 133 deletions(-) diff --git a/src/main/java/org/team498/C2022/RobotContainer.java b/src/main/java/org/team498/C2022/RobotContainer.java index 206f9ca..55c50fe 100644 --- a/src/main/java/org/team498/C2022/RobotContainer.java +++ b/src/main/java/org/team498/C2022/RobotContainer.java @@ -188,7 +188,7 @@ private void configureButtonBindings() { .whenInactive(new SetShooter(shooter, 0)); new JoystickButton(operatorController, Button.kBack.value) - .toggleWhenActive(new ControllerApp(driverController, "trajectory1.txt")); + .toggleWhenActive(new ControllerApp(driverController, "trajectory1.txt", "/home/lvuser/paths")); //TODO: Test the input recorder new JoystickButton(driverController, Button.kRightBumper.value) // .and(flywheelAtSpeed) diff --git a/src/main/java/org/team498/C2022/commands/auto/TableFollower.java b/src/main/java/org/team498/C2022/commands/auto/TableFollower.java index 41b0725..55c3473 100644 --- a/src/main/java/org/team498/C2022/commands/auto/TableFollower.java +++ b/src/main/java/org/team498/C2022/commands/auto/TableFollower.java @@ -1,64 +1,59 @@ package org.team498.C2022.commands.auto; -import org.team498.C2022.Tables.DriveTables; import org.team498.C2022.subsystems.Drivetrain; import org.team498.lib.util.DriveInterpolator; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; public class TableFollower extends CommandBase { - private Drivetrain drivetrain; - private Timer timer = new Timer(); - private DriveInterpolator interpolator; - private double time; - - private static double deadzone = 0.1; - private double driveSpeed = 2; + private Drivetrain drivetrain; + private Timer timer = new Timer(); + private DriveInterpolator interpolator; + private double time = 0; + + private static double deadzone = 0.1; + private double driveSpeed = 2; + + public TableFollower(Drivetrain drivetrain, double[][] trajectory) { + this.drivetrain = drivetrain; + interpolator = new DriveInterpolator(trajectory); + addRequirements(this.drivetrain); + } - public TableFollower(Drivetrain drivetrain, double[][] trajectory) { - time = 0; - this.drivetrain = drivetrain; - interpolator = new DriveInterpolator(DriveTables.trajectory1); - addRequirements(drivetrain); - } + @Override + public void initialize() { + timer.reset(); + timer.start(); + } - @Override - public void initialize() { - timer.start(); - } + @Override + public void execute() { + time = timer.get(); + double[] state = interpolator.getInterpolatedValue(time); + + drivetrain.drive( + ChassisSpeeds.fromFieldRelativeSpeeds( + deadzone(((state[0] * driveSpeed) * (state[0] * driveSpeed)) * state[0], deadzone), + deadzone(((state[1] * driveSpeed) * (state[1] * driveSpeed)) * state[1], deadzone), + deadzone(((state[2] * driveSpeed) * (state[2] * driveSpeed)) * state[2], deadzone), + drivetrain.getGyroAngle())); + } - @Override - public void execute() { - time = timer.get(); - double[] state = getState(time); - SmartDashboard.putNumber("timer", time); - drivetrain.drive( - ChassisSpeeds.fromFieldRelativeSpeeds( - deadzone(((state[0] * driveSpeed) * (state[0] * driveSpeed)) * state[0], deadzone), - deadzone(((state[1] * driveSpeed) * (state[1] * driveSpeed)) * state[1], deadzone), - deadzone(((state[2] * driveSpeed) * (state[2] * driveSpeed)) * state[2], deadzone), - drivetrain.getGyroAngle())); - drivetrain.drive(new ChassisSpeeds(state[0], state[1], state[2])); - } - @Override - public void end(boolean interrupted) { - drivetrain.drive(new ChassisSpeeds(0,0,0)); - timer.stop(); - timer.reset(); - } + @Override + public void end(boolean interrupted) { + drivetrain.drive(new ChassisSpeeds(0, 0, 0)); + timer.stop(); + timer.reset(); + } - @Override - public boolean isFinished() { - return interpolator.isFinished(time); - } + @Override + public boolean isFinished() { + return interpolator.isFinished(); + } - private double[] getState(double time) { - return interpolator.getInterpolatedValue(time); - } - private double deadzone(double input, double deadzone) { + private double deadzone(double input, double deadzone) { if (Math.abs(input) > deadzone) return input; return 0; diff --git a/src/main/java/org/team498/app/ControllerApp.java b/src/main/java/org/team498/app/ControllerApp.java index cf5520d..9b5a012 100644 --- a/src/main/java/org/team498/app/ControllerApp.java +++ b/src/main/java/org/team498/app/ControllerApp.java @@ -1,82 +1,86 @@ package org.team498.app; import java.io.File; -import java.io.FileNotFoundException; -import java.io.FileOutputStream; import java.io.FileWriter; import java.io.IOException; -import java.io.PrintWriter; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.CommandBase; -public class ControllerApp extends CommandBase{ - //normally, I would make this a separate thing, but we want to run it at the same time as the robot - //also this is the easiest way to access the controllers lol - private FileWriter printer; - private XboxController controller; - private Timer timer = new Timer(); +public class ControllerApp extends CommandBase { + // normally, I would make this a separate thing, but we want to run it at the + // same time as the robot + // also this is the easiest way to access the controllers lol + private FileWriter printer; + private XboxController controller; + private Timer timer = new Timer(); private File file; - public ControllerApp(XboxController controller, String fileName) { - this.controller = controller; - File filepath = new File("/home/lvuser/paths".replace('/', File.separatorChar)); + public ControllerApp(XboxController controller, String fileName, String filePath) { + this.controller = controller; + + File filepath = new File(filePath.replace('/', File.separatorChar)); file = new File(filepath, fileName); - openFile(); - } - @Override - public void initialize() { - //printer.print("{\n"); - timer.start(); - } - @Override - public void execute() { - double timeStamp = timer.get(); - double dx = controller.getLeftY(); - double dy = controller.getLeftX(); - double dr = controller.getRightX(); - printInfo(timeStamp, dx, dy, dr); + openFile(); + } + + @Override + public void initialize() { + // printer.print("{\n"); + timer.start(); + } - System.out.println("running"); - } - @Override - public void end(boolean interrupted) { - //printer.print("\n}"); - closeFile(); - timer.stop(); - timer.reset(); - } + int printTimer = 0; - private void printInfo(double timeStamp, double dx, double dy, double dr) { + @Override + public void execute() { + double timeStamp = timer.get(); + double dx = controller.getLeftY(); + double dy = controller.getLeftX(); + double dr = controller.getRightX(); + + if (printTimer++ >= 5) { + printInfo(timeStamp, dx, dy, dr); + printTimer = 0; + } + } + + @Override + public void end(boolean interrupted) { + // printer.print("\n}"); + closeFile(); + timer.stop(); + timer.reset(); + } + + private void printInfo(double timeStamp, double dx, double dy, double dr) { try { printer.write(String.format("{ %.3f, %.4f, %.4f, %.4f },\n", timeStamp, dx, dy, dr)); } catch (IOException e) { e.printStackTrace(); } - // printer.printf("{ %.3f, %.4f, %.4f, %.4f },\n", timeStamp, dx, dy, dr); - } + } - private void openFile() { + private void openFile() { try { file.createNewFile(); - } catch (IOException e1) { - e1.printStackTrace(); + } catch (IOException e) { + e.printStackTrace(); } - - try { + try { printer = new FileWriter(file, true); } catch (IOException e) { e.printStackTrace(); } - } + } - private void closeFile() { - try { - printer.close(); - } catch (IOException e) { - e.printStackTrace(); - } - } + private void closeFile() { + try { + printer.close(); + } catch (IOException e) { + e.printStackTrace(); + } + } } diff --git a/src/main/java/org/team498/lib/util/DriveInterpolator.java b/src/main/java/org/team498/lib/util/DriveInterpolator.java index 17667f9..e91df47 100644 --- a/src/main/java/org/team498/lib/util/DriveInterpolator.java +++ b/src/main/java/org/team498/lib/util/DriveInterpolator.java @@ -13,12 +13,15 @@ * * Credit: FRC team 5013 */ -public class DriveInterpolator { +public class + +DriveInterpolator { // https://therevisionist.org/software-engineering/java/tutorials/passing-2d-arrays/ private double[][] table; private boolean initialized = false; - private int flexIndex = 0; + private boolean finished = false; + private int lastIndex; /** * create linearInterpolator class @@ -26,6 +29,7 @@ public class DriveInterpolator { * @param data, a table of x -> y mappings to be interpolated */ public DriveInterpolator(double[][] data) { + lastIndex = 0; build_table(data); } @@ -52,6 +56,10 @@ private void build_table(double[][] data) { initialized = true; } + public boolean isFinished() { + return finished; + } + /** * getInterpolatedValue() - return the interpolated value of y given x. * @@ -68,12 +76,6 @@ private void build_table(double[][] data) { * @param x, the value of x to get an interpolated y value for * @return the linear interpolated value y */ - public boolean isFinished(double time) { - if (flexIndex >= table.length) { - return true; - } - return false; - } public double[] getInterpolatedValue(double x) { if (!initialized) { @@ -88,41 +90,35 @@ public double[] getInterpolatedValue(double x) { if (index >= table.length) { return table[table.length - 1]; } - double high_x = table[index][1]; + double high_x = table[index][1]; double high_y = table[index][2]; - double high_r = table[index][3]; + double high_r = table[index][3]; double high_t = table[index][0]; if ((high_t == x) || (index == 0)) { - return new double[] {high_t, high_x, high_y, high_r}; + return new double[] { high_t, high_x, high_y, high_r }; } - double low_x = table[index - 1][1]; + double low_x = table[index - 1][1]; double low_y = table[index - 1][2]; - double low_r = table[index - 1][3]; + double low_r = table[index - 1][3]; double low_t = table[index - 1][0]; return new double[] { - (low_x + (x - low_t) * (high_x - low_x) / (high_t - low_t)), - (low_y + (x - low_t) * (high_y - low_y) / (high_t - low_t)), - (low_r + (x - low_t) * (high_r - low_r) / (high_t - low_t)) - }; + (low_x + (x - low_t) * (high_x - low_x) / (high_t - low_t)), + (low_y + (x - low_t) * (high_y - low_y) / (high_t - low_t)), + (low_r + (x - low_t) * (high_r - low_r) / (high_t - low_t)) + }; } - //changed from linear search to binary -- since we use massive tables, we want it to be faster - public int getIndex(double time) { - int min = 0; - int max = table.length - 1; - int mid = (max + min) / 2; - while (time <= table[mid][0] - 0.1 || time >= table[mid][0] + 0.1) { - if (time > table[mid][0]) { - min = mid; - } else if (time < table[mid][0]) { - max = mid; - } - mid = min + max / 2; - } - flexIndex = mid; - return mid; - } - public int getFlexIndex() { - return flexIndex; + + public int getIndex(double time) { + while (true) { + if (time <= table[lastIndex][0]) + return lastIndex; + lastIndex++; + + if (lastIndex == table.length) { + finished = true; + return lastIndex; + } + } } } \ No newline at end of file