From 6ab2bea3c8cf4881efa0bb28ffa0fcf04c428b19 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Mon, 6 Feb 2023 19:47:14 -0700 Subject: [PATCH 01/16] printed odometry to smartdashboard --- src/main/java/frc4388/robot/RobotContainer.java | 2 -- src/main/java/frc4388/robot/subsystems/SwerveDrive.java | 7 ++++--- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 97f5db1..a3690d9 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -55,8 +55,6 @@ public class RobotContainer { , m_robotSwerveDrive).withName("SwerveDrive DefaultCommand")); // m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); - - } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 38e7415..a15f6ef 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -86,6 +86,7 @@ public class SwerveDrive extends SubsystemBase { // 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); } @@ -172,9 +173,9 @@ public class SwerveDrive extends SubsystemBase { // This method will be called once per scheduler run updateOdometry(); - // SmartDashboard.putNumberArray("Odometry", new double[] {getOdometry().getX(), getOdometry().getY(), getOdometry().getRotation().getDegrees()}); - - SmartDashboard.putNumber("Gyro Angle", getGyroAngle()); + 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()); } /** From 58a07ae8a63027292060821a9f54b999a480943a Mon Sep 17 00:00:00 2001 From: aarav18 Date: Tue, 7 Feb 2023 17:53:32 -0700 Subject: [PATCH 02/16] fixed gear ratios --- src/main/java/frc4388/robot/Constants.java | 10 +++++----- .../java/frc4388/robot/subsystems/SwerveDrive.java | 1 + 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 1b94ed1..efe227b 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -67,11 +67,11 @@ public final class Constants { public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11.0; // TODO: find the actual value public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 1.0; // TODO: find the actual value - public static final double MOTOR_REV_PER_WHEEL_REV = 6.12; // TODO: find the actual value - public static final double MOTOR_REV_PER_STEER_REV = 12.8; // TODO: find the actual value + public static final double MOTOR_REV_PER_WHEEL_REV = 5.12; + public static final double MOTOR_REV_PER_STEER_REV = 12.8; public static final double TICKS_PER_MOTOR_REV = 2048; - public static final double WHEEL_DIAMETER_INCHES = 4.0; // TODO: the actual value + public static final double WHEEL_DIAMETER_INCHES = 3.9; public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI; public static final double WHEEL_REV_PER_MOTOR_REV = 1 / MOTOR_REV_PER_WHEEL_REV; @@ -108,8 +108,8 @@ public final class Constants { public static final double MAX_SPEED_FEET_PER_SECOND = 5; // TODO: find the actual value // dimensions - public static final double WIDTH = 18.5; // TODO: find the actual value - public static final double HEIGHT = 18.5; // TODO: find the actual value + public static final double WIDTH = 18.5; + public static final double HEIGHT = 18.5; public static final double HALF_WIDTH = WIDTH / 2.d; public static final double HALF_HEIGHT = HEIGHT / 2.d; diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index a15f6ef..a8a3c9f 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -176,6 +176,7 @@ public class SwerveDrive extends SubsystemBase { 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()); } /** From d5dbbdf1908540755bc300669d361f6079e7657f Mon Sep 17 00:00:00 2001 From: aarav18 Date: Tue, 7 Feb 2023 18:54:09 -0700 Subject: [PATCH 03/16] drift fix --- src/main/java/frc4388/robot/subsystems/SwerveDrive.java | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index a8a3c9f..17bdd5b 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -79,9 +79,9 @@ public class SwerveDrive extends SubsystemBase { // Use the left joystick to set speed. Apply a quadratic curve and the set max speed. Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); - if (rightStick.getNorm() < 0.1) { - rot = 0; - } + // if (rightStick.getNorm() < .1) { + // rot = 0; + // } // Convert field-relative speeds to robot-relative speeds. chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), speed.getY(), rot * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1)); @@ -176,7 +176,9 @@ public class SwerveDrive extends SubsystemBase { 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()); } /** From 867e16f0a722944138553fd37eca8d7ce34471c0 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Wed, 8 Feb 2023 00:35:10 -0700 Subject: [PATCH 04/16] basic joystick recording and playback test --- .../frc4388/robot/commands/JoystickInputs.txt | 0 .../robot/commands/JoystickPlayback.java | 73 +++++++++++++++++++ .../robot/commands/JoystickRecorder.java | 59 +++++++++++++++ 3 files changed, 132 insertions(+) create mode 100644 src/main/java/frc4388/robot/commands/JoystickInputs.txt create mode 100644 src/main/java/frc4388/robot/commands/JoystickPlayback.java create mode 100644 src/main/java/frc4388/robot/commands/JoystickRecorder.java diff --git a/src/main/java/frc4388/robot/commands/JoystickInputs.txt b/src/main/java/frc4388/robot/commands/JoystickInputs.txt new file mode 100644 index 0000000..e69de29 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..d26a818 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/JoystickPlayback.java @@ -0,0 +1,73 @@ +// 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.io.IOException; +import java.nio.file.Files; +import java.nio.file.Path; +import java.util.Scanner; +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.controller.DeadbandedXboxController; + +public class JoystickPlayback extends CommandBase { + + SwerveDrive swerve; + Scanner input; + + /** Creates a new JoystickPlayback. */ + public JoystickPlayback(SwerveDrive swerve) { + // Use addRequirements() here to declare subsystem dependencies. + this.swerve = swerve; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + try { + input = new Scanner(new File("JoystickInput.txt")); + } catch (FileNotFoundException e) { + e.printStackTrace(); + } + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + + String line = ""; + if (input.hasNextLine()) { + line = input.nextLine(); + } + + int ileftX = line.indexOf("leftX: "); + int ileftY = line.indexOf(", leftY: "); + int irightX = line.indexOf(", rightX: "); + int irightY = line.indexOf(", rightY: "); + + double leftX = Double.parseDouble(line.substring(ileftX + 1, ileftY)); + double leftY = Double.parseDouble(line.substring(ileftY + 1, irightX)); + double rightX = Double.parseDouble(line.substring(irightX + 1, irightY)); + double rightY = Double.parseDouble(line.substring(irightY + 1)); + + this.swerve.driveWithInput(new Translation2d(leftX, leftY), new Translation2d(-rightX, rightY), true); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + input.close(); + return false; + } +} 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..d69eb6e --- /dev/null +++ b/src/main/java/frc4388/robot/commands/JoystickRecorder.java @@ -0,0 +1,59 @@ +// 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.IOException; +import java.nio.file.Files; +import java.nio.file.Path; +import java.util.function.Supplier; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.utility.controller.DeadbandedXboxController; + +public class JoystickRecorder extends CommandBase { + + DeadbandedXboxController joystick; + Supplier joystickSupplier; + + int numEntries = 0; + + /** Creates a new JoystickRecorder. */ + public JoystickRecorder(Supplier joystickSupplier) { + // Use addRequirements() here to declare subsystem dependencies. + this.joystickSupplier = joystickSupplier; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + joystick = joystickSupplier.get(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + double leftX = joystick.getLeftX(); + double leftY = joystick.getLeftY(); + double rightX = joystick.getRightX(); + double rightY = joystick.getRightY(); + + try { + Files.writeString(Path.of("JoystickInputs.txt"), "leftX: " + leftX + ", " + "leftY: " + leftY + ", " + "rightX: " + rightX + ", " + "rightY: " + rightY); + numEntries = numEntries + 1; + } catch (IOException e) { + e.printStackTrace(); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return numEntries > 10; + } +} From c9a4274c82b009e4dd4c388790c04e4b72b54b48 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 10 Feb 2023 17:33:19 -0700 Subject: [PATCH 05/16] time --- .../java/frc4388/robot/commands/JoystickPlayback.java | 5 +++-- .../java/frc4388/robot/commands/JoystickRecorder.java | 11 ++++++----- 2 files changed, 9 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/JoystickPlayback.java b/src/main/java/frc4388/robot/commands/JoystickPlayback.java index d26a818..774cffa 100644 --- a/src/main/java/frc4388/robot/commands/JoystickPlayback.java +++ b/src/main/java/frc4388/robot/commands/JoystickPlayback.java @@ -62,12 +62,13 @@ public class JoystickPlayback extends CommandBase { // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + input.close(); + } // Returns true when the command should end. @Override public boolean isFinished() { - input.close(); return false; } } diff --git a/src/main/java/frc4388/robot/commands/JoystickRecorder.java b/src/main/java/frc4388/robot/commands/JoystickRecorder.java index d69eb6e..d658a6d 100644 --- a/src/main/java/frc4388/robot/commands/JoystickRecorder.java +++ b/src/main/java/frc4388/robot/commands/JoystickRecorder.java @@ -10,19 +10,21 @@ import java.nio.file.Path; import java.util.function.Supplier; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.utility.RobotTime; import frc4388.utility.controller.DeadbandedXboxController; public class JoystickRecorder extends CommandBase { DeadbandedXboxController joystick; Supplier joystickSupplier; - - int numEntries = 0; + + RobotTime time; /** Creates a new JoystickRecorder. */ public JoystickRecorder(Supplier joystickSupplier) { // Use addRequirements() here to declare subsystem dependencies. this.joystickSupplier = joystickSupplier; + time = RobotTime.getInstance(); } // Called when the command is initially scheduled. @@ -40,8 +42,7 @@ public class JoystickRecorder extends CommandBase { double rightY = joystick.getRightY(); try { - Files.writeString(Path.of("JoystickInputs.txt"), "leftX: " + leftX + ", " + "leftY: " + leftY + ", " + "rightX: " + rightX + ", " + "rightY: " + rightY); - numEntries = numEntries + 1; + Files.writeString(Path.of("JoystickInputs.txt"), "Time: " + time.m_robotTime + ", leftX: " + leftX + ", " + "leftY: " + leftY + ", " + "rightX: " + rightX + ", " + "rightY: " + rightY + "\n"); } catch (IOException e) { e.printStackTrace(); } @@ -54,6 +55,6 @@ public class JoystickRecorder extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - return numEntries > 10; + return false; } } From f9b237592f4b724c4ccb7fa5a03a91188a05dd87 Mon Sep 17 00:00:00 2001 From: Abhi <90010729+Abhishrek05@users.noreply.github.com> Date: Fri, 10 Feb 2023 18:12:56 -0700 Subject: [PATCH 06/16] untested joystick recording --- .../robot/commands/JoystickRecorder.java | 58 +++++++++++++------ 1 file changed, 40 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/JoystickRecorder.java b/src/main/java/frc4388/robot/commands/JoystickRecorder.java index d658a6d..ea14c81 100644 --- a/src/main/java/frc4388/robot/commands/JoystickRecorder.java +++ b/src/main/java/frc4388/robot/commands/JoystickRecorder.java @@ -4,53 +4,75 @@ package frc4388.robot.commands; +import java.io.File; import java.io.IOException; +import java.io.PrintWriter; import java.nio.file.Files; import java.nio.file.Path; +import java.util.HashMap; 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.RobotTime; import frc4388.utility.controller.DeadbandedXboxController; public class JoystickRecorder extends CommandBase { - - DeadbandedXboxController joystick; - Supplier joystickSupplier; - RobotTime time; + SwerveDrive swerve; + + Supplier leftXSupplier; + Supplier leftYSupplier; + Supplier rightXSupplier; + Supplier rightYSupplier; + + HashMap timedInput; + + private long startTime; + /** Creates a new JoystickRecorder. */ - public JoystickRecorder(Supplier joystickSupplier) { + public JoystickRecorder(SwerveDrive swerve, Supplier leftXSupplier, Supplier leftYSupplier, Supplier rightXSupplier, Supplier rightYSupplier) { // Use addRequirements() here to declare subsystem dependencies. - this.joystickSupplier = joystickSupplier; - time = RobotTime.getInstance(); + this.swerve = swerve; + this.leftXSupplier = leftXSupplier; + this.leftYSupplier = leftYSupplier; + this.rightXSupplier = rightXSupplier; + this.rightYSupplier = rightYSupplier; } // Called when the command is initially scheduled. @Override public void initialize() { - joystick = joystickSupplier.get(); + timedInput.put((long) 0, new double[] {0.0, 0.0, 0.0, 0.0}); + startTime = System.currentTimeMillis(); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double leftX = joystick.getLeftX(); - double leftY = joystick.getLeftY(); - double rightX = joystick.getRightX(); - double rightY = joystick.getRightY(); + double[] inputs = new double[] {leftXSupplier.get(), leftYSupplier.get(), rightXSupplier.get(), rightYSupplier.get()}; + timedInput.put(System.currentTimeMillis() - startTime, inputs); - try { - Files.writeString(Path.of("JoystickInputs.txt"), "Time: " + time.m_robotTime + ", leftX: " + leftX + ", " + "leftY: " + leftY + ", " + "rightX: " + rightX + ", " + "rightY: " + rightY + "\n"); - } catch (IOException e) { - e.printStackTrace(); - } + swerve.driveWithInput(new Translation2d(inputs[0], inputs[1]), new Translation2d(-inputs[2], inputs[3]), true); } // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + File output = new File("JoystickInputs.txt"); + + try(PrintWriter writer = new PrintWriter(output)) { + for(long millis : timedInput.keySet()) { + writer.println("time: " + millis + ", leftX: " + timedInput.get(millis)[0] + ", leftY: " + timedInput.get(millis)[1] + ", rightX: " + timedInput.get(millis)[2] + ", rightY: " + timedInput.get(millis)[3]); + } + writer.close(); + } catch(IOException e) { + e.printStackTrace(); + } + + } // Returns true when the command should end. @Override From b5a9f59c59df3a4473ffb01dabad5f52a495ba1d Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 10 Feb 2023 20:53:33 -0700 Subject: [PATCH 07/16] AUTO RECORDING WORKS --- .../java/frc4388/robot/RobotContainer.java | 19 ++- .../frc4388/robot/commands/JoystickInputs.txt | 132 ++++++++++++++++++ .../robot/commands/JoystickPlayback.java | 24 ++-- .../robot/commands/JoystickRecorder.java | 37 +++-- 4 files changed, 194 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index a3690d9..aa94317 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -7,6 +7,8 @@ package frc4388.robot; +import javax.print.attribute.standard.OrientationRequested; + import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -14,6 +16,8 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; import frc4388.robot.commands.AutoBalance; +import frc4388.robot.commands.JoystickPlayback; +import frc4388.robot.commands.JoystickRecorder; import frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.controller.DeadbandedXboxController; import frc4388.utility.controller.IHandController; @@ -76,7 +80,20 @@ public class RobotContainer { new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) .onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); - + + new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) + .whileTrue(new JoystickRecorder(m_robotSwerveDrive, + () -> getDeadbandedDriverController().getLeftX(), + () -> getDeadbandedDriverController().getLeftY(), + () -> getDeadbandedDriverController().getRightX(), + () -> getDeadbandedDriverController().getRightY() + + )) + .onFalse(new InstantCommand()); + + new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) + .onTrue(new JoystickPlayback(m_robotSwerveDrive)); + // /* Operator Buttons */ // // interrupt button // new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) diff --git a/src/main/java/frc4388/robot/commands/JoystickInputs.txt b/src/main/java/frc4388/robot/commands/JoystickInputs.txt index e69de29..8ca32ac 100644 --- a/src/main/java/frc4388/robot/commands/JoystickInputs.txt +++ b/src/main/java/frc4388/robot/commands/JoystickInputs.txt @@ -0,0 +1,132 @@ +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,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,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,0.0 +0.0,0.0,0.0,0.0 +0.0,0.0,0.0,0.0 +0.0,-0.1015625,0.0,0.0 +0.0,-0.109375,0.0,0.0 +0.0,-0.1171875,0.0,0.0 +0.0,-0.140625,0.0,0.0 +0.0,-0.140625,0.0,0.0 +0.0,-0.1484375,0.0,0.0 +0.0,-0.1796875,0.0,0.0 +0.0,-0.1796875,0.0,0.0 +0.0,-0.1875,0.0,0.0 +0.0,-0.1953125,0.0,0.0 +0.0,-0.21875,0.0,0.0 +0.0,-0.2265625,0.0,0.0 +0.0,-0.25,0.0,0.0 +0.0,-0.2890625,0.0,0.0 +0.0,-0.3046875,0.0,0.0 +0.0,-0.3125,0.0,0.0 +0.0,-0.3125,0.0,0.0 +0.0,-0.3125,0.0,0.0 +0.0,-0.3359375,0.0,0.0 +0.0,-0.359375,0.0,0.0 +0.0,-0.390625,0.0,0.0 +0.0,-0.3984375,0.0,0.0 +0.0,-0.40625,0.0,0.0 +0.0,-0.4296875,0.0,0.0 +0.0,-0.4453125,0.0,0.0 +0.0,-0.4609375,0.0,0.0 +0.0,-0.46875,0.0,0.0 +0.0,-0.484375,0.0,0.0 +0.0,-0.484375,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.4921875,0.0,0.0 +0.0,-0.4921875,0.0,0.0 +0.0,-0.4921875,0.0,0.0 +0.0,-0.4921875,0.0,0.0 +0.0,-0.4921875,0.0,0.0 +0.0,-0.4921875,0.0,0.0 +0.0,-0.4921875,0.0,0.0 +0.0,-0.4921875,0.0,0.0 +0.0,-0.4921875,0.0,0.0 +0.0,-0.4921875,0.0,0.0 +0.0,-0.4921875,0.0,0.0 +0.0,-0.4921875,0.0,0.0 +0.0,-0.4921875,0.0,0.0 +0.0,-0.4921875,0.0,0.0 +0.0,-0.4921875,0.0,0.0 +0.0,-0.4921875,0.0,0.0 +0.0,-0.4921875,0.0,0.0 +0.0,-0.4921875,0.0,0.0 +0.0,-0.4921875,0.0,0.0 +0.0,-0.4921875,0.0,0.0 +0.0,-0.4921875,0.0,0.0 +0.0,-0.4921875,0.0,0.0 +0.0,-0.4921875,0.0,0.0 +0.0,-0.4921875,0.0,0.0 +0.0,-0.4921875,0.0,0.0 +0.0,-0.484375,0.0,0.0 +0.0,-0.484375,0.0,0.0 +0.0,-0.484375,0.0,0.0 +0.0,-0.484375,0.0,0.0 +0.0,-0.484375,0.0,0.0 +0.0,-0.484375,0.0,0.0 +0.0,-0.484375,0.0,0.0 +0.0,-0.484375,0.0,0.0 +0.0,-0.46875,0.0,0.0 +0.0,-0.4296875,0.0,0.0 +0.0,-0.359375,0.0,0.0 +0.0,-0.2734375,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,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 +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,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,0.0,0.0 +0.0,0.0,0.0,0.0 +0.0,0.0,0.0,0.0 \ 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 index 774cffa..a15eb24 100644 --- a/src/main/java/frc4388/robot/commands/JoystickPlayback.java +++ b/src/main/java/frc4388/robot/commands/JoystickPlayback.java @@ -26,16 +26,23 @@ public class JoystickPlayback extends CommandBase { public JoystickPlayback(SwerveDrive swerve) { // Use addRequirements() here to declare subsystem dependencies. this.swerve = swerve; + + addRequirements(this.swerve); } // Called when the command is initially scheduled. @Override public void initialize() { try { - input = new Scanner(new File("JoystickInput.txt")); + input = new Scanner(new File("/home/lvuser/JoystickInputs.txt")); } catch (FileNotFoundException e) { e.printStackTrace(); } + + System.out.println("STARTING PLAYBACK"); + System.out.println("STARTING PLAYBACK"); + System.out.println("STARTING PLAYBACK"); + System.out.println("STARTING PLAYBACK"); } // Called every time the scheduler runs while the command is scheduled. @@ -47,17 +54,16 @@ public class JoystickPlayback extends CommandBase { line = input.nextLine(); } - int ileftX = line.indexOf("leftX: "); - int ileftY = line.indexOf(", leftY: "); - int irightX = line.indexOf(", rightX: "); - int irightY = line.indexOf(", rightY: "); + String[] values = line.split(","); - double leftX = Double.parseDouble(line.substring(ileftX + 1, ileftY)); - double leftY = Double.parseDouble(line.substring(ileftY + 1, irightX)); - double rightX = Double.parseDouble(line.substring(irightX + 1, irightY)); - double rightY = Double.parseDouble(line.substring(irightY + 1)); + double leftX = Double.parseDouble(values[0]); + double leftY = Double.parseDouble(values[1]); + double rightX = Double.parseDouble(values[2]); + double rightY = Double.parseDouble(values[3]); this.swerve.driveWithInput(new Translation2d(leftX, leftY), new Translation2d(-rightX, rightY), true); + + System.out.println("PLAYING"); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/commands/JoystickRecorder.java b/src/main/java/frc4388/robot/commands/JoystickRecorder.java index ea14c81..134af83 100644 --- a/src/main/java/frc4388/robot/commands/JoystickRecorder.java +++ b/src/main/java/frc4388/robot/commands/JoystickRecorder.java @@ -9,6 +9,7 @@ import java.io.IOException; import java.io.PrintWriter; import java.nio.file.Files; import java.nio.file.Path; +import java.util.ArrayList; import java.util.HashMap; import java.util.function.Supplier; @@ -27,9 +28,10 @@ public class JoystickRecorder extends CommandBase { Supplier rightXSupplier; Supplier rightYSupplier; - HashMap timedInput; + // HashMap timedInput; + ArrayList outputs; - private long startTime; + private final long startTime; /** Creates a new JoystickRecorder. */ @@ -40,38 +42,57 @@ public class JoystickRecorder extends CommandBase { this.leftYSupplier = leftYSupplier; this.rightXSupplier = rightXSupplier; this.rightYSupplier = rightYSupplier; + + this.startTime = System.currentTimeMillis(); + // this.timedInput = new HashMap(); + this.outputs = new ArrayList(); + + addRequirements(this.swerve); } // Called when the command is initially scheduled. @Override public void initialize() { - timedInput.put((long) 0, new double[] {0.0, 0.0, 0.0, 0.0}); - startTime = System.currentTimeMillis(); + // timedInput.put((long) 0, new double[] {0.0, 0.0, 0.0, 0.0}); + outputs.add(new double[] {0.0, 0.0, 0.0, 0.0}); + + System.out.println("STARTING RECORDING"); + System.out.println("STARTING RECORDING"); + System.out.println("STARTING RECORDING"); + System.out.println("STARTING RECORDING"); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { double[] inputs = new double[] {leftXSupplier.get(), leftYSupplier.get(), rightXSupplier.get(), rightYSupplier.get()}; - timedInput.put(System.currentTimeMillis() - startTime, inputs); + // timedInput.put(System.currentTimeMillis() - startTime, inputs); + outputs.add(inputs); swerve.driveWithInput(new Translation2d(inputs[0], inputs[1]), new Translation2d(-inputs[2], inputs[3]), true); + + System.out.println("RECORDING"); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - File output = new File("JoystickInputs.txt"); + File output = new File("/home/lvuser/JoystickInputs.txt"); try(PrintWriter writer = new PrintWriter(output)) { - for(long millis : timedInput.keySet()) { - writer.println("time: " + millis + ", leftX: " + timedInput.get(millis)[0] + ", leftY: " + timedInput.get(millis)[1] + ", rightX: " + timedInput.get(millis)[2] + ", rightY: " + timedInput.get(millis)[3]); + for(double[] input : outputs) { + writer.println(input[0] + "," + input[1] + "," + input[2] + "," + input[3]); } writer.close(); } catch(IOException e) { e.printStackTrace(); } + System.out.println("STOPPED RECORDING"); + System.out.println("STOPPED RECORDING"); + System.out.println("STOPPED RECORDING"); + System.out.println("STOPPED RECORDING"); + } // Returns true when the command should end. From 539668ddb10dbe9968a51a454607229e42a75976 Mon Sep 17 00:00:00 2001 From: Abhi <90010729+Abhishrek05@users.noreply.github.com> Date: Sat, 11 Feb 2023 11:22:51 -0700 Subject: [PATCH 08/16] better files and data structures NOTE: still haven't added the actual lerp function yet I did add the end condition --- .../robot/commands/JoystickPlayback.java | 85 ++++++++++++++----- .../frc4388/robot/subsystems/SwerveDrive.java | 6 ++ 2 files changed, 68 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/JoystickPlayback.java b/src/main/java/frc4388/robot/commands/JoystickPlayback.java index a15eb24..7b3fedc 100644 --- a/src/main/java/frc4388/robot/commands/JoystickPlayback.java +++ b/src/main/java/frc4388/robot/commands/JoystickPlayback.java @@ -9,36 +9,69 @@ import java.io.FileNotFoundException; import java.io.IOException; import java.nio.file.Files; import java.nio.file.Path; +import java.sql.Time; +import java.util.ArrayList; import java.util.Scanner; import java.util.function.Supplier; +import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.controller.DeadbandedXboxController; public class JoystickPlayback extends CommandBase { + private static class TimedOutput { + public double leftX = 0d; + public double leftY = 0d; + public double rightX = 0d; + public double rightY = 0d; - SwerveDrive swerve; - Scanner input; + public long timed_offset = 0l; + } + + private final SwerveDrive m_swerve; + private Scanner m_input; + private final ArrayList m_outputs; + private long m_playback_time; + private int m_last_index; + private boolean m_finished = false; // ! find a better way /** Creates a new JoystickPlayback. */ public JoystickPlayback(SwerveDrive swerve) { // Use addRequirements() here to declare subsystem dependencies. - this.swerve = swerve; + this.m_swerve = swerve; + m_outputs = new ArrayList<>(); - addRequirements(this.swerve); + try { + m_input = new Scanner(new File("/home/lvuser/JoystickInputs.txt")); + + String line = ""; + while (m_input.hasNextLine()) { + line = m_input.nextLine(); + + String[] values = line.split(","); + + var out = new TimedOutput(); + out.leftX = Double.parseDouble(values[0]); + out.leftY = Double.parseDouble(values[1]); + out.rightX = Double.parseDouble(values[2]); + out.rightY = Double.parseDouble(values[3]); + + out.timed_offset = Long.MAX_VALUE; + + m_outputs.add(out); + } + } catch (FileNotFoundException e) { + e.printStackTrace(); + } + + addRequirements(this.m_swerve); } // Called when the command is initially scheduled. @Override public void initialize() { - try { - input = new Scanner(new File("/home/lvuser/JoystickInputs.txt")); - } catch (FileNotFoundException e) { - e.printStackTrace(); - } - System.out.println("STARTING PLAYBACK"); System.out.println("STARTING PLAYBACK"); System.out.println("STARTING PLAYBACK"); @@ -48,33 +81,39 @@ public class JoystickPlayback extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + // 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 = m_last_index + 1; + while (i < m_outputs.size() && m_outputs.get(i).timed_offset < m_playback_time) { + i++; + } - String line = ""; - if (input.hasNextLine()) { - line = input.nextLine(); + if (i >= m_outputs.size()) { + m_finished = true; // ! kind of a hack + return; + } + m_last_index = i; } - String[] values = line.split(","); - - double leftX = Double.parseDouble(values[0]); - double leftY = Double.parseDouble(values[1]); - double rightX = Double.parseDouble(values[2]); - double rightY = Double.parseDouble(values[3]); - - this.swerve.driveWithInput(new Translation2d(leftX, leftY), new Translation2d(-rightX, rightY), true); + TimedOutput out = m_outputs.get(m_last_index); + this.m_swerve.driveWithInput(new Translation2d(out.leftX, out.leftY), + new Translation2d(-out.rightX, out.rightY), + true); System.out.println("PLAYING"); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - input.close(); + m_input.close(); + m_swerve.stopModules(); } // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return m_finished; } } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 17bdd5b..9d73542 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -156,6 +156,12 @@ public class SwerveDrive extends SubsystemBase { ); } + public void stopModules() { + for (SwerveModule module : this.modules) { + module.stop(); + } + } + /** * Resets the odometry of the SwerveDrive to 0. * *NOTE: If you reset your gyro, this method MUST be called with the new gyro angle and wheel encoder positions. From 79114c64de2b98701e177296478f0124ff6f3060 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 11 Feb 2023 11:51:13 -0700 Subject: [PATCH 09/16] Revert "better files and data structures" This reverts commit 539668ddb10dbe9968a51a454607229e42a75976. --- .../robot/commands/JoystickPlayback.java | 85 +++++-------------- .../frc4388/robot/subsystems/SwerveDrive.java | 6 -- 2 files changed, 23 insertions(+), 68 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/JoystickPlayback.java b/src/main/java/frc4388/robot/commands/JoystickPlayback.java index 7b3fedc..a15eb24 100644 --- a/src/main/java/frc4388/robot/commands/JoystickPlayback.java +++ b/src/main/java/frc4388/robot/commands/JoystickPlayback.java @@ -9,69 +9,36 @@ import java.io.FileNotFoundException; import java.io.IOException; import java.nio.file.Files; import java.nio.file.Path; -import java.sql.Time; -import java.util.ArrayList; import java.util.Scanner; import java.util.function.Supplier; -import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.controller.DeadbandedXboxController; public class JoystickPlayback extends CommandBase { - private static class TimedOutput { - public double leftX = 0d; - public double leftY = 0d; - public double rightX = 0d; - public double rightY = 0d; - public long timed_offset = 0l; - } - - private final SwerveDrive m_swerve; - private Scanner m_input; - private final ArrayList m_outputs; - private long m_playback_time; - private int m_last_index; - private boolean m_finished = false; // ! find a better way + SwerveDrive swerve; + Scanner input; /** Creates a new JoystickPlayback. */ public JoystickPlayback(SwerveDrive swerve) { // Use addRequirements() here to declare subsystem dependencies. - this.m_swerve = swerve; - m_outputs = new ArrayList<>(); + this.swerve = swerve; - try { - m_input = new Scanner(new File("/home/lvuser/JoystickInputs.txt")); - - String line = ""; - while (m_input.hasNextLine()) { - line = m_input.nextLine(); - - String[] values = line.split(","); - - var out = new TimedOutput(); - out.leftX = Double.parseDouble(values[0]); - out.leftY = Double.parseDouble(values[1]); - out.rightX = Double.parseDouble(values[2]); - out.rightY = Double.parseDouble(values[3]); - - out.timed_offset = Long.MAX_VALUE; - - m_outputs.add(out); - } - } catch (FileNotFoundException e) { - e.printStackTrace(); - } - - addRequirements(this.m_swerve); + addRequirements(this.swerve); } // Called when the command is initially scheduled. @Override public void initialize() { + try { + input = new Scanner(new File("/home/lvuser/JoystickInputs.txt")); + } catch (FileNotFoundException e) { + e.printStackTrace(); + } + System.out.println("STARTING PLAYBACK"); System.out.println("STARTING PLAYBACK"); System.out.println("STARTING PLAYBACK"); @@ -81,39 +48,33 @@ public class JoystickPlayback extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - // 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 = m_last_index + 1; - while (i < m_outputs.size() && m_outputs.get(i).timed_offset < m_playback_time) { - i++; - } - if (i >= m_outputs.size()) { - m_finished = true; // ! kind of a hack - return; - } - m_last_index = i; + String line = ""; + if (input.hasNextLine()) { + line = input.nextLine(); } - TimedOutput out = m_outputs.get(m_last_index); + String[] values = line.split(","); + + double leftX = Double.parseDouble(values[0]); + double leftY = Double.parseDouble(values[1]); + double rightX = Double.parseDouble(values[2]); + double rightY = Double.parseDouble(values[3]); + + this.swerve.driveWithInput(new Translation2d(leftX, leftY), new Translation2d(-rightX, rightY), true); - this.m_swerve.driveWithInput(new Translation2d(out.leftX, out.leftY), - new Translation2d(-out.rightX, out.rightY), - true); System.out.println("PLAYING"); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - m_input.close(); - m_swerve.stopModules(); + input.close(); } // Returns true when the command should end. @Override public boolean isFinished() { - return m_finished; + return false; } } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 9d73542..17bdd5b 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -156,12 +156,6 @@ public class SwerveDrive extends SubsystemBase { ); } - public void stopModules() { - for (SwerveModule module : this.modules) { - module.stop(); - } - } - /** * Resets the odometry of the SwerveDrive to 0. * *NOTE: If you reset your gyro, this method MUST be called with the new gyro angle and wheel encoder positions. From 3259182ea47429548218744dee79acee49803188 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 11 Feb 2023 12:44:51 -0700 Subject: [PATCH 10/16] Revert "Revert "better files and data structures"" This reverts commit 79114c64de2b98701e177296478f0124ff6f3060. --- .../robot/commands/JoystickPlayback.java | 85 ++++++++++++++----- .../frc4388/robot/subsystems/SwerveDrive.java | 6 ++ 2 files changed, 68 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/JoystickPlayback.java b/src/main/java/frc4388/robot/commands/JoystickPlayback.java index a15eb24..7b3fedc 100644 --- a/src/main/java/frc4388/robot/commands/JoystickPlayback.java +++ b/src/main/java/frc4388/robot/commands/JoystickPlayback.java @@ -9,36 +9,69 @@ import java.io.FileNotFoundException; import java.io.IOException; import java.nio.file.Files; import java.nio.file.Path; +import java.sql.Time; +import java.util.ArrayList; import java.util.Scanner; import java.util.function.Supplier; +import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.controller.DeadbandedXboxController; public class JoystickPlayback extends CommandBase { + private static class TimedOutput { + public double leftX = 0d; + public double leftY = 0d; + public double rightX = 0d; + public double rightY = 0d; - SwerveDrive swerve; - Scanner input; + public long timed_offset = 0l; + } + + private final SwerveDrive m_swerve; + private Scanner m_input; + private final ArrayList m_outputs; + private long m_playback_time; + private int m_last_index; + private boolean m_finished = false; // ! find a better way /** Creates a new JoystickPlayback. */ public JoystickPlayback(SwerveDrive swerve) { // Use addRequirements() here to declare subsystem dependencies. - this.swerve = swerve; + this.m_swerve = swerve; + m_outputs = new ArrayList<>(); - addRequirements(this.swerve); + try { + m_input = new Scanner(new File("/home/lvuser/JoystickInputs.txt")); + + String line = ""; + while (m_input.hasNextLine()) { + line = m_input.nextLine(); + + String[] values = line.split(","); + + var out = new TimedOutput(); + out.leftX = Double.parseDouble(values[0]); + out.leftY = Double.parseDouble(values[1]); + out.rightX = Double.parseDouble(values[2]); + out.rightY = Double.parseDouble(values[3]); + + out.timed_offset = Long.MAX_VALUE; + + m_outputs.add(out); + } + } catch (FileNotFoundException e) { + e.printStackTrace(); + } + + addRequirements(this.m_swerve); } // Called when the command is initially scheduled. @Override public void initialize() { - try { - input = new Scanner(new File("/home/lvuser/JoystickInputs.txt")); - } catch (FileNotFoundException e) { - e.printStackTrace(); - } - System.out.println("STARTING PLAYBACK"); System.out.println("STARTING PLAYBACK"); System.out.println("STARTING PLAYBACK"); @@ -48,33 +81,39 @@ public class JoystickPlayback extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + // 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 = m_last_index + 1; + while (i < m_outputs.size() && m_outputs.get(i).timed_offset < m_playback_time) { + i++; + } - String line = ""; - if (input.hasNextLine()) { - line = input.nextLine(); + if (i >= m_outputs.size()) { + m_finished = true; // ! kind of a hack + return; + } + m_last_index = i; } - String[] values = line.split(","); - - double leftX = Double.parseDouble(values[0]); - double leftY = Double.parseDouble(values[1]); - double rightX = Double.parseDouble(values[2]); - double rightY = Double.parseDouble(values[3]); - - this.swerve.driveWithInput(new Translation2d(leftX, leftY), new Translation2d(-rightX, rightY), true); + TimedOutput out = m_outputs.get(m_last_index); + this.m_swerve.driveWithInput(new Translation2d(out.leftX, out.leftY), + new Translation2d(-out.rightX, out.rightY), + true); System.out.println("PLAYING"); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - input.close(); + m_input.close(); + m_swerve.stopModules(); } // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return m_finished; } } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 17bdd5b..9d73542 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -156,6 +156,12 @@ public class SwerveDrive extends SubsystemBase { ); } + public void stopModules() { + for (SwerveModule module : this.modules) { + module.stop(); + } + } + /** * Resets the odometry of the SwerveDrive to 0. * *NOTE: If you reset your gyro, this method MUST be called with the new gyro angle and wheel encoder positions. From 1e79aa28ee4776b5aa31a381937d831068a2d798 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 11 Feb 2023 15:21:44 -0700 Subject: [PATCH 11/16] lerp kinda works --- src/main/java/frc4388/robot/Robot.java | 3 + .../frc4388/robot/commands/JoystickInputs.txt | 730 ++++++++++++++---- .../robot/commands/JoystickPlayback.java | 113 ++- .../robot/commands/JoystickRecorder.java | 24 +- 4 files changed, 695 insertions(+), 175 deletions(-) diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 8fc1ca3..2d6f348 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -74,6 +74,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) @@ -92,6 +93,7 @@ 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 @@ -99,6 +101,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/commands/JoystickInputs.txt b/src/main/java/frc4388/robot/commands/JoystickInputs.txt index 8ca32ac..263ff3f 100644 --- a/src/main/java/frc4388/robot/commands/JoystickInputs.txt +++ b/src/main/java/frc4388/robot/commands/JoystickInputs.txt @@ -1,132 +1,598 @@ -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,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,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,0.0 -0.0,0.0,0.0,0.0 -0.0,0.0,0.0,0.0 -0.0,-0.1015625,0.0,0.0 -0.0,-0.109375,0.0,0.0 -0.0,-0.1171875,0.0,0.0 -0.0,-0.140625,0.0,0.0 -0.0,-0.140625,0.0,0.0 -0.0,-0.1484375,0.0,0.0 -0.0,-0.1796875,0.0,0.0 -0.0,-0.1796875,0.0,0.0 -0.0,-0.1875,0.0,0.0 -0.0,-0.1953125,0.0,0.0 -0.0,-0.21875,0.0,0.0 -0.0,-0.2265625,0.0,0.0 -0.0,-0.25,0.0,0.0 -0.0,-0.2890625,0.0,0.0 -0.0,-0.3046875,0.0,0.0 -0.0,-0.3125,0.0,0.0 -0.0,-0.3125,0.0,0.0 -0.0,-0.3125,0.0,0.0 -0.0,-0.3359375,0.0,0.0 -0.0,-0.359375,0.0,0.0 -0.0,-0.390625,0.0,0.0 -0.0,-0.3984375,0.0,0.0 -0.0,-0.40625,0.0,0.0 -0.0,-0.4296875,0.0,0.0 -0.0,-0.4453125,0.0,0.0 -0.0,-0.4609375,0.0,0.0 -0.0,-0.46875,0.0,0.0 -0.0,-0.484375,0.0,0.0 -0.0,-0.484375,0.0,0.0 -0.0,-0.5,0.0,0.0 -0.0,-0.5,0.0,0.0 -0.0,-0.5,0.0,0.0 -0.0,-0.5,0.0,0.0 -0.0,-0.5,0.0,0.0 -0.0,-0.5,0.0,0.0 -0.0,-0.5,0.0,0.0 -0.0,-0.5,0.0,0.0 -0.0,-0.5,0.0,0.0 -0.0,-0.5,0.0,0.0 -0.0,-0.5,0.0,0.0 -0.0,-0.5,0.0,0.0 -0.0,-0.5,0.0,0.0 -0.0,-0.5,0.0,0.0 -0.0,-0.5,0.0,0.0 -0.0,-0.5,0.0,0.0 -0.0,-0.5,0.0,0.0 -0.0,-0.5,0.0,0.0 -0.0,-0.5,0.0,0.0 -0.0,-0.5,0.0,0.0 -0.0,-0.5,0.0,0.0 -0.0,-0.5,0.0,0.0 -0.0,-0.5,0.0,0.0 -0.0,-0.5,0.0,0.0 -0.0,-0.5,0.0,0.0 -0.0,-0.5,0.0,0.0 -0.0,-0.5,0.0,0.0 -0.0,-0.5,0.0,0.0 -0.0,-0.5,0.0,0.0 -0.0,-0.5,0.0,0.0 -0.0,-0.5,0.0,0.0 -0.0,-0.5,0.0,0.0 -0.0,-0.5,0.0,0.0 -0.0,-0.5,0.0,0.0 -0.0,-0.5,0.0,0.0 -0.0,-0.5,0.0,0.0 -0.0,-0.5,0.0,0.0 -0.0,-0.5,0.0,0.0 -0.0,-0.5,0.0,0.0 -0.0,-0.4921875,0.0,0.0 -0.0,-0.4921875,0.0,0.0 -0.0,-0.4921875,0.0,0.0 -0.0,-0.4921875,0.0,0.0 -0.0,-0.4921875,0.0,0.0 -0.0,-0.4921875,0.0,0.0 -0.0,-0.4921875,0.0,0.0 -0.0,-0.4921875,0.0,0.0 -0.0,-0.4921875,0.0,0.0 -0.0,-0.4921875,0.0,0.0 -0.0,-0.4921875,0.0,0.0 -0.0,-0.4921875,0.0,0.0 -0.0,-0.4921875,0.0,0.0 -0.0,-0.4921875,0.0,0.0 -0.0,-0.4921875,0.0,0.0 -0.0,-0.4921875,0.0,0.0 -0.0,-0.4921875,0.0,0.0 -0.0,-0.4921875,0.0,0.0 -0.0,-0.4921875,0.0,0.0 -0.0,-0.4921875,0.0,0.0 -0.0,-0.4921875,0.0,0.0 -0.0,-0.4921875,0.0,0.0 -0.0,-0.4921875,0.0,0.0 -0.0,-0.4921875,0.0,0.0 -0.0,-0.4921875,0.0,0.0 -0.0,-0.484375,0.0,0.0 -0.0,-0.484375,0.0,0.0 -0.0,-0.484375,0.0,0.0 -0.0,-0.484375,0.0,0.0 -0.0,-0.484375,0.0,0.0 -0.0,-0.484375,0.0,0.0 -0.0,-0.484375,0.0,0.0 -0.0,-0.484375,0.0,0.0 -0.0,-0.46875,0.0,0.0 -0.0,-0.4296875,0.0,0.0 -0.0,-0.359375,0.0,0.0 -0.0,-0.2734375,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,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 -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,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,0.0,0.0 -0.0,0.0,0.0,0.0 -0.0,0.0,0.0,0.0 \ No newline at end of file +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,19 +0.0,0.0,0.0,0.0,31 +0.0,0.0,0.0,0.0,46 +0.0,0.0,0.0,0.0,64 +0.0,0.0,0.0,0.0,84 +0.0,0.0,0.0,0.0,104 +0.0,0.0,0.0,0.0,124 +0.0,0.0,0.0,0.0,144 +0.0,0.0,0.0,0.0,165 +0.0,0.0,0.0,0.0,185 +0.0,0.0,0.0,0.0,204 +0.0,0.0,0.0,0.0,239 +0.0,0.0,0.0,0.0,250 +0.0,0.0,0.0,0.0,267 +0.0,0.0,0.0,0.0,284 +0.0,0.0,0.0,0.0,305 +0.0,0.0,0.0,0.0,325 +-0.046875,-0.140625,0.0,0.0,345 +-0.046875,-0.15625,0.0,0.0,364 +-0.0390625,-0.1640625,0.0,0.0,385 +-0.046875,-0.1640625,0.0,0.0,404 +-0.046875,-0.1640625,0.0,0.0,425 +-0.0390625,-0.1796875,0.0,0.0,444 +-0.0390625,-0.2109375,0.0,0.0,481 +-0.0390625,-0.2109375,0.0,0.0,495 +-0.0390625,-0.2265625,0.0,0.0,512 +-0.0390625,-0.234375,0.0,0.0,524 +-0.03125,-0.25,0.0,0.0,544 +-0.03125,-0.2578125,0.0,0.0,564 +-0.0234375,-0.2890625,0.0,0.0,584 +-0.0234375,-0.3046875,0.0,0.0,604 +-0.015625,-0.3125,0.0,0.0,624 +-0.015625,-0.3125,0.0,0.0,644 +-0.015625,-0.3125,0.0,0.0,665 +-0.015625,-0.3125,0.0,0.0,684 +-0.0078125,-0.3125,0.0,0.0,727 +-0.015625,-0.3125,0.0,0.0,744 +-0.015625,-0.3125,0.0,0.0,757 +-0.015625,-0.3125,0.0,0.0,775 +-0.0078125,-0.3125,0.0,0.0,795 +-0.015625,-0.3125,0.0,0.0,809 +-0.0234375,-0.3125,0.0,0.0,824 +-0.015625,-0.3125,0.0,0.0,844 +-0.0078125,-0.3125,0.0,0.0,864 +-0.0078125,-0.3046875,0.0,0.0,885 +-0.0078125,-0.3203125,0.0,0.0,904 +-0.0078125,-0.3359375,0.0,0.0,924 +-0.0078125,-0.3515625,0.0,0.0,1013 +-0.0078125,-0.3515625,0.0,0.0,1028 +-0.0078125,-0.3515625,0.0,0.0,1040 +-0.0078125,-0.3515625,0.0,0.0,1056 +-0.0078125,-0.359375,0.0,0.0,1074 +-0.0078125,-0.3671875,0.0,0.0,1087 +-0.0078125,-0.3671875,0.0,0.0,1098 +-0.0078125,-0.375,0.0,0.0,1110 +-0.0078125,-0.375,0.0,0.0,1121 +-0.0078125,-0.375,0.0,0.0,1133 +-0.0078125,-0.375,0.0,0.0,1146 +-0.0078125,-0.390625,0.0,0.0,1164 +-0.0078125,-0.3984375,0.0,0.0,1203 +-0.0078125,-0.3984375,0.0,0.0,1216 +-0.0078125,-0.40625,0.0,0.0,1228 +-0.0078125,-0.421875,0.0,0.0,1245 +-0.0078125,-0.421875,0.0,0.0,1264 +-0.0078125,-0.421875,0.0,0.0,1284 +-0.0078125,-0.421875,0.0,0.0,1305 +-0.0078125,-0.421875,0.0,0.0,1324 +-0.0078125,-0.421875,0.0,0.0,1345 +-0.0078125,-0.421875,0.0,0.0,1364 +-0.0078125,-0.421875,0.0,0.0,1384 +-0.0078125,-0.421875,0.0,0.0,1404 +-0.0078125,-0.421875,0.0,0.0,1441 +-0.0078125,-0.421875,0.0,0.0,1456 +-0.0078125,-0.421875,0.0,0.0,1468 +-0.0078125,-0.421875,0.0,0.0,1484 +-0.0078125,-0.421875,0.0,0.0,1505 +-0.0078125,-0.421875,0.0,0.0,1525 +-0.0078125,-0.421875,0.0,0.0,1546 +-0.0078125,-0.421875,0.0,0.0,1564 +-0.015625,-0.421875,0.0,0.0,1584 +-0.015625,-0.421875,0.0,0.0,1604 +-0.015625,-0.421875,0.0,0.0,1624 +-0.015625,-0.421875,0.0,0.0,1644 +-0.015625,-0.421875,0.0,0.0,1685 +-0.015625,-0.421875,0.0,0.0,1700 +-0.015625,-0.421875,0.0,0.0,1716 +-0.015625,-0.421875,0.0,0.0,1730 +-0.015625,-0.421875,0.0,0.0,1744 +-0.015625,-0.421875,0.0,0.0,1764 +-0.015625,-0.421875,0.0,0.0,1785 +-0.015625,-0.421875,0.0,0.0,1805 +-0.015625,-0.421875,0.0,0.0,1825 +-0.015625,-0.4375,0.0,0.0,1845 +-0.0234375,-0.4375,0.0,0.0,1864 +-0.0234375,-0.4375,0.0,0.0,1885 +-0.0234375,-0.4375,0.0,0.0,1925 +-0.0234375,-0.4375,0.0,0.0,1940 +-0.0234375,-0.4375,0.0,0.0,1955 +-0.0234375,-0.4375,0.0,0.0,1967 +-0.0234375,-0.4375,0.0,0.0,1986 +-0.0234375,-0.4375,0.0,0.0,2006 +-0.0234375,-0.4375,0.0,0.0,2024 +-0.0234375,-0.4375,0.0,0.0,2044 +-0.0234375,-0.4375,0.0,0.0,2064 +-0.0234375,-0.4375,0.0,0.0,2085 +-0.0234375,-0.4375,0.0,0.0,2105 +-0.0234375,-0.4375,0.0,0.0,2125 +-0.0234375,-0.4375,0.0,0.0,2174 +-0.0234375,-0.4375,0.0,0.0,2189 +-0.0234375,-0.4375,0.0,0.0,2202 +-0.0234375,-0.4375,0.0,0.0,2218 +-0.0234375,-0.4375,0.0,0.0,2233 +-0.0234375,-0.4375,0.0,0.0,2250 +-0.0234375,-0.4375,0.0,0.0,2264 +-0.015625,-0.4375,0.0,0.0,2284 +-0.015625,-0.4375,0.0,0.0,2304 +-0.015625,-0.4375,0.0,0.0,2324 +-0.015625,-0.4375,0.0,0.0,2344 +-0.015625,-0.4375,0.0,0.0,2364 +-0.015625,-0.4375,0.0,0.0,2407 +-0.015625,-0.4375,0.0,0.0,2423 +-0.015625,-0.4375,0.03125,-0.1015625,2440 +-0.015625,-0.4375,0.03125,-0.1015625,2451 +-0.015625,-0.4296875,0.0703125,-0.203125,2466 +-0.015625,-0.4296875,0.1171875,-0.28125,2484 +-0.015625,-0.4296875,0.1484375,-0.359375,2505 +-0.015625,-0.4296875,0.1875,-0.453125,2524 +-0.015625,-0.4296875,0.1875,-0.453125,2544 +-0.015625,-0.4296875,0.21875,-0.5625,2564 +-0.015625,-0.4296875,0.2421875,-0.6015625,2584 +-0.015625,-0.4296875,0.28125,-0.7109375,2606 +-0.015625,-0.4296875,0.2890625,-0.84375,2644 +-0.015625,-0.4296875,0.28089875327071334,-0.9597374070082707,2661 +-0.015625,-0.4296875,0.28089875327071334,-0.9597374070082707,2672 +-0.015625,-0.4296875,0.2707455769182841,-0.962650940153899,2689 +-0.015625,-0.4296875,0.2707455769182841,-0.962650940153899,2704 +-0.015625,-0.4296875,0.19905992429010463,-0.9799873195820535,2724 +-0.015625,-0.4296875,0.18428853505018536,-0.9828721869343219,2745 +-0.015625,-0.4296875,0.16939121559933262,-0.9855488907597534,2764 +-0.015625,-0.4296875,0.1543768802736096,-0.9880120337511015,2784 +-0.015625,-0.4296875,0.07788766729290965,-0.9969621413492434,2804 +-0.015625,-0.4296875,0.03903273174035336,-0.999237932553046,2825 +-0.015625,-0.4296875,0.023431065349237362,-0.9997254549007941,2845 +-0.015625,-0.4296875,-0.0,-1.0,2885 +-0.015625,-0.4296875,-0.0,-1.0,2900 +-0.015625,-0.4296875,-0.0,-1.0,2913 +-0.015625,-0.4296875,-0.0,-1.0,2930 +-0.015625,-0.4296875,-0.0,-1.0,2944 +-0.015625,-0.4296875,-0.08629110282549445,-0.9962699662105448,2965 +-0.015625,-0.4296875,-0.10957246682398805,-0.9939788098918941,2984 +-0.015625,-0.4296875,-0.14795964550591104,-0.9889933990182973,3004 +-0.015625,-0.4296875,-0.20795087166990847,-0.9781392717664111,3025 +-0.015625,-0.4296875,-0.22261635546060413,-0.974906127933063,3045 +-0.015625,-0.4296875,-0.24433176541606075,-0.9696916976073742,3064 +-0.015625,-0.4296875,-0.2586093438103028,-0.9659819911851382,3084 +-0.015625,-0.4296875,-0.3072217021263233,-0.9516379698932809,3117 +-0.015625,-0.4296875,-0.3535437990815464,-0.9354179718879616,3129 +-0.015625,-0.4296875,-0.3535437990815464,-0.9354179718879616,3144 +-0.015625,-0.421875,-0.43296180587754685,-0.9014122667521522,3164 +-0.015625,-0.4140625,-0.46113272661989396,-0.8873311717955715,3184 +-0.015625,-0.4140625,-0.46113272661989396,-0.8873311717955715,3204 +-0.015625,-0.40625,-0.5034479718548448,-0.8640255433928116,3224 +-0.015625,-0.40625,-0.5034479718548448,-0.8640255433928116,3245 +-0.015625,-0.40625,-0.5424281727058204,-0.8401021827462566,3264 +-0.015625,-0.40625,-0.5470717552428331,-0.8370857152141146,3284 +-0.015625,-0.40625,-0.5695464964539495,-0.8219591160009305,3304 +-0.015625,-0.40625,-0.594924795762871,-0.8037813679020596,3325 +-0.015625,-0.40625,-0.6851738491603396,-0.7283795689246123,3417 +-0.015625,-0.4140625,-0.7305249012857322,-0.6828860582860593,3430 +-0.015625,-0.4140625,-0.7363002262116671,-0.6766549910261859,3442 +-0.015625,-0.4140625,-0.7363002262116671,-0.6766549910261859,3453 +-0.015625,-0.421875,-0.7392885165971433,-0.673388809847324,3468 +-0.0234375,-0.421875,-0.7731493128109427,-0.6342240456652264,3482 +-0.0234375,-0.421875,-0.7731493128109427,-0.6342240456652264,3499 +-0.0234375,-0.421875,-0.7820595514434193,-0.6232037050564748,3513 +-0.0234375,-0.421875,-0.7940055545690287,-0.6079105027169126,3529 +-0.0234375,-0.421875,-0.7940055545690287,-0.6079105027169126,3541 +-0.0234375,-0.421875,-0.7940055545690287,-0.6079105027169126,3552 +-0.0234375,-0.421875,-0.8030011791882969,-0.5959774376788141,3565 +-0.0234375,-0.421875,-0.8210359184762267,-0.5708765370655013,3598 +-0.0234375,-0.421875,-0.8300495997825932,-0.5576895748539298,3614 +-0.0234375,-0.421875,-0.8360479108370626,-0.5486564414868224,3626 +-0.0078125,-0.421875,-0.8420323756982735,-0.5394269906817064,3645 +-0.0078125,-0.421875,-0.8450179582407706,-0.5347379266992377,3664 +-0.0078125,-0.421875,-0.8509727940026032,-0.5252097712984816,3685 +0.0,-0.421875,-0.862799360311186,-0.5055465001823355,3705 +0.0,-0.421875,-0.8744793416769986,-0.48506275983646013,3724 +0.0,-0.421875,-0.8888031674084939,-0.45828913319500464,3744 +0.0,-0.421875,-0.8888031674084939,-0.45828913319500464,3764 +0.0,-0.421875,-0.8972134044289655,-0.4415972224923814,3784 +0.0,-0.421875,-0.8999813238235361,-0.4359284537270253,3804 +0.0,-0.421875,-0.9027301154156147,-0.43020732062775385,3846 +0.0,-0.421875,-0.9135170055402336,-0.4068005415296353,3860 +0.0,-0.421875,-0.9187733502125238,-0.3947854239194439,3875 +0.0,-0.4140625,-0.9264665771223092,-0.3763770469559381,3891 +0.0,-0.4140625,-0.9264665771223092,-0.3763770469559381,3905 +0.0,-0.40625,-0.9363291775690445,-0.3511234415883917,3925 +0.0,-0.40625,-0.9363291775690445,-0.3511234415883917,3944 +0.007874015718698502,-0.3984375,-0.9586468662780967,-0.28459828842630996,3964 +0.023622047156095505,-0.390625,-0.9645897061200785,-0.263754997767209,3984 +0.06299212574958801,-0.3828125,-0.9828721869343219,-0.18428853505018536,4005 +0.07086614519357681,-0.3828125,-0.9902565788380345,-0.1392548313990986,4025 +0.07086614519357681,-0.3828125,-0.9940716917543756,-0.10872659128563483,4044 +0.07874015718698502,-0.3828125,-0.9995120760870788,-0.031234752377721213,4082 +0.07874015718698502,-0.3828125,-0.9995120760870788,-0.031234752377721213,4094 +0.07874015718698502,-0.3828125,-0.9995120760870788,-0.031234752377721213,4110 +0.07874015718698502,-0.3828125,-0.9997254549007941,-0.023431065349237362,4126 +0.07874015718698502,-0.3828125,-0.9997254549007941,-0.023431065349237362,4144 +0.07874015718698502,-0.3828125,-0.9997254549007941,-0.023431065349237362,4164 +0.07874015718698502,-0.3828125,-0.9997254549007941,-0.023431065349237362,4184 +0.07874015718698502,-0.3828125,-0.9997254549007941,-0.023431065349237362,4204 +0.07874015718698502,-0.3828125,-0.9997254549007941,-0.023431065349237362,4224 +0.07874015718698502,-0.3828125,-0.9999694838187878,-0.00781226159233428,4253 +0.07874015718698502,-0.3828125,-1.0,0.0,4265 +0.07874015718698502,-0.3828125,-1.0,0.0,4285 +0.08661417663097382,-0.3828125,-1.0,0.0,4324 +0.08661417663097382,-0.3828125,-1.0,0.0,4337 +0.08661417663097382,-0.3828125,-1.0,0.0,4354 +0.08661417663097382,-0.3828125,-1.0,0.0,4367 +0.09448818862438202,-0.3828125,-0.9997211161517748,0.02361545934868166,4386 +0.09448818862438202,-0.3828125,-0.999504367732367,0.03148045240972986,4404 +0.09448818862438202,-0.3828125,-0.9988858624996851,0.04719145789504938,4424 +0.10236220806837082,-0.3828125,-0.9980218809979032,0.06286751982866029,4445 +0.11811023950576782,-0.3828125,-0.994801803805476,0.10183010922792674,4465 +0.12598425149917603,-0.3828125,-0.9901048130433447,0.14032982287597778,4484 +0.12598425149917603,-0.3828125,-0.9796804960291332,0.20056451755011773,4504 +0.13385826349258423,-0.375,-0.9678596169756838,0.2514910770339234,4525 +0.15748031437397003,-0.359375,-0.9494282307963439,0.31398413107500234,4605 +0.16535432636737823,-0.34375,-0.9378378069653893,0.34707383627457283,4621 +0.16535432636737823,-0.34375,-0.9378378069653893,0.34707383627457283,4634 +0.16535432636737823,-0.34375,-0.9378378069653893,0.34707383627457283,4649 +0.18110236525535583,-0.34375,-0.9354179718879616,0.3535437990815464,4661 +0.20472441613674164,-0.3359375,-0.9279719185115246,0.3726501287455037,4680 +0.20472441613674164,-0.3359375,-0.9279719185115246,0.3726501287455037,4694 +0.23622047901153564,-0.3203125,-0.912324307310268,0.40946838497109816,4710 +0.26771652698516846,-0.3203125,-0.9014122667521522,0.43296180587754685,4721 +0.28346458077430725,-0.3125,-0.893016444170718,0.4500240331812145,4740 +0.28346458077430725,-0.3125,-0.893016444170718,0.4500240331812145,4751 +0.30708661675453186,-0.3125,-0.8786876142186625,0.47739718957982463,4764 +0.32283464074134827,-0.3046875,-0.8728573979584765,0.48797537112968914,4800 +0.32283464074134827,-0.3046875,-0.8640255433928116,0.5034479718548448,4817 +0.32283464074134827,-0.3046875,-0.8580885160559446,0.5135018000094129,4828 +0.32283464074134827,-0.3046875,-0.8521187304508965,0.5233485160146654,4844 +0.32283464074134827,-0.28125,-0.8461215648085673,0.5329899600985946,4864 +0.32283464074134827,-0.28125,-0.8401021827462566,0.5424281727058204,4884 +0.32283464074134827,-0.2734375,-0.8340655321723693,0.5516653768744438,4905 +0.32283464074134827,-0.2734375,-0.8249884705123031,0.5651495585433743,4924 +0.32283464074134827,-0.265625,-0.8158981942547746,0.5781955868145294,4944 +0.32283464074134827,-0.2578125,-0.8128676272595764,0.5824484702987778,4964 +0.32283464074134827,-0.2578125,-0.8068087413186813,0.5908127071515688,4984 +0.32283464074134827,-0.25,-0.781483297246057,0.6239261623985893,5004 +0.32283464074134827,-0.2421875,-0.7697579751547384,0.6383358517940827,5038 +0.3385826647281647,-0.234375,-0.7614733174322016,0.6481962564214621,5050 +0.34645670652389526,-0.21875,-0.7259526750390194,0.6877446572701906,5064 +0.35433071851730347,-0.21875,-0.6974437947486879,0.7166394861899184,5084 +0.36220473051071167,-0.21875,-0.6675450352577801,0.7445694231585723,5104 +0.3700787425041199,-0.2109375,-0.6427352020960505,0.7660884152541071,5124 +0.3700787425041199,-0.203125,-0.60209217677236,0.7984265843955356,5145 +0.3700787425041199,-0.203125,-0.5681906995469479,0.8228969127104258,5165 +0.3779527544975281,-0.203125,-0.5516653768744438,0.8340655321723693,5184 +0.3779527544975281,-0.203125,-0.5470717552428331,0.8370857152141146,5204 +0.3700787425041199,-0.203125,-0.5329899600985946,0.8461215648085673,5224 +0.3779527544975281,-0.203125,-0.5233485160146654,0.8521187304508965,5244 +0.3779527544975281,-0.203125,-0.5135018000094129,0.8580885160559446,5278 +0.3779527544975281,-0.203125,-0.5135018000094129,0.8580885160559446,5294 +0.4015747904777527,-0.171875,-0.5085008816513332,0.8610614689787348,5305 +0.4015747904777527,-0.1640625,-0.5085008816513332,0.8610614689787348,5324 +0.4015747904777527,-0.1640625,-0.5085008816513332,0.8610614689787348,5344 +0.4015747904777527,-0.15625,-0.5034479718548448,0.8640255433928116,5365 +0.4015747904777527,-0.15625,-0.49318540967799457,0.8699242218036859,5384 +0.4015747904777527,-0.15625,-0.49318540967799457,0.8699242218036859,5404 +0.4015747904777527,-0.15625,-0.49318540967799457,0.8699242218036859,5424 +0.4015747904777527,-0.15625,-0.49318540967799457,0.8699242218036859,5445 +0.4094488322734833,-0.15625,-0.49318540967799457,0.8699242218036859,5464 +0.4094488322734833,-0.15625,-0.49318540967799457,0.8699242218036859,5484 +0.4251968562602997,-0.15625,-0.472028758555712,0.8815831504154066,5518 +0.4251968562602997,-0.15625,-0.472028758555712,0.8815831504154066,5534 +0.4330708682537079,-0.15625,-0.472028758555712,0.8815831504154066,5551 +0.4409448802471161,-0.15625,-0.466607297434484,0.8844646007505824,5565 +0.4409448802471161,-0.15625,-0.46113272661989396,0.8873311717955715,5584 +0.4409448802471161,-0.15625,-0.46113272661989396,0.8873311717955715,5605 +0.4409448802471161,-0.15625,-0.46113272661989396,0.8873311717955715,5624 +0.4409448802471161,-0.15625,-0.466607297434484,0.8844646007505824,5644 +0.4488188922405243,-0.15625,-0.45560498552550965,0.8901820584376546,5665 +0.4409448802471161,-0.15625,-0.4500240331812145,0.893016444170718,5685 +0.4409448802471161,-0.15625,-0.4500240331812145,0.893016444170718,5704 +0.4409448802471161,-0.15625,-0.4500240331812145,0.893016444170718,5725 +0.4409448802471161,-0.15625,-0.43296180587754685,0.9014122667521522,5798 +0.4409448802471161,-0.15625,-0.42716801226970363,0.9041722674875349,5815 +0.4803149700164795,-0.15625,-0.415421189526273,0.9096291746050015,5829 +0.4960629940032959,-0.1484375,-0.40946838497109816,0.912324307310268,5844 +0.4960629940032959,-0.1484375,-0.40946838497109816,0.912324307310268,5858 +0.5039370059967041,-0.1328125,-0.3912940791117502,0.920265691880387,5871 +0.5039370059967041,-0.1328125,-0.3535437990815464,0.9354179718879616,5884 +0.5039370059967041,-0.1328125,-0.3535437990815464,0.9354179718879616,5896 +0.5118110179901123,-0.125,-0.3206991089217746,0.9471811239339495,5909 +0.5196850299835205,-0.1171875,-0.3072217021263233,0.9516379698932809,5926 +0.5118110179901123,-0.09375,-0.27271945919792484,0.9620936007347682,5944 +0.5118110179901123,-0.0859375,-0.2586093438103028,0.9659819911851382,5965 +0.5196850299835205,-0.046875,-0.22261635546060413,0.974906127933063,6005 +0.5196850299835205,-0.046875,-0.22261635546060413,0.974906127933063,6018 +0.5196850299835205,-0.0078125,-0.21530182214487537,0.9765475540807507,6031 +0.5275590419769287,0.0,-0.18568977934940398,0.9826084193844309,6044 +0.5275590419769287,0.0,-0.17820357578581228,0.9839936410247528,6065 +0.5275590419769287,0.0,-0.1631390877346237,0.9866030802978037,6084 +0.5275590419769287,0.0,-0.14032982287597778,0.9901048130433447,6104 +0.5275590419769287,0.0,-0.09406919601194683,0.995565661501875,6125 +0.5275590419769287,0.0,-0.055034575740075024,0.998484449289577,6144 +0.5275590419769287,0.015748031437397003,-0.02361545934868166,0.9997211161517748,6164 +0.5275590419769287,0.031496062874794006,-0.0,1.0,6184 +0.5275590419769287,0.05511811003088951,-0.0,1.0,6204 +0.5275590419769287,0.07086614519357681,-0.0,1.0,6233 +0.5275590419769287,0.08661417663097382,-0.0,1.0,6247 +0.5275590419769287,0.11811023950576782,0.023431065349237362,0.9997254549007941,6265 +0.5275590419769287,0.13385826349258423,0.05460590540193252,0.9985079844924803,6285 +0.5275590419769287,0.13385826349258423,0.07013933466922019,0.997537204184465,6304 +0.5275590419769287,0.14173229038715363,0.12403473458920847,0.9922778767136677,6324 +0.5275590419769287,0.14173229038715363,0.1543768802736096,0.9880120337511015,6344 +0.5196850299835205,0.14173229038715363,0.18428853505018536,0.9828721869343219,6365 +0.4881889820098877,0.14960630238056183,0.19169051231966405,0.98145542307668,6384 +0.4645669162273407,0.14960630238056183,0.24253562503633297,0.9701425001453319,6405 +0.4566929042339325,0.14960630238056183,0.28459828842630996,0.9586468662780967,6425 +0.4488188922405243,0.15748031437397003,0.33819872616315155,0.941074716280074,6444 +0.4409448802471161,0.15748031437397003,0.3763770469559381,0.9264665771223092,6479 +0.4409448802471161,0.15748031437397003,0.3763770469559381,0.9264665771223092,6493 +0.4251968562602997,0.15748031437397003,0.4359284537270253,0.8999813238235361,6509 +0.4173228442668915,0.15748031437397003,0.47450986523092026,0.8802501847761999,6527 +0.4094488322734833,0.15748031437397003,0.49026123963255896,0.8715755371245493,6544 +0.4015747904777527,0.16535432636737823,0.5252097712984816,0.8509727940026032,6564 +0.3937007784843445,0.18110236525535583,0.5394269906817064,0.8420323756982735,6585 +0.3937007784843445,0.18110236525535583,0.5531973991682577,0.833050201100435,6604 +0.3700787425041199,0.18110236525535583,0.5576895748539298,0.8300495997825932,6624 +0.3700787425041199,0.18110236525535583,0.5576895748539298,0.8300495997825932,6645 +0.33070865273475647,0.18110236525535583,0.6079105027169126,0.7940055545690287,6664 +0.32283464074134827,0.18110236525535583,0.6117992010161086,0.7910131083844636,6684 +0.29921260476112366,0.18110236525535583,0.6232037050564748,0.7820595514434193,6705 +0.27559053897857666,0.18897637724876404,0.6305926250944657,0.7761140001162655,6725 +0.27559053897857666,0.18897637724876404,0.6342240456652264,0.7731493128109427,6744 +0.27559053897857666,0.18897637724876404,0.6378139711853661,0.7701904557710081,6764 +0.28346458077430725,0.18897637724876404,0.6483387853676998,0.7613519681382164,6784 +0.28346458077430725,0.18897637724876404,0.6610770596337712,0.7503180133956318,6805 +0.28346458077430725,0.18897637724876404,0.6610770596337712,0.7503180133956318,6825 +0.27559053897857666,0.18897637724876404,0.673388809847324,0.7392885165971433,6845 +0.28346458077430725,0.18897637724876404,0.6828860582860593,0.7305249012857322,6864 +0.28346458077430725,0.18897637724876404,0.6828860582860593,0.7305249012857322,6884 +0.28346458077430725,0.18897637724876404,0.6890717737753316,0.7246931009648968,6905 +0.28346458077430725,0.18897637724876404,0.6951319975197523,0.7188821224819818,6924 +0.27559053897857666,0.18897637724876404,0.7283795689246123,0.6851738491603396,7006 +0.27559053897857666,0.18897637724876404,0.7283795689246123,0.6851738491603396,7019 +0.27559053897857666,0.18897637724876404,0.7373074742730845,0.6755573168732946,7035 +0.27559053897857666,0.18897637724876404,0.7519173304971933,0.6592574065552654,7052 +0.27559053897857666,0.18897637724876404,0.7601819876238485,0.6497102013145976,7066 +0.27559053897857666,0.20472441613674164,0.7657444477625389,0.6431449608920561,7083 +0.27559053897857666,0.22047244012355804,0.7742502396090526,0.6328795829107815,7100 +0.27559053897857666,0.22047244012355804,0.7742502396090526,0.6328795829107815,7113 +0.27559053897857666,0.22834645211696625,0.7856739074383975,0.6186408579866678,7126 +0.27559053897857666,0.22834645211696625,0.7947129335869222,0.6069854637383567,7141 +0.27559053897857666,0.22834645211696625,0.7947129335869222,0.6069854637383567,7154 +0.27559053897857666,0.22834645211696625,0.8158981942547746,0.5781955868145294,7170 +0.26771652698516846,0.22834645211696625,0.8219591160009305,0.5695464964539495,7208 +0.26771652698516846,0.23622047901153564,0.8219591160009305,0.5695464964539495,7223 +0.26771652698516846,0.26771652698516846,0.8401021827462566,0.5424281727058204,7240 +0.26771652698516846,0.26771652698516846,0.8401021827462566,0.5424281727058204,7253 +0.25196850299835205,0.27559053897857666,0.8461215648085673,0.5329899600985946,7266 +0.25196850299835205,0.28346458077430725,0.8580885160559446,0.5135018000094129,7287 +0.25196850299835205,0.28346458077430725,0.8728573979584765,0.48797537112968914,7304 +0.22834645211696625,0.28346458077430725,0.8986323918355212,0.4387024325712936,7325 +0.22834645211696625,0.28346458077430725,0.9041722674875349,0.42716801226970363,7344 +0.21259842813014984,0.28346458077430725,0.9096291746050015,0.415421189526273,7364 +0.21259842813014984,0.28346458077430725,0.920265691880387,0.3912940791117502,7384 +0.21259842813014984,0.28346458077430725,0.9254308345456299,0.3789165745545833,7405 +0.21259842813014984,0.29133859276771545,0.932966418843752,0.35996341662711334,7439 +0.21259842813014984,0.29133859276771545,0.932966418843752,0.35996341662711334,7452 +0.22047244012355804,0.29133859276771545,0.9494282307963439,0.31398413107500234,7465 +0.22047244012355804,0.29133859276771545,0.9538094063872763,0.300412410341436,7509 +0.22047244012355804,0.29133859276771545,0.9640596934790533,0.2656857305334137,7533 +0.22047244012355804,0.29133859276771545,0.9678596169756838,0.2514910770339234,7550 +0.22047244012355804,0.29133859276771545,0.9781392717664111,0.20795087166990847,7563 +0.22047244012355804,0.29133859276771545,0.9781392717664111,0.20795087166990847,7579 +0.22047244012355804,0.29133859276771545,0.9811704629340057,0.19314378754148134,7592 +0.22047244012355804,0.29133859276771545,0.9853254218311062,0.17068630025093642,7606 +0.22047244012355804,0.29133859276771545,0.992157222284201,0.12499618501897668,7624 +0.22047244012355804,0.29133859276771545,0.9939788098918941,0.10957246682398805,7644 +0.22047244012355804,0.29133859276771545,0.995565661501875,0.09406919601194683,7679 +0.22047244012355804,0.29133859276771545,0.9969143348043864,0.07849719142445599,7693 +0.22047244012355804,0.29133859276771545,0.998484449289577,0.055034575740075024,7710 +0.22047244012355804,0.29133859276771545,0.999504367732367,0.03148045240972986,7726 +0.22047244012355804,0.29133859276771545,0.9998760228122497,0.01574607904074679,7744 +0.22047244012355804,0.29133859276771545,1.0,0.0,7764 +0.22047244012355804,0.29133859276771545,1.0,0.0,7784 +0.22047244012355804,0.29133859276771545,1.0,0.0,7804 +0.22047244012355804,0.29133859276771545,1.0,0.0,7825 +0.22047244012355804,0.29133859276771545,1.0,0.0,7844 +0.22047244012355804,0.29133859276771545,1.0,0.0,7864 +0.22047244012355804,0.29133859276771545,1.0,0.0,7884 +0.22047244012355804,0.29133859276771545,1.0,0.0,7923 +0.22047244012355804,0.29133859276771545,1.0,0.0,7938 +0.22047244012355804,0.29133859276771545,0.9997254549007941,-0.023431065349237362,7950 +0.22047244012355804,0.29133859276771545,0.9985079844924803,-0.05460590540193252,7967 +0.22047244012355804,0.29133859276771545,0.997537204184465,-0.07013933466922019,7984 +0.22047244012355804,0.29133859276771545,0.9963277012186765,-0.08562191182348002,8004 +0.22047244012355804,0.29133859276771545,0.9956342260592882,-0.09334070869305827,8024 +0.22047244012355804,0.29133859276771545,0.9956342260592882,-0.09334070869305827,8044 +0.22047244012355804,0.30708661675453186,0.9855488907597534,-0.16939121559933262,8064 +0.22047244012355804,0.30708661675453186,0.9855488907597534,-0.16939121559933262,8086 +0.22047244012355804,0.31496062874794006,0.9799873195820535,-0.19905992429010463,8105 +0.22047244012355804,0.33070865273475647,0.9784686026666812,-0.20639572087500307,8124 +0.22047244012355804,0.35433071851730347,0.9701425001453319,-0.24253562503633297,8186 +0.22047244012355804,0.36220473051071167,0.9664851269264961,-0.2567226118398505,8200 +0.22047244012355804,0.36220473051071167,0.9645897061200785,-0.263754997767209,8213 +0.22047244012355804,0.3937007784843445,0.9565833270275276,-0.29145898245369983,8228 +0.22047244012355804,0.4015747904777527,0.9479401182738548,-0.3184486334826231,8243 +0.22047244012355804,0.4015747904777527,0.9479401182738548,-0.3184486334826231,8257 +0.20472441613674164,0.4173228442668915,0.9456865993048666,-0.3250797685110479,8271 +0.20472441613674164,0.4173228442668915,0.9456865993048666,-0.3250797685110479,8285 +0.19685038924217224,0.4566929042339325,0.9387181932794337,-0.3446855865947921,8305 +0.19685038924217224,0.4566929042339325,0.9387181932794337,-0.3446855865947921,8325 +0.18897637724876404,0.4645669162273407,0.9339085898481578,-0.35751188205124795,8344 +0.18110236525535583,0.4724409580230713,0.9289763737447976,-0.37013902391394277,8364 +0.15748031437397003,0.4881889820098877,0.8972134044289655,-0.4415972224923814,8400 +0.15748031437397003,0.4881889820098877,0.8972134044289655,-0.4415972224923814,8416 +0.14960630238056183,0.4960629940032959,0.8888031674084939,-0.45828913319500464,8426 +0.14960630238056183,0.4960629940032959,0.8831157194574106,-0.4691552259617494,8444 +0.15748031437397003,0.5039370059967041,0.8802501847761999,-0.47450986523092026,8464 +0.15748031437397003,0.5039370059967041,0.8773711395523853,-0.4798123419427107,8486 +0.14960630238056183,0.5039370059967041,0.8744793416769986,-0.48506275983646013,8505 +0.11023622006177902,0.5039370059967041,0.8657348311141284,-0.5005029492378555,8524 +0.09448818862438202,0.5039370059967041,0.8598547438407345,-0.5105387541554361,8544 +0.07874015718698502,0.5039370059967041,0.8569016654805386,-0.5154799081406365,8564 +0.07874015718698502,0.5039370059967041,0.8539407961853737,-0.520370172675462,8584 +0.07874015718698502,0.5039370059967041,0.8509727940026032,-0.5252097712984816,8604 +0.07086614519357681,0.5039370059967041,0.8450179582407706,-0.5347379266992377,8625 +0.06299212574958801,0.5039370059967041,0.8360479108370626,-0.5486564414868224,8661 +0.06299212574958801,0.5039370059967041,0.8360479108370626,-0.5486564414868224,8673 +0.06299212574958801,0.5039370059967041,0.8240419241993676,-0.5665288228870652,8689 +0.05511811003088951,0.5039370059967041,0.8240419241993676,-0.5665288228870652,8704 +0.05511811003088951,0.5039370059967041,0.8210359184762267,-0.5708765370655013,8724 +0.031496062874794006,0.5039370059967041,0.8090093109439124,-0.5877958274826863,8745 +0.03937007859349251,0.5039370059967041,0.8030011791882969,-0.5959774376788141,8764 +0.031496062874794006,0.5039370059967041,0.8,-0.6,8784 +0.023622047156095505,0.5039370059967041,0.7970013198156258,-0.6039775626727789,8805 +0.031496062874794006,0.5039370059967041,0.7880243737245634,-0.6156440419723151,8824 +0.031496062874794006,0.5039370059967041,0.7820595514434193,-0.6232037050564748,8844 +0.031496062874794006,0.5039370059967041,0.7707025822937636,-0.6371950483531117,8864 +0.023622047156095505,0.5039370059967041,0.7421026427511382,-0.6702862579687701,8897 +0.023622047156095505,0.5039370059967041,0.7421026427511382,-0.6702862579687701,8909 +0.023622047156095505,0.5039370059967041,0.7393958180127342,-0.6732709887595629,8928 +0.015748031437397003,0.5039370059967041,0.7364081333395653,-0.6765375533932593,8945 +0.0,0.5039370059967041,0.7278467063026898,-0.6857398720537738,8964 +0.0,0.5039370059967041,0.7219882051154242,-0.6919053632356148,8985 +0.0,0.5039370059967041,0.7219882051154242,-0.6919053632356148,9005 +0.0,0.5039370059967041,0.7071067811865476,-0.7071067811865476,9024 +0.0,0.5039370059967041,0.6981759636203039,-0.7159261999835319,9046 +0.0,0.5039370059967041,0.6952251158274647,-0.7187920689063618,9064 +0.0,0.5039370059967041,0.6922875402198978,-0.7216217580258257,9085 +0.0,0.5039370059967041,0.6862789815591375,-0.7273384078062654,9122 +0.0,0.5039370059967041,0.6862789815591375,-0.7273384078062654,9133 +0.0,0.5039370059967041,0.6862789815591375,-0.7273384078062654,9146 +0.0,0.5039370059967041,0.6862789815591375,-0.7273384078062654,9165 +0.0,0.5039370059967041,0.6833580581622772,-0.7300833954725184,9184 +0.0,0.5039370059967041,0.6741303561283304,-0.7386123901927794,9204 +0.0,0.5039370059967041,0.6741303561283304,-0.7386123901927794,9224 +0.0,0.5039370059967041,0.6741303561283304,-0.7386123901927794,9244 +0.0,0.5039370059967041,0.6741303561283304,-0.7386123901927794,9265 +0.0,0.5039370059967041,0.6741303561283304,-0.7386123901927794,9285 +0.0,0.5039370059967041,0.6741303561283304,-0.7386123901927794,9305 +0.0,0.5039370059967041,0.6741303561283304,-0.7386123901927794,9325 +0.0,0.5039370059967041,0.6618153215021395,-0.749666912851981,9345 +0.0,0.5039370059967041,0.6618153215021395,-0.749666912851981,9388 +0.0,0.5039370059967041,0.6618153215021395,-0.749666912851981,9401 +0.0,0.5039370059967041,0.6618153215021395,-0.749666912851981,9414 +0.0,0.4960629940032959,0.658504607868518,-0.7525766947068778,9424 +0.0,0.4960629940032959,0.658504607868518,-0.7525766947068778,9445 +0.0,0.4960629940032959,0.6551552162877278,-0.7554943034669294,9465 +0.0,0.4960629940032959,0.6551552162877278,-0.7554943034669294,9484 +0.0,0.4960629940032959,0.6551552162877278,-0.7554943034669294,9504 +0.0,0.4960629940032959,0.6551552162877278,-0.7554943034669294,9524 +0.0,0.4960629940032959,0.6551552162877278,-0.7554943034669294,9544 +0.0,0.4960629940032959,0.6517667432879864,-0.7584194830987478,9564 +-0.0234375,0.4173228442668915,0.6483387853676998,-0.7613519681382164,9630 +-0.0234375,0.4173228442668915,0.6483387853676998,-0.7613519681382164,9643 +-0.03125,0.4173228442668915,0.6483387853676998,-0.7613519681382164,9658 +-0.03125,0.4173228442668915,0.6483387853676998,-0.7613519681382164,9670 +-0.03125,0.4173228442668915,0.6483387853676998,-0.7613519681382164,9686 +-0.03125,0.4173228442668915,0.6483387853676998,-0.7613519681382164,9703 +-0.03125,0.4173228442668915,0.6483387853676998,-0.7613519681382164,9719 +-0.03125,0.4173228442668915,0.6483387853676998,-0.7613519681382164,9733 +-0.03125,0.4173228442668915,0.6483387853676998,-0.7613519681382164,9745 +-0.03125,0.4173228442668915,0.6483387853676998,-0.7613519681382164,9764 +-0.03125,0.4173228442668915,0.6483387853676998,-0.7613519681382164,9784 +-0.03125,0.4173228442668915,0.6483387853676998,-0.7613519681382164,9806 +-0.03125,0.4173228442668915,0.6483387853676998,-0.7613519681382164,9835 +-0.03125,0.4094488322734833,0.6483387853676998,-0.7613519681382164,9850 +-0.03125,0.4094488322734833,0.6483387853676998,-0.7613519681382164,9864 +-0.03125,0.4094488322734833,0.6483387853676998,-0.7613519681382164,9884 +-0.03125,0.4094488322734833,0.6483387853676998,-0.7613519681382164,9905 +-0.03125,0.3937007784843445,0.6483387853676998,-0.7613519681382164,9924 +-0.03125,0.3937007784843445,0.6483387853676998,-0.7613519681382164,9945 +-0.03125,0.3937007784843445,0.6483387853676998,-0.7613519681382164,9964 +-0.03125,0.4015747904777527,0.6483387853676998,-0.7613519681382164,9984 +-0.03125,0.4015747904777527,0.6483387853676998,-0.7613519681382164,10004 +-0.03125,0.3937007784843445,0.6483387853676998,-0.7613519681382164,10024 +-0.03125,0.3937007784843445,0.6483387853676998,-0.7613519681382164,10044 +-0.03125,0.3937007784843445,0.6483387853676998,-0.7613519681382164,10064 +-0.03125,0.3937007784843445,0.6483387853676998,-0.7613519681382164,10094 +-0.03125,0.3779527544975281,0.6483387853676998,-0.7613519681382164,10112 +-0.03125,0.3779527544975281,0.6483387853676998,-0.7613519681382164,10125 +-0.03125,0.3779527544975281,0.6483387853676998,-0.7613519681382164,10145 +-0.03125,0.3779527544975281,0.6483387853676998,-0.7613519681382164,10164 +-0.03125,0.3779527544975281,0.6483387853676998,-0.7613519681382164,10184 +-0.03125,0.3779527544975281,0.6483387853676998,-0.7613519681382164,10204 +-0.03125,0.3779527544975281,0.6483387853676998,-0.7613519681382164,10225 +-0.03125,0.3858267664909363,0.6483387853676998,-0.7613519681382164,10244 +-0.03125,0.3858267664909363,0.6483387853676998,-0.7613519681382164,10265 +-0.03125,0.3858267664909363,0.6483387853676998,-0.7613519681382164,10285 +-0.03125,0.3858267664909363,0.6448709392097829,-0.7642914835078909,10327 +-0.03125,0.3858267664909363,0.6448709392097829,-0.7642914835078909,10339 +-0.03125,0.3858267664909363,0.6448709392097829,-0.7642914835078909,10352 +-0.03125,0.3779527544975281,0.6448709392097829,-0.7642914835078909,10369 +-0.03125,0.3779527544975281,0.6448709392097829,-0.7642914835078909,10385 +-0.03125,0.3779527544975281,0.6448709392097829,-0.7642914835078909,10404 +-0.03125,0.3779527544975281,0.6448709392097829,-0.7642914835078909,10424 +-0.03125,0.3700787425041199,0.6448709392097829,-0.7642914835078909,10445 +-0.03125,0.36220473051071167,0.6378139711853661,-0.7701904557710081,10465 +-0.03125,0.3385826647281647,0.6305926250944657,-0.7761140001162655,10484 +-0.03125,0.31496062874794006,0.6305926250944657,-0.7761140001162655,10504 +-0.03125,0.30708661675453186,0.6156440419723151,-0.7880243737245634,10524 +-0.03125,0.31496062874794006,0.0234375,-0.109375,10580 +-0.0234375,0.25196850299835205,0.0,0.0,10599 +-0.0234375,0.25196850299835205,0.0,0.0,10614 +-0.0234375,0.21259842813014984,0.0,0.0,10627 +-0.0234375,0.10236220806837082,0.0,0.0,10644 +-0.0234375,0.10236220806837082,0.0,0.0,10656 +-0.0234375,0.10236220806837082,0.0,0.0,10667 +0.0,0.0,0.0,0.0,10684 +0.0,0.0,0.0,0.0,10704 +0.0,0.0,0.0,0.0,10724 +0.0,0.0,0.0,0.0,10744 +0.0,0.0,0.0,0.0,10764 +0.0,0.0,0.0,0.0,10843 +0.0,0.0,0.0,0.0,10855 +0.0,0.0,0.0,0.0,10873 +0.0,0.0,0.0,0.0,10885 +0.0,0.0,0.0,0.0,10896 +0.0,0.0,0.0,0.0,10909 +0.0,0.0,0.0,0.0,10922 +0.0,0.0,0.0,0.0,10935 +0.0,0.0,0.0,0.0,10952 +0.0,0.0,0.0,0.0,10965 +0.0,0.0,0.0,0.0,10984 +0.0,0.0,0.0,0.0,11004 +0.0,0.0,0.0,0.0,11035 +0.0,0.0,0.0,0.0,11049 +0.0,0.0,0.0,0.0,11064 +0.0,0.0,0.0,0.0,11084 +0.0,0.0,0.0,0.0,11104 +0.0,0.0,0.0,0.0,11125 +0.0,0.0,0.0,0.0,11145 +0.0,0.0,0.0,0.0,11165 +0.0,0.0,0.0,0.0,11185 +0.0,0.0,0.0,0.0,11204 +0.0,0.0,0.0,0.0,11225 +0.0,0.0,0.0,0.0,11244 +0.0,0.0,0.0,0.0,11284 +0.0,0.0,0.0,0.0,11296 +0.0,0.0,0.0,0.0,11309 +0.0,0.0,0.0,0.0,11325 +0.0,0.0,0.0,0.0,11344 +0.0,0.0,0.0,0.0,11364 +0.0,0.0,0.0,0.0,11384 +0.0,0.0,0.0,0.0,11405 +0.0,0.0,0.0,0.0,11424 +0.0,0.0,0.0,0.0,11444 +0.0,0.0,0.0,0.0,11464 +0.0,0.0,0.0,0.0,11484 +0.0,0.0,0.0,0.0,11526 +0.0,0.0,0.0,0.0,11540 +0.0,0.0,0.0,0.0,11552 +0.0,0.0,0.0,0.0,11566 +0.0,0.0,0.0,0.0,11584 +0.0,0.0,0.0,0.0,11604 +0.0,0.0,0.0,0.0,11625 +0.0,0.0,0.0,0.0,11644 +0.0,0.0,0.0,0.0,11664 +0.0,0.0,0.0,0.0,11684 +0.0,0.0,0.0,0.0,11705 +0.0,0.0,0.0,0.0,11725 +0.0,0.0,0.0,0.0,11789 +0.0,0.0,0.0,0.0,11802 +0.0,0.0,0.0,0.0,11814 +0.0,0.0,0.0,0.0,11829 +0.0,0.0,0.0,0.0,11842 +0.0,0.0,0.0,0.0,11863 +0.0,0.0,0.0,0.0,11879 +0.0,0.0,0.0,0.0,11892 +0.0,0.0,0.0,0.0,11907 \ 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 index 7b3fedc..f43329a 100644 --- a/src/main/java/frc4388/robot/commands/JoystickPlayback.java +++ b/src/main/java/frc4388/robot/commands/JoystickPlayback.java @@ -7,6 +7,7 @@ package frc4388.robot.commands; import java.io.File; import java.io.FileNotFoundException; import java.io.IOException; +import java.io.PrintWriter; import java.nio.file.Files; import java.nio.file.Path; import java.sql.Time; @@ -27,28 +28,39 @@ public class JoystickPlayback extends CommandBase { public double rightX = 0d; public double rightY = 0d; - public long timed_offset = 0l; + public long timedOffset = 0; } - private final SwerveDrive m_swerve; - private Scanner m_input; - private final ArrayList m_outputs; - private long m_playback_time; - private int m_last_index; - private boolean m_finished = false; // ! find a better way + private final SwerveDrive swerve; + private Scanner input; + private final ArrayList outputs; + 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) { // Use addRequirements() here to declare subsystem dependencies. - this.m_swerve = swerve; - m_outputs = new ArrayList<>(); + this.swerve = swerve; + outputs = new ArrayList<>(); + addRequirements(this.swerve); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + startTime = System.currentTimeMillis(); + playbackTime = 0; + lastIndex = 0; try { - m_input = new Scanner(new File("/home/lvuser/JoystickInputs.txt")); + input = new Scanner(new File("/home/lvuser/JoystickInputs.txt")); String line = ""; - while (m_input.hasNextLine()) { - line = m_input.nextLine(); + while (input.hasNextLine()) { + line = input.nextLine(); String[] values = line.split(","); @@ -58,20 +70,16 @@ public class JoystickPlayback extends CommandBase { out.rightX = Double.parseDouble(values[2]); out.rightY = Double.parseDouble(values[3]); - out.timed_offset = Long.MAX_VALUE; + out.timedOffset = Long.parseLong(values[4]); - m_outputs.add(out); + outputs.add(out); } + + input.close(); } catch (FileNotFoundException e) { e.printStackTrace(); } - addRequirements(this.m_swerve); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { System.out.println("STARTING PLAYBACK"); System.out.println("STARTING PLAYBACK"); System.out.println("STARTING PLAYBACK"); @@ -81,34 +89,77 @@ public class JoystickPlayback extends CommandBase { // 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 = m_last_index + 1; - while (i < m_outputs.size() && m_outputs.get(i).timed_offset < m_playback_time) { + int i = lastIndex == 0 ? 1 : lastIndex; + while (i < outputs.size() && outputs.get(i).timedOffset < playbackTime) { i++; } - if (i >= m_outputs.size()) { + if (i >= outputs.size()) { m_finished = true; // ! kind of a hack return; } - m_last_index = i; + lastIndex = i; } - TimedOutput out = m_outputs.get(m_last_index); + TimedOutput lastOut = outputs.get(lastIndex - 1); + TimedOutput out = outputs.get(lastIndex); - this.m_swerve.driveWithInput(new Translation2d(out.leftX, out.leftY), - new Translation2d(-out.rightX, out.rightY), - true); - System.out.println("PLAYING"); + double deltaTime = out.timedOffset - lastOut.timedOffset; + double playbackDelta = playbackTime - lastOut.timedOffset; + + // System.out.println("LastOut.timedOffset: " + lastOut.timedOffset); + // System.out.println("PlaybackTime: " + playbackTime); + // System.out.println("PlaybackDelta: " + playbackDelta); + // System.out.println("DeltaTime: " + deltaTime); + + // // double slopeLX = (out.leftX - lastOut.leftX) / deltaTime; + // // double slopeLY = (out.leftY - lastOut.leftY) / deltaTime; + // // double slopeRX = (out.rightX - lastOut.rightX) / deltaTime; + // // double slopeRY = (out.rightY - lastOut.rightY) / deltaTime; + + 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); + + // System.out.println("----------------------------"); + // System.out.println("lerpLX: " + lerpLX); + // System.out.println("lerpLY: " + lerpLY); + // System.out.println("lerpRX: " + lerpRX); + // System.out.println("lerpRY: " + lerpRY); + // System.out.println("----------------------------"); + + // // double lerpLX = slopeLX * playbackTime + (lastOut.leftX - slopeLX * lastOut.timedOffset); + // // double lerpLY = slopeLY * playbackTime + (lastOut.leftY - slopeLY * lastOut.timedOffset); + // // double lerpRX = slopeRX * playbackTime + (lastOut.rightX - slopeRX * lastOut.timedOffset); + // // double lerpRY = slopeRY * playbackTime + (lastOut.rightY - slopeRY * lastOut.timedOffset); + + // 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); + // System.out.println("PLAYING"); + counter++; } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - m_input.close(); - m_swerve.stopModules(); + input.close(); + swerve.stopModules(); } // Returns true when the command should end. diff --git a/src/main/java/frc4388/robot/commands/JoystickRecorder.java b/src/main/java/frc4388/robot/commands/JoystickRecorder.java index 134af83..29614bb 100644 --- a/src/main/java/frc4388/robot/commands/JoystickRecorder.java +++ b/src/main/java/frc4388/robot/commands/JoystickRecorder.java @@ -28,10 +28,9 @@ public class JoystickRecorder extends CommandBase { Supplier rightXSupplier; Supplier rightYSupplier; - // HashMap timedInput; - ArrayList outputs; + ArrayList outputs; - private final long startTime; + private long startTime; /** Creates a new JoystickRecorder. */ @@ -43,9 +42,7 @@ public class JoystickRecorder extends CommandBase { this.rightXSupplier = rightXSupplier; this.rightYSupplier = rightYSupplier; - this.startTime = System.currentTimeMillis(); - // this.timedInput = new HashMap(); - this.outputs = new ArrayList(); + this.outputs = new ArrayList(); addRequirements(this.swerve); } @@ -53,8 +50,10 @@ public class JoystickRecorder extends CommandBase { // Called when the command is initially scheduled. @Override public void initialize() { + this.startTime = System.currentTimeMillis(); + // timedInput.put((long) 0, new double[] {0.0, 0.0, 0.0, 0.0}); - outputs.add(new double[] {0.0, 0.0, 0.0, 0.0}); + outputs.add(new Object[] {(double) 0.0, (double) 0.0, (double) 0.0, (double) 0.0, (long) 0}); System.out.println("STARTING RECORDING"); System.out.println("STARTING RECORDING"); @@ -65,11 +64,10 @@ public class JoystickRecorder extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double[] inputs = new double[] {leftXSupplier.get(), leftYSupplier.get(), rightXSupplier.get(), rightYSupplier.get()}; - // timedInput.put(System.currentTimeMillis() - startTime, inputs); + Object[] inputs = new Object[] {(double) leftXSupplier.get(), (double) leftYSupplier.get(), (double) rightXSupplier.get(), (double) rightYSupplier.get(), (long) (System.currentTimeMillis() - startTime)}; outputs.add(inputs); - swerve.driveWithInput(new Translation2d(inputs[0], inputs[1]), new Translation2d(-inputs[2], inputs[3]), true); + swerve.driveWithInput(new Translation2d((double) inputs[0], (double) inputs[1]), new Translation2d((double) inputs[2], (double) inputs[3]), true); System.out.println("RECORDING"); } @@ -80,9 +78,11 @@ public class JoystickRecorder extends CommandBase { File output = new File("/home/lvuser/JoystickInputs.txt"); try(PrintWriter writer = new PrintWriter(output)) { - for(double[] input : outputs) { - writer.println(input[0] + "," + input[1] + "," + input[2] + "," + input[3]); + + for(Object[] input : outputs) { + writer.println(input[0] + "," + input[1] + "," + input[2] + "," + input[3] + "," + input[4]); } + writer.close(); } catch(IOException e) { e.printStackTrace(); From 9e9652faaecf6180957331446a835d8709ea060d Mon Sep 17 00:00:00 2001 From: aarav18 Date: Mon, 13 Feb 2023 18:26:00 -0700 Subject: [PATCH 12/16] change --- .../robot/commands/JoystickPlayback.java | 36 ++++--------------- .../robot/commands/JoystickRecorder.java | 12 ------- 2 files changed, 6 insertions(+), 42 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/JoystickPlayback.java b/src/main/java/frc4388/robot/commands/JoystickPlayback.java index f43329a..4240ecf 100644 --- a/src/main/java/frc4388/robot/commands/JoystickPlayback.java +++ b/src/main/java/frc4388/robot/commands/JoystickPlayback.java @@ -23,10 +23,10 @@ import frc4388.utility.controller.DeadbandedXboxController; public class JoystickPlayback extends CommandBase { private static class TimedOutput { - public double leftX = 0d; - public double leftY = 0d; - public double rightX = 0d; - public double rightY = 0d; + public double leftX = 0.0; + public double leftY = 0.0; + public double rightX = 0.0; + public double rightY = 0.0; public long timedOffset = 0; } @@ -81,9 +81,6 @@ public class JoystickPlayback extends CommandBase { } System.out.println("STARTING PLAYBACK"); - System.out.println("STARTING PLAYBACK"); - System.out.println("STARTING PLAYBACK"); - System.out.println("STARTING PLAYBACK"); } // Called every time the scheduler runs while the command is scheduled. @@ -117,33 +114,11 @@ public class JoystickPlayback extends CommandBase { double deltaTime = out.timedOffset - lastOut.timedOffset; double playbackDelta = playbackTime - lastOut.timedOffset; - // System.out.println("LastOut.timedOffset: " + lastOut.timedOffset); - // System.out.println("PlaybackTime: " + playbackTime); - // System.out.println("PlaybackDelta: " + playbackDelta); - // System.out.println("DeltaTime: " + deltaTime); - - // // double slopeLX = (out.leftX - lastOut.leftX) / deltaTime; - // // double slopeLY = (out.leftY - lastOut.leftY) / deltaTime; - // // double slopeRX = (out.rightX - lastOut.rightX) / deltaTime; - // // double slopeRY = (out.rightY - lastOut.rightY) / deltaTime; - 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); - // System.out.println("----------------------------"); - // System.out.println("lerpLX: " + lerpLX); - // System.out.println("lerpLY: " + lerpLY); - // System.out.println("lerpRX: " + lerpRX); - // System.out.println("lerpRY: " + lerpRY); - // System.out.println("----------------------------"); - - // // double lerpLX = slopeLX * playbackTime + (lastOut.leftX - slopeLX * lastOut.timedOffset); - // // double lerpLY = slopeLY * playbackTime + (lastOut.leftY - slopeLY * lastOut.timedOffset); - // // double lerpRX = slopeRX * playbackTime + (lastOut.rightX - slopeRX * lastOut.timedOffset); - // // double lerpRY = slopeRY * playbackTime + (lastOut.rightY - slopeRY * lastOut.timedOffset); - // this.swerve.driveWithInput(new Translation2d(out.leftX, out.leftY), // new Translation2d(out.rightX, out.rightY), // true); @@ -151,7 +126,8 @@ public class JoystickPlayback extends CommandBase { this.swerve.driveWithInput( new Translation2d(lerpLX, lerpLY), new Translation2d(lerpRX, lerpRY), true); - // System.out.println("PLAYING"); + + System.out.println("PLAYING"); counter++; } diff --git a/src/main/java/frc4388/robot/commands/JoystickRecorder.java b/src/main/java/frc4388/robot/commands/JoystickRecorder.java index 29614bb..c13ea6c 100644 --- a/src/main/java/frc4388/robot/commands/JoystickRecorder.java +++ b/src/main/java/frc4388/robot/commands/JoystickRecorder.java @@ -7,17 +7,12 @@ package frc4388.robot.commands; import java.io.File; import java.io.IOException; import java.io.PrintWriter; -import java.nio.file.Files; -import java.nio.file.Path; import java.util.ArrayList; -import java.util.HashMap; 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.RobotTime; -import frc4388.utility.controller.DeadbandedXboxController; public class JoystickRecorder extends CommandBase { @@ -56,9 +51,6 @@ public class JoystickRecorder extends CommandBase { outputs.add(new Object[] {(double) 0.0, (double) 0.0, (double) 0.0, (double) 0.0, (long) 0}); System.out.println("STARTING RECORDING"); - System.out.println("STARTING RECORDING"); - System.out.println("STARTING RECORDING"); - System.out.println("STARTING RECORDING"); } // Called every time the scheduler runs while the command is scheduled. @@ -89,10 +81,6 @@ public class JoystickRecorder extends CommandBase { } System.out.println("STOPPED RECORDING"); - System.out.println("STOPPED RECORDING"); - System.out.println("STOPPED RECORDING"); - System.out.println("STOPPED RECORDING"); - } // Returns true when the command should end. From 603be6538bfce3ccd231e969591db93e89a9b616 Mon Sep 17 00:00:00 2001 From: Abhi <90010729+Abhishrek05@users.noreply.github.com> Date: Tue, 14 Feb 2023 19:00:09 -0700 Subject: [PATCH 13/16] Updated Auto recording --- .../java/frc4388/robot/RobotContainer.java | 71 ++++--------------- .../robot/commands/JoystickPlayback.java | 24 +------ .../robot/commands/JoystickRecorder.java | 65 +++++++++-------- .../java/frc4388/utility/UtilityStructs.java | 12 ++++ 4 files changed, 61 insertions(+), 111 deletions(-) create mode 100644 src/main/java/frc4388/utility/UtilityStructs.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index aa94317..731275b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -7,9 +7,6 @@ package frc4388.robot; -import javax.print.attribute.standard.OrientationRequested; - -import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; @@ -20,7 +17,6 @@ import frc4388.robot.commands.JoystickPlayback; import frc4388.robot.commands.JoystickRecorder; import frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.controller.DeadbandedXboxController; -import frc4388.utility.controller.IHandController; import frc4388.utility.controller.XboxController; /** @@ -35,14 +31,13 @@ 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); @@ -52,13 +47,13 @@ public class RobotContainer { public RobotContainer() { configureButtonBindings(); - /* 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)); + // * Default Commands + m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { + m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(), + getDeadbandedDriverController().getRight(), + true); + }, m_robotSwerveDrive) + .withName("SwerveDrive DefaultCommand")); } @@ -69,14 +64,12 @@ 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(getDeadbandedDriverController(), XboxController.X_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(), m_robotSwerveDrive)); - // // .onFalse() new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) .onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); @@ -86,18 +79,13 @@ public class RobotContainer { () -> getDeadbandedDriverController().getLeftX(), () -> getDeadbandedDriverController().getLeftY(), () -> getDeadbandedDriverController().getRightX(), - () -> getDeadbandedDriverController().getRightY() - - )) + () -> getDeadbandedDriverController().getRightY())) .onFalse(new InstantCommand()); new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) .onTrue(new JoystickPlayback(m_robotSwerveDrive)); - // /* Operator Buttons */ - // // interrupt button - // new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) - // .onTrue(new InstantCommand()); + // * Operator Buttons } /** @@ -106,17 +94,9 @@ public class RobotContainer { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - return new InstantCommand(); } - /** - * Add your docs here. - */ - // public IHandController getDriverController() { - // return m_driverXbox; - // } - public DeadbandedXboxController getDeadbandedDriverController() { return this.m_driverXbox; } @@ -124,25 +104,4 @@ public class RobotContainer { public DeadbandedXboxController getDeadbandedOperatorController() { return this.m_operatorXbox; } - - /** - * Add your docs here. - */ - // public IHandController getOperatorController() { - // return 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/JoystickPlayback.java b/src/main/java/frc4388/robot/commands/JoystickPlayback.java index 4240ecf..6961d09 100644 --- a/src/main/java/frc4388/robot/commands/JoystickPlayback.java +++ b/src/main/java/frc4388/robot/commands/JoystickPlayback.java @@ -6,34 +6,17 @@ package frc4388.robot.commands; import java.io.File; import java.io.FileNotFoundException; -import java.io.IOException; -import java.io.PrintWriter; -import java.nio.file.Files; -import java.nio.file.Path; -import java.sql.Time; import java.util.ArrayList; import java.util.Scanner; -import java.util.function.Supplier; - -import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.subsystems.SwerveDrive; -import frc4388.utility.controller.DeadbandedXboxController; +import frc4388.utility.UtilityStructs.TimedOutput; public class JoystickPlayback extends CommandBase { - private 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; - } - private final SwerveDrive swerve; private Scanner input; - private final ArrayList outputs; + private final ArrayList outputs = new ArrayList<>(); private int counter = 0; private long startTime = 0; private long playbackTime = 0; @@ -42,10 +25,7 @@ public class JoystickPlayback extends CommandBase { /** Creates a new JoystickPlayback. */ public JoystickPlayback(SwerveDrive swerve) { - // Use addRequirements() here to declare subsystem dependencies. this.swerve = swerve; - outputs = new ArrayList<>(); - addRequirements(this.swerve); } diff --git a/src/main/java/frc4388/robot/commands/JoystickRecorder.java b/src/main/java/frc4388/robot/commands/JoystickRecorder.java index c13ea6c..2b3fd24 100644 --- a/src/main/java/frc4388/robot/commands/JoystickRecorder.java +++ b/src/main/java/frc4388/robot/commands/JoystickRecorder.java @@ -13,31 +13,28 @@ 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 { - - SwerveDrive swerve; + public final SwerveDrive swerve; - Supplier leftXSupplier; - Supplier leftYSupplier; - Supplier rightXSupplier; - Supplier rightYSupplier; - - ArrayList outputs; - - private long startTime; + public final Supplier leftX; + public final Supplier leftY; + public final Supplier rightX; + public final Supplier rightY; + public final ArrayList outputs = new ArrayList<>(); + private long startTime = -1; /** Creates a new JoystickRecorder. */ - public JoystickRecorder(SwerveDrive swerve, Supplier leftXSupplier, Supplier leftYSupplier, Supplier rightXSupplier, Supplier rightYSupplier) { - // Use addRequirements() here to declare subsystem dependencies. + public JoystickRecorder(SwerveDrive swerve, Supplier leftX, Supplier leftY, + Supplier rightX, Supplier rightY) + { this.swerve = swerve; - this.leftXSupplier = leftXSupplier; - this.leftYSupplier = leftYSupplier; - this.rightXSupplier = rightXSupplier; - this.rightYSupplier = rightYSupplier; - - this.outputs = new ArrayList(); + this.leftX = leftX; + this.leftY = leftY; + this.rightX = rightX; + this.rightY = rightY; addRequirements(this.swerve); } @@ -47,21 +44,24 @@ public class JoystickRecorder extends CommandBase { public void initialize() { this.startTime = System.currentTimeMillis(); - // timedInput.put((long) 0, new double[] {0.0, 0.0, 0.0, 0.0}); - outputs.add(new Object[] {(double) 0.0, (double) 0.0, (double) 0.0, (double) 0.0, (long) 0}); - - System.out.println("STARTING RECORDING"); + outputs.add(new TimedOutput()); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - Object[] inputs = new Object[] {(double) leftXSupplier.get(), (double) leftYSupplier.get(), (double) rightXSupplier.get(), (double) rightYSupplier.get(), (long) (System.currentTimeMillis() - startTime)}; + 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((double) inputs[0], (double) inputs[1]), new Translation2d((double) inputs[2], (double) inputs[3]), true); - - System.out.println("RECORDING"); + swerve.driveWithInput(new Translation2d(inputs.leftX, inputs.leftY), + new Translation2d(inputs.rightX, inputs.rightY), + true); } // Called once the command ends or is interrupted. @@ -69,18 +69,17 @@ public class JoystickRecorder extends CommandBase { public void end(boolean interrupted) { File output = new File("/home/lvuser/JoystickInputs.txt"); - try(PrintWriter writer = new PrintWriter(output)) { - - for(Object[] input : outputs) { - writer.println(input[0] + "," + input[1] + "," + input[2] + "," + input[3] + "," + input[4]); + 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) { + } catch (IOException e) { e.printStackTrace(); } - - System.out.println("STOPPED RECORDING"); } // Returns true when the command should end. 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; + } +} From b0179bb63929382b196365a15b445b5ddc63ca8b Mon Sep 17 00:00:00 2001 From: Abhi <90010729+Abhishrek05@users.noreply.github.com> Date: Tue, 14 Feb 2023 19:19:52 -0700 Subject: [PATCH 14/16] figure 8 path --- .../frc4388/robot/commands/JoystickInputs.txt | 1483 ++++++++++------- 1 file changed, 886 insertions(+), 597 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/JoystickInputs.txt b/src/main/java/frc4388/robot/commands/JoystickInputs.txt index 263ff3f..b503e48 100644 --- a/src/main/java/frc4388/robot/commands/JoystickInputs.txt +++ b/src/main/java/frc4388/robot/commands/JoystickInputs.txt @@ -1,598 +1,887 @@ 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,19 -0.0,0.0,0.0,0.0,31 -0.0,0.0,0.0,0.0,46 -0.0,0.0,0.0,0.0,64 -0.0,0.0,0.0,0.0,84 -0.0,0.0,0.0,0.0,104 -0.0,0.0,0.0,0.0,124 -0.0,0.0,0.0,0.0,144 -0.0,0.0,0.0,0.0,165 -0.0,0.0,0.0,0.0,185 -0.0,0.0,0.0,0.0,204 -0.0,0.0,0.0,0.0,239 -0.0,0.0,0.0,0.0,250 -0.0,0.0,0.0,0.0,267 -0.0,0.0,0.0,0.0,284 -0.0,0.0,0.0,0.0,305 -0.0,0.0,0.0,0.0,325 --0.046875,-0.140625,0.0,0.0,345 --0.046875,-0.15625,0.0,0.0,364 --0.0390625,-0.1640625,0.0,0.0,385 --0.046875,-0.1640625,0.0,0.0,404 --0.046875,-0.1640625,0.0,0.0,425 --0.0390625,-0.1796875,0.0,0.0,444 --0.0390625,-0.2109375,0.0,0.0,481 --0.0390625,-0.2109375,0.0,0.0,495 --0.0390625,-0.2265625,0.0,0.0,512 --0.0390625,-0.234375,0.0,0.0,524 --0.03125,-0.25,0.0,0.0,544 --0.03125,-0.2578125,0.0,0.0,564 --0.0234375,-0.2890625,0.0,0.0,584 --0.0234375,-0.3046875,0.0,0.0,604 --0.015625,-0.3125,0.0,0.0,624 --0.015625,-0.3125,0.0,0.0,644 --0.015625,-0.3125,0.0,0.0,665 --0.015625,-0.3125,0.0,0.0,684 --0.0078125,-0.3125,0.0,0.0,727 --0.015625,-0.3125,0.0,0.0,744 --0.015625,-0.3125,0.0,0.0,757 --0.015625,-0.3125,0.0,0.0,775 --0.0078125,-0.3125,0.0,0.0,795 --0.015625,-0.3125,0.0,0.0,809 --0.0234375,-0.3125,0.0,0.0,824 --0.015625,-0.3125,0.0,0.0,844 --0.0078125,-0.3125,0.0,0.0,864 --0.0078125,-0.3046875,0.0,0.0,885 --0.0078125,-0.3203125,0.0,0.0,904 --0.0078125,-0.3359375,0.0,0.0,924 --0.0078125,-0.3515625,0.0,0.0,1013 --0.0078125,-0.3515625,0.0,0.0,1028 --0.0078125,-0.3515625,0.0,0.0,1040 --0.0078125,-0.3515625,0.0,0.0,1056 --0.0078125,-0.359375,0.0,0.0,1074 --0.0078125,-0.3671875,0.0,0.0,1087 --0.0078125,-0.3671875,0.0,0.0,1098 --0.0078125,-0.375,0.0,0.0,1110 --0.0078125,-0.375,0.0,0.0,1121 --0.0078125,-0.375,0.0,0.0,1133 --0.0078125,-0.375,0.0,0.0,1146 --0.0078125,-0.390625,0.0,0.0,1164 --0.0078125,-0.3984375,0.0,0.0,1203 --0.0078125,-0.3984375,0.0,0.0,1216 --0.0078125,-0.40625,0.0,0.0,1228 --0.0078125,-0.421875,0.0,0.0,1245 --0.0078125,-0.421875,0.0,0.0,1264 --0.0078125,-0.421875,0.0,0.0,1284 --0.0078125,-0.421875,0.0,0.0,1305 --0.0078125,-0.421875,0.0,0.0,1324 --0.0078125,-0.421875,0.0,0.0,1345 --0.0078125,-0.421875,0.0,0.0,1364 --0.0078125,-0.421875,0.0,0.0,1384 --0.0078125,-0.421875,0.0,0.0,1404 --0.0078125,-0.421875,0.0,0.0,1441 --0.0078125,-0.421875,0.0,0.0,1456 --0.0078125,-0.421875,0.0,0.0,1468 --0.0078125,-0.421875,0.0,0.0,1484 --0.0078125,-0.421875,0.0,0.0,1505 --0.0078125,-0.421875,0.0,0.0,1525 --0.0078125,-0.421875,0.0,0.0,1546 --0.0078125,-0.421875,0.0,0.0,1564 --0.015625,-0.421875,0.0,0.0,1584 --0.015625,-0.421875,0.0,0.0,1604 --0.015625,-0.421875,0.0,0.0,1624 --0.015625,-0.421875,0.0,0.0,1644 --0.015625,-0.421875,0.0,0.0,1685 --0.015625,-0.421875,0.0,0.0,1700 --0.015625,-0.421875,0.0,0.0,1716 --0.015625,-0.421875,0.0,0.0,1730 --0.015625,-0.421875,0.0,0.0,1744 --0.015625,-0.421875,0.0,0.0,1764 --0.015625,-0.421875,0.0,0.0,1785 --0.015625,-0.421875,0.0,0.0,1805 --0.015625,-0.421875,0.0,0.0,1825 --0.015625,-0.4375,0.0,0.0,1845 --0.0234375,-0.4375,0.0,0.0,1864 --0.0234375,-0.4375,0.0,0.0,1885 --0.0234375,-0.4375,0.0,0.0,1925 --0.0234375,-0.4375,0.0,0.0,1940 --0.0234375,-0.4375,0.0,0.0,1955 --0.0234375,-0.4375,0.0,0.0,1967 --0.0234375,-0.4375,0.0,0.0,1986 --0.0234375,-0.4375,0.0,0.0,2006 --0.0234375,-0.4375,0.0,0.0,2024 --0.0234375,-0.4375,0.0,0.0,2044 --0.0234375,-0.4375,0.0,0.0,2064 --0.0234375,-0.4375,0.0,0.0,2085 --0.0234375,-0.4375,0.0,0.0,2105 --0.0234375,-0.4375,0.0,0.0,2125 --0.0234375,-0.4375,0.0,0.0,2174 --0.0234375,-0.4375,0.0,0.0,2189 --0.0234375,-0.4375,0.0,0.0,2202 --0.0234375,-0.4375,0.0,0.0,2218 --0.0234375,-0.4375,0.0,0.0,2233 --0.0234375,-0.4375,0.0,0.0,2250 --0.0234375,-0.4375,0.0,0.0,2264 --0.015625,-0.4375,0.0,0.0,2284 --0.015625,-0.4375,0.0,0.0,2304 --0.015625,-0.4375,0.0,0.0,2324 --0.015625,-0.4375,0.0,0.0,2344 --0.015625,-0.4375,0.0,0.0,2364 --0.015625,-0.4375,0.0,0.0,2407 --0.015625,-0.4375,0.0,0.0,2423 --0.015625,-0.4375,0.03125,-0.1015625,2440 --0.015625,-0.4375,0.03125,-0.1015625,2451 --0.015625,-0.4296875,0.0703125,-0.203125,2466 --0.015625,-0.4296875,0.1171875,-0.28125,2484 --0.015625,-0.4296875,0.1484375,-0.359375,2505 --0.015625,-0.4296875,0.1875,-0.453125,2524 --0.015625,-0.4296875,0.1875,-0.453125,2544 --0.015625,-0.4296875,0.21875,-0.5625,2564 --0.015625,-0.4296875,0.2421875,-0.6015625,2584 --0.015625,-0.4296875,0.28125,-0.7109375,2606 --0.015625,-0.4296875,0.2890625,-0.84375,2644 --0.015625,-0.4296875,0.28089875327071334,-0.9597374070082707,2661 --0.015625,-0.4296875,0.28089875327071334,-0.9597374070082707,2672 --0.015625,-0.4296875,0.2707455769182841,-0.962650940153899,2689 --0.015625,-0.4296875,0.2707455769182841,-0.962650940153899,2704 --0.015625,-0.4296875,0.19905992429010463,-0.9799873195820535,2724 --0.015625,-0.4296875,0.18428853505018536,-0.9828721869343219,2745 --0.015625,-0.4296875,0.16939121559933262,-0.9855488907597534,2764 --0.015625,-0.4296875,0.1543768802736096,-0.9880120337511015,2784 --0.015625,-0.4296875,0.07788766729290965,-0.9969621413492434,2804 --0.015625,-0.4296875,0.03903273174035336,-0.999237932553046,2825 --0.015625,-0.4296875,0.023431065349237362,-0.9997254549007941,2845 --0.015625,-0.4296875,-0.0,-1.0,2885 --0.015625,-0.4296875,-0.0,-1.0,2900 --0.015625,-0.4296875,-0.0,-1.0,2913 --0.015625,-0.4296875,-0.0,-1.0,2930 --0.015625,-0.4296875,-0.0,-1.0,2944 --0.015625,-0.4296875,-0.08629110282549445,-0.9962699662105448,2965 --0.015625,-0.4296875,-0.10957246682398805,-0.9939788098918941,2984 --0.015625,-0.4296875,-0.14795964550591104,-0.9889933990182973,3004 --0.015625,-0.4296875,-0.20795087166990847,-0.9781392717664111,3025 --0.015625,-0.4296875,-0.22261635546060413,-0.974906127933063,3045 --0.015625,-0.4296875,-0.24433176541606075,-0.9696916976073742,3064 --0.015625,-0.4296875,-0.2586093438103028,-0.9659819911851382,3084 --0.015625,-0.4296875,-0.3072217021263233,-0.9516379698932809,3117 --0.015625,-0.4296875,-0.3535437990815464,-0.9354179718879616,3129 --0.015625,-0.4296875,-0.3535437990815464,-0.9354179718879616,3144 --0.015625,-0.421875,-0.43296180587754685,-0.9014122667521522,3164 --0.015625,-0.4140625,-0.46113272661989396,-0.8873311717955715,3184 --0.015625,-0.4140625,-0.46113272661989396,-0.8873311717955715,3204 --0.015625,-0.40625,-0.5034479718548448,-0.8640255433928116,3224 --0.015625,-0.40625,-0.5034479718548448,-0.8640255433928116,3245 --0.015625,-0.40625,-0.5424281727058204,-0.8401021827462566,3264 --0.015625,-0.40625,-0.5470717552428331,-0.8370857152141146,3284 --0.015625,-0.40625,-0.5695464964539495,-0.8219591160009305,3304 --0.015625,-0.40625,-0.594924795762871,-0.8037813679020596,3325 --0.015625,-0.40625,-0.6851738491603396,-0.7283795689246123,3417 --0.015625,-0.4140625,-0.7305249012857322,-0.6828860582860593,3430 --0.015625,-0.4140625,-0.7363002262116671,-0.6766549910261859,3442 --0.015625,-0.4140625,-0.7363002262116671,-0.6766549910261859,3453 --0.015625,-0.421875,-0.7392885165971433,-0.673388809847324,3468 --0.0234375,-0.421875,-0.7731493128109427,-0.6342240456652264,3482 --0.0234375,-0.421875,-0.7731493128109427,-0.6342240456652264,3499 --0.0234375,-0.421875,-0.7820595514434193,-0.6232037050564748,3513 --0.0234375,-0.421875,-0.7940055545690287,-0.6079105027169126,3529 --0.0234375,-0.421875,-0.7940055545690287,-0.6079105027169126,3541 --0.0234375,-0.421875,-0.7940055545690287,-0.6079105027169126,3552 --0.0234375,-0.421875,-0.8030011791882969,-0.5959774376788141,3565 --0.0234375,-0.421875,-0.8210359184762267,-0.5708765370655013,3598 --0.0234375,-0.421875,-0.8300495997825932,-0.5576895748539298,3614 --0.0234375,-0.421875,-0.8360479108370626,-0.5486564414868224,3626 --0.0078125,-0.421875,-0.8420323756982735,-0.5394269906817064,3645 --0.0078125,-0.421875,-0.8450179582407706,-0.5347379266992377,3664 --0.0078125,-0.421875,-0.8509727940026032,-0.5252097712984816,3685 -0.0,-0.421875,-0.862799360311186,-0.5055465001823355,3705 -0.0,-0.421875,-0.8744793416769986,-0.48506275983646013,3724 -0.0,-0.421875,-0.8888031674084939,-0.45828913319500464,3744 -0.0,-0.421875,-0.8888031674084939,-0.45828913319500464,3764 -0.0,-0.421875,-0.8972134044289655,-0.4415972224923814,3784 -0.0,-0.421875,-0.8999813238235361,-0.4359284537270253,3804 -0.0,-0.421875,-0.9027301154156147,-0.43020732062775385,3846 -0.0,-0.421875,-0.9135170055402336,-0.4068005415296353,3860 -0.0,-0.421875,-0.9187733502125238,-0.3947854239194439,3875 -0.0,-0.4140625,-0.9264665771223092,-0.3763770469559381,3891 -0.0,-0.4140625,-0.9264665771223092,-0.3763770469559381,3905 -0.0,-0.40625,-0.9363291775690445,-0.3511234415883917,3925 -0.0,-0.40625,-0.9363291775690445,-0.3511234415883917,3944 -0.007874015718698502,-0.3984375,-0.9586468662780967,-0.28459828842630996,3964 -0.023622047156095505,-0.390625,-0.9645897061200785,-0.263754997767209,3984 -0.06299212574958801,-0.3828125,-0.9828721869343219,-0.18428853505018536,4005 -0.07086614519357681,-0.3828125,-0.9902565788380345,-0.1392548313990986,4025 -0.07086614519357681,-0.3828125,-0.9940716917543756,-0.10872659128563483,4044 -0.07874015718698502,-0.3828125,-0.9995120760870788,-0.031234752377721213,4082 -0.07874015718698502,-0.3828125,-0.9995120760870788,-0.031234752377721213,4094 -0.07874015718698502,-0.3828125,-0.9995120760870788,-0.031234752377721213,4110 -0.07874015718698502,-0.3828125,-0.9997254549007941,-0.023431065349237362,4126 -0.07874015718698502,-0.3828125,-0.9997254549007941,-0.023431065349237362,4144 -0.07874015718698502,-0.3828125,-0.9997254549007941,-0.023431065349237362,4164 -0.07874015718698502,-0.3828125,-0.9997254549007941,-0.023431065349237362,4184 -0.07874015718698502,-0.3828125,-0.9997254549007941,-0.023431065349237362,4204 -0.07874015718698502,-0.3828125,-0.9997254549007941,-0.023431065349237362,4224 -0.07874015718698502,-0.3828125,-0.9999694838187878,-0.00781226159233428,4253 -0.07874015718698502,-0.3828125,-1.0,0.0,4265 -0.07874015718698502,-0.3828125,-1.0,0.0,4285 -0.08661417663097382,-0.3828125,-1.0,0.0,4324 -0.08661417663097382,-0.3828125,-1.0,0.0,4337 -0.08661417663097382,-0.3828125,-1.0,0.0,4354 -0.08661417663097382,-0.3828125,-1.0,0.0,4367 -0.09448818862438202,-0.3828125,-0.9997211161517748,0.02361545934868166,4386 -0.09448818862438202,-0.3828125,-0.999504367732367,0.03148045240972986,4404 -0.09448818862438202,-0.3828125,-0.9988858624996851,0.04719145789504938,4424 -0.10236220806837082,-0.3828125,-0.9980218809979032,0.06286751982866029,4445 -0.11811023950576782,-0.3828125,-0.994801803805476,0.10183010922792674,4465 -0.12598425149917603,-0.3828125,-0.9901048130433447,0.14032982287597778,4484 -0.12598425149917603,-0.3828125,-0.9796804960291332,0.20056451755011773,4504 -0.13385826349258423,-0.375,-0.9678596169756838,0.2514910770339234,4525 -0.15748031437397003,-0.359375,-0.9494282307963439,0.31398413107500234,4605 -0.16535432636737823,-0.34375,-0.9378378069653893,0.34707383627457283,4621 -0.16535432636737823,-0.34375,-0.9378378069653893,0.34707383627457283,4634 -0.16535432636737823,-0.34375,-0.9378378069653893,0.34707383627457283,4649 -0.18110236525535583,-0.34375,-0.9354179718879616,0.3535437990815464,4661 -0.20472441613674164,-0.3359375,-0.9279719185115246,0.3726501287455037,4680 -0.20472441613674164,-0.3359375,-0.9279719185115246,0.3726501287455037,4694 -0.23622047901153564,-0.3203125,-0.912324307310268,0.40946838497109816,4710 -0.26771652698516846,-0.3203125,-0.9014122667521522,0.43296180587754685,4721 -0.28346458077430725,-0.3125,-0.893016444170718,0.4500240331812145,4740 -0.28346458077430725,-0.3125,-0.893016444170718,0.4500240331812145,4751 -0.30708661675453186,-0.3125,-0.8786876142186625,0.47739718957982463,4764 -0.32283464074134827,-0.3046875,-0.8728573979584765,0.48797537112968914,4800 -0.32283464074134827,-0.3046875,-0.8640255433928116,0.5034479718548448,4817 -0.32283464074134827,-0.3046875,-0.8580885160559446,0.5135018000094129,4828 -0.32283464074134827,-0.3046875,-0.8521187304508965,0.5233485160146654,4844 -0.32283464074134827,-0.28125,-0.8461215648085673,0.5329899600985946,4864 -0.32283464074134827,-0.28125,-0.8401021827462566,0.5424281727058204,4884 -0.32283464074134827,-0.2734375,-0.8340655321723693,0.5516653768744438,4905 -0.32283464074134827,-0.2734375,-0.8249884705123031,0.5651495585433743,4924 -0.32283464074134827,-0.265625,-0.8158981942547746,0.5781955868145294,4944 -0.32283464074134827,-0.2578125,-0.8128676272595764,0.5824484702987778,4964 -0.32283464074134827,-0.2578125,-0.8068087413186813,0.5908127071515688,4984 -0.32283464074134827,-0.25,-0.781483297246057,0.6239261623985893,5004 -0.32283464074134827,-0.2421875,-0.7697579751547384,0.6383358517940827,5038 -0.3385826647281647,-0.234375,-0.7614733174322016,0.6481962564214621,5050 -0.34645670652389526,-0.21875,-0.7259526750390194,0.6877446572701906,5064 -0.35433071851730347,-0.21875,-0.6974437947486879,0.7166394861899184,5084 -0.36220473051071167,-0.21875,-0.6675450352577801,0.7445694231585723,5104 -0.3700787425041199,-0.2109375,-0.6427352020960505,0.7660884152541071,5124 -0.3700787425041199,-0.203125,-0.60209217677236,0.7984265843955356,5145 -0.3700787425041199,-0.203125,-0.5681906995469479,0.8228969127104258,5165 -0.3779527544975281,-0.203125,-0.5516653768744438,0.8340655321723693,5184 -0.3779527544975281,-0.203125,-0.5470717552428331,0.8370857152141146,5204 -0.3700787425041199,-0.203125,-0.5329899600985946,0.8461215648085673,5224 -0.3779527544975281,-0.203125,-0.5233485160146654,0.8521187304508965,5244 -0.3779527544975281,-0.203125,-0.5135018000094129,0.8580885160559446,5278 -0.3779527544975281,-0.203125,-0.5135018000094129,0.8580885160559446,5294 -0.4015747904777527,-0.171875,-0.5085008816513332,0.8610614689787348,5305 -0.4015747904777527,-0.1640625,-0.5085008816513332,0.8610614689787348,5324 -0.4015747904777527,-0.1640625,-0.5085008816513332,0.8610614689787348,5344 -0.4015747904777527,-0.15625,-0.5034479718548448,0.8640255433928116,5365 -0.4015747904777527,-0.15625,-0.49318540967799457,0.8699242218036859,5384 -0.4015747904777527,-0.15625,-0.49318540967799457,0.8699242218036859,5404 -0.4015747904777527,-0.15625,-0.49318540967799457,0.8699242218036859,5424 -0.4015747904777527,-0.15625,-0.49318540967799457,0.8699242218036859,5445 -0.4094488322734833,-0.15625,-0.49318540967799457,0.8699242218036859,5464 -0.4094488322734833,-0.15625,-0.49318540967799457,0.8699242218036859,5484 -0.4251968562602997,-0.15625,-0.472028758555712,0.8815831504154066,5518 -0.4251968562602997,-0.15625,-0.472028758555712,0.8815831504154066,5534 -0.4330708682537079,-0.15625,-0.472028758555712,0.8815831504154066,5551 -0.4409448802471161,-0.15625,-0.466607297434484,0.8844646007505824,5565 -0.4409448802471161,-0.15625,-0.46113272661989396,0.8873311717955715,5584 -0.4409448802471161,-0.15625,-0.46113272661989396,0.8873311717955715,5605 -0.4409448802471161,-0.15625,-0.46113272661989396,0.8873311717955715,5624 -0.4409448802471161,-0.15625,-0.466607297434484,0.8844646007505824,5644 -0.4488188922405243,-0.15625,-0.45560498552550965,0.8901820584376546,5665 -0.4409448802471161,-0.15625,-0.4500240331812145,0.893016444170718,5685 -0.4409448802471161,-0.15625,-0.4500240331812145,0.893016444170718,5704 -0.4409448802471161,-0.15625,-0.4500240331812145,0.893016444170718,5725 -0.4409448802471161,-0.15625,-0.43296180587754685,0.9014122667521522,5798 -0.4409448802471161,-0.15625,-0.42716801226970363,0.9041722674875349,5815 -0.4803149700164795,-0.15625,-0.415421189526273,0.9096291746050015,5829 -0.4960629940032959,-0.1484375,-0.40946838497109816,0.912324307310268,5844 -0.4960629940032959,-0.1484375,-0.40946838497109816,0.912324307310268,5858 -0.5039370059967041,-0.1328125,-0.3912940791117502,0.920265691880387,5871 -0.5039370059967041,-0.1328125,-0.3535437990815464,0.9354179718879616,5884 -0.5039370059967041,-0.1328125,-0.3535437990815464,0.9354179718879616,5896 -0.5118110179901123,-0.125,-0.3206991089217746,0.9471811239339495,5909 -0.5196850299835205,-0.1171875,-0.3072217021263233,0.9516379698932809,5926 -0.5118110179901123,-0.09375,-0.27271945919792484,0.9620936007347682,5944 -0.5118110179901123,-0.0859375,-0.2586093438103028,0.9659819911851382,5965 -0.5196850299835205,-0.046875,-0.22261635546060413,0.974906127933063,6005 -0.5196850299835205,-0.046875,-0.22261635546060413,0.974906127933063,6018 -0.5196850299835205,-0.0078125,-0.21530182214487537,0.9765475540807507,6031 -0.5275590419769287,0.0,-0.18568977934940398,0.9826084193844309,6044 -0.5275590419769287,0.0,-0.17820357578581228,0.9839936410247528,6065 -0.5275590419769287,0.0,-0.1631390877346237,0.9866030802978037,6084 -0.5275590419769287,0.0,-0.14032982287597778,0.9901048130433447,6104 -0.5275590419769287,0.0,-0.09406919601194683,0.995565661501875,6125 -0.5275590419769287,0.0,-0.055034575740075024,0.998484449289577,6144 -0.5275590419769287,0.015748031437397003,-0.02361545934868166,0.9997211161517748,6164 -0.5275590419769287,0.031496062874794006,-0.0,1.0,6184 -0.5275590419769287,0.05511811003088951,-0.0,1.0,6204 -0.5275590419769287,0.07086614519357681,-0.0,1.0,6233 -0.5275590419769287,0.08661417663097382,-0.0,1.0,6247 -0.5275590419769287,0.11811023950576782,0.023431065349237362,0.9997254549007941,6265 -0.5275590419769287,0.13385826349258423,0.05460590540193252,0.9985079844924803,6285 -0.5275590419769287,0.13385826349258423,0.07013933466922019,0.997537204184465,6304 -0.5275590419769287,0.14173229038715363,0.12403473458920847,0.9922778767136677,6324 -0.5275590419769287,0.14173229038715363,0.1543768802736096,0.9880120337511015,6344 -0.5196850299835205,0.14173229038715363,0.18428853505018536,0.9828721869343219,6365 -0.4881889820098877,0.14960630238056183,0.19169051231966405,0.98145542307668,6384 -0.4645669162273407,0.14960630238056183,0.24253562503633297,0.9701425001453319,6405 -0.4566929042339325,0.14960630238056183,0.28459828842630996,0.9586468662780967,6425 -0.4488188922405243,0.15748031437397003,0.33819872616315155,0.941074716280074,6444 -0.4409448802471161,0.15748031437397003,0.3763770469559381,0.9264665771223092,6479 -0.4409448802471161,0.15748031437397003,0.3763770469559381,0.9264665771223092,6493 -0.4251968562602997,0.15748031437397003,0.4359284537270253,0.8999813238235361,6509 -0.4173228442668915,0.15748031437397003,0.47450986523092026,0.8802501847761999,6527 -0.4094488322734833,0.15748031437397003,0.49026123963255896,0.8715755371245493,6544 -0.4015747904777527,0.16535432636737823,0.5252097712984816,0.8509727940026032,6564 -0.3937007784843445,0.18110236525535583,0.5394269906817064,0.8420323756982735,6585 -0.3937007784843445,0.18110236525535583,0.5531973991682577,0.833050201100435,6604 -0.3700787425041199,0.18110236525535583,0.5576895748539298,0.8300495997825932,6624 -0.3700787425041199,0.18110236525535583,0.5576895748539298,0.8300495997825932,6645 -0.33070865273475647,0.18110236525535583,0.6079105027169126,0.7940055545690287,6664 -0.32283464074134827,0.18110236525535583,0.6117992010161086,0.7910131083844636,6684 -0.29921260476112366,0.18110236525535583,0.6232037050564748,0.7820595514434193,6705 -0.27559053897857666,0.18897637724876404,0.6305926250944657,0.7761140001162655,6725 -0.27559053897857666,0.18897637724876404,0.6342240456652264,0.7731493128109427,6744 -0.27559053897857666,0.18897637724876404,0.6378139711853661,0.7701904557710081,6764 -0.28346458077430725,0.18897637724876404,0.6483387853676998,0.7613519681382164,6784 -0.28346458077430725,0.18897637724876404,0.6610770596337712,0.7503180133956318,6805 -0.28346458077430725,0.18897637724876404,0.6610770596337712,0.7503180133956318,6825 -0.27559053897857666,0.18897637724876404,0.673388809847324,0.7392885165971433,6845 -0.28346458077430725,0.18897637724876404,0.6828860582860593,0.7305249012857322,6864 -0.28346458077430725,0.18897637724876404,0.6828860582860593,0.7305249012857322,6884 -0.28346458077430725,0.18897637724876404,0.6890717737753316,0.7246931009648968,6905 -0.28346458077430725,0.18897637724876404,0.6951319975197523,0.7188821224819818,6924 -0.27559053897857666,0.18897637724876404,0.7283795689246123,0.6851738491603396,7006 -0.27559053897857666,0.18897637724876404,0.7283795689246123,0.6851738491603396,7019 -0.27559053897857666,0.18897637724876404,0.7373074742730845,0.6755573168732946,7035 -0.27559053897857666,0.18897637724876404,0.7519173304971933,0.6592574065552654,7052 -0.27559053897857666,0.18897637724876404,0.7601819876238485,0.6497102013145976,7066 -0.27559053897857666,0.20472441613674164,0.7657444477625389,0.6431449608920561,7083 -0.27559053897857666,0.22047244012355804,0.7742502396090526,0.6328795829107815,7100 -0.27559053897857666,0.22047244012355804,0.7742502396090526,0.6328795829107815,7113 -0.27559053897857666,0.22834645211696625,0.7856739074383975,0.6186408579866678,7126 -0.27559053897857666,0.22834645211696625,0.7947129335869222,0.6069854637383567,7141 -0.27559053897857666,0.22834645211696625,0.7947129335869222,0.6069854637383567,7154 -0.27559053897857666,0.22834645211696625,0.8158981942547746,0.5781955868145294,7170 -0.26771652698516846,0.22834645211696625,0.8219591160009305,0.5695464964539495,7208 -0.26771652698516846,0.23622047901153564,0.8219591160009305,0.5695464964539495,7223 -0.26771652698516846,0.26771652698516846,0.8401021827462566,0.5424281727058204,7240 -0.26771652698516846,0.26771652698516846,0.8401021827462566,0.5424281727058204,7253 -0.25196850299835205,0.27559053897857666,0.8461215648085673,0.5329899600985946,7266 -0.25196850299835205,0.28346458077430725,0.8580885160559446,0.5135018000094129,7287 -0.25196850299835205,0.28346458077430725,0.8728573979584765,0.48797537112968914,7304 -0.22834645211696625,0.28346458077430725,0.8986323918355212,0.4387024325712936,7325 -0.22834645211696625,0.28346458077430725,0.9041722674875349,0.42716801226970363,7344 -0.21259842813014984,0.28346458077430725,0.9096291746050015,0.415421189526273,7364 -0.21259842813014984,0.28346458077430725,0.920265691880387,0.3912940791117502,7384 -0.21259842813014984,0.28346458077430725,0.9254308345456299,0.3789165745545833,7405 -0.21259842813014984,0.29133859276771545,0.932966418843752,0.35996341662711334,7439 -0.21259842813014984,0.29133859276771545,0.932966418843752,0.35996341662711334,7452 -0.22047244012355804,0.29133859276771545,0.9494282307963439,0.31398413107500234,7465 -0.22047244012355804,0.29133859276771545,0.9538094063872763,0.300412410341436,7509 -0.22047244012355804,0.29133859276771545,0.9640596934790533,0.2656857305334137,7533 -0.22047244012355804,0.29133859276771545,0.9678596169756838,0.2514910770339234,7550 -0.22047244012355804,0.29133859276771545,0.9781392717664111,0.20795087166990847,7563 -0.22047244012355804,0.29133859276771545,0.9781392717664111,0.20795087166990847,7579 -0.22047244012355804,0.29133859276771545,0.9811704629340057,0.19314378754148134,7592 -0.22047244012355804,0.29133859276771545,0.9853254218311062,0.17068630025093642,7606 -0.22047244012355804,0.29133859276771545,0.992157222284201,0.12499618501897668,7624 -0.22047244012355804,0.29133859276771545,0.9939788098918941,0.10957246682398805,7644 -0.22047244012355804,0.29133859276771545,0.995565661501875,0.09406919601194683,7679 -0.22047244012355804,0.29133859276771545,0.9969143348043864,0.07849719142445599,7693 -0.22047244012355804,0.29133859276771545,0.998484449289577,0.055034575740075024,7710 -0.22047244012355804,0.29133859276771545,0.999504367732367,0.03148045240972986,7726 -0.22047244012355804,0.29133859276771545,0.9998760228122497,0.01574607904074679,7744 -0.22047244012355804,0.29133859276771545,1.0,0.0,7764 -0.22047244012355804,0.29133859276771545,1.0,0.0,7784 -0.22047244012355804,0.29133859276771545,1.0,0.0,7804 -0.22047244012355804,0.29133859276771545,1.0,0.0,7825 -0.22047244012355804,0.29133859276771545,1.0,0.0,7844 -0.22047244012355804,0.29133859276771545,1.0,0.0,7864 -0.22047244012355804,0.29133859276771545,1.0,0.0,7884 -0.22047244012355804,0.29133859276771545,1.0,0.0,7923 -0.22047244012355804,0.29133859276771545,1.0,0.0,7938 -0.22047244012355804,0.29133859276771545,0.9997254549007941,-0.023431065349237362,7950 -0.22047244012355804,0.29133859276771545,0.9985079844924803,-0.05460590540193252,7967 -0.22047244012355804,0.29133859276771545,0.997537204184465,-0.07013933466922019,7984 -0.22047244012355804,0.29133859276771545,0.9963277012186765,-0.08562191182348002,8004 -0.22047244012355804,0.29133859276771545,0.9956342260592882,-0.09334070869305827,8024 -0.22047244012355804,0.29133859276771545,0.9956342260592882,-0.09334070869305827,8044 -0.22047244012355804,0.30708661675453186,0.9855488907597534,-0.16939121559933262,8064 -0.22047244012355804,0.30708661675453186,0.9855488907597534,-0.16939121559933262,8086 -0.22047244012355804,0.31496062874794006,0.9799873195820535,-0.19905992429010463,8105 -0.22047244012355804,0.33070865273475647,0.9784686026666812,-0.20639572087500307,8124 -0.22047244012355804,0.35433071851730347,0.9701425001453319,-0.24253562503633297,8186 -0.22047244012355804,0.36220473051071167,0.9664851269264961,-0.2567226118398505,8200 -0.22047244012355804,0.36220473051071167,0.9645897061200785,-0.263754997767209,8213 -0.22047244012355804,0.3937007784843445,0.9565833270275276,-0.29145898245369983,8228 -0.22047244012355804,0.4015747904777527,0.9479401182738548,-0.3184486334826231,8243 -0.22047244012355804,0.4015747904777527,0.9479401182738548,-0.3184486334826231,8257 -0.20472441613674164,0.4173228442668915,0.9456865993048666,-0.3250797685110479,8271 -0.20472441613674164,0.4173228442668915,0.9456865993048666,-0.3250797685110479,8285 -0.19685038924217224,0.4566929042339325,0.9387181932794337,-0.3446855865947921,8305 -0.19685038924217224,0.4566929042339325,0.9387181932794337,-0.3446855865947921,8325 -0.18897637724876404,0.4645669162273407,0.9339085898481578,-0.35751188205124795,8344 -0.18110236525535583,0.4724409580230713,0.9289763737447976,-0.37013902391394277,8364 -0.15748031437397003,0.4881889820098877,0.8972134044289655,-0.4415972224923814,8400 -0.15748031437397003,0.4881889820098877,0.8972134044289655,-0.4415972224923814,8416 -0.14960630238056183,0.4960629940032959,0.8888031674084939,-0.45828913319500464,8426 -0.14960630238056183,0.4960629940032959,0.8831157194574106,-0.4691552259617494,8444 -0.15748031437397003,0.5039370059967041,0.8802501847761999,-0.47450986523092026,8464 -0.15748031437397003,0.5039370059967041,0.8773711395523853,-0.4798123419427107,8486 -0.14960630238056183,0.5039370059967041,0.8744793416769986,-0.48506275983646013,8505 -0.11023622006177902,0.5039370059967041,0.8657348311141284,-0.5005029492378555,8524 -0.09448818862438202,0.5039370059967041,0.8598547438407345,-0.5105387541554361,8544 -0.07874015718698502,0.5039370059967041,0.8569016654805386,-0.5154799081406365,8564 -0.07874015718698502,0.5039370059967041,0.8539407961853737,-0.520370172675462,8584 -0.07874015718698502,0.5039370059967041,0.8509727940026032,-0.5252097712984816,8604 -0.07086614519357681,0.5039370059967041,0.8450179582407706,-0.5347379266992377,8625 -0.06299212574958801,0.5039370059967041,0.8360479108370626,-0.5486564414868224,8661 -0.06299212574958801,0.5039370059967041,0.8360479108370626,-0.5486564414868224,8673 -0.06299212574958801,0.5039370059967041,0.8240419241993676,-0.5665288228870652,8689 -0.05511811003088951,0.5039370059967041,0.8240419241993676,-0.5665288228870652,8704 -0.05511811003088951,0.5039370059967041,0.8210359184762267,-0.5708765370655013,8724 -0.031496062874794006,0.5039370059967041,0.8090093109439124,-0.5877958274826863,8745 -0.03937007859349251,0.5039370059967041,0.8030011791882969,-0.5959774376788141,8764 -0.031496062874794006,0.5039370059967041,0.8,-0.6,8784 -0.023622047156095505,0.5039370059967041,0.7970013198156258,-0.6039775626727789,8805 -0.031496062874794006,0.5039370059967041,0.7880243737245634,-0.6156440419723151,8824 -0.031496062874794006,0.5039370059967041,0.7820595514434193,-0.6232037050564748,8844 -0.031496062874794006,0.5039370059967041,0.7707025822937636,-0.6371950483531117,8864 -0.023622047156095505,0.5039370059967041,0.7421026427511382,-0.6702862579687701,8897 -0.023622047156095505,0.5039370059967041,0.7421026427511382,-0.6702862579687701,8909 -0.023622047156095505,0.5039370059967041,0.7393958180127342,-0.6732709887595629,8928 -0.015748031437397003,0.5039370059967041,0.7364081333395653,-0.6765375533932593,8945 -0.0,0.5039370059967041,0.7278467063026898,-0.6857398720537738,8964 -0.0,0.5039370059967041,0.7219882051154242,-0.6919053632356148,8985 -0.0,0.5039370059967041,0.7219882051154242,-0.6919053632356148,9005 -0.0,0.5039370059967041,0.7071067811865476,-0.7071067811865476,9024 -0.0,0.5039370059967041,0.6981759636203039,-0.7159261999835319,9046 -0.0,0.5039370059967041,0.6952251158274647,-0.7187920689063618,9064 -0.0,0.5039370059967041,0.6922875402198978,-0.7216217580258257,9085 -0.0,0.5039370059967041,0.6862789815591375,-0.7273384078062654,9122 -0.0,0.5039370059967041,0.6862789815591375,-0.7273384078062654,9133 -0.0,0.5039370059967041,0.6862789815591375,-0.7273384078062654,9146 -0.0,0.5039370059967041,0.6862789815591375,-0.7273384078062654,9165 -0.0,0.5039370059967041,0.6833580581622772,-0.7300833954725184,9184 -0.0,0.5039370059967041,0.6741303561283304,-0.7386123901927794,9204 -0.0,0.5039370059967041,0.6741303561283304,-0.7386123901927794,9224 -0.0,0.5039370059967041,0.6741303561283304,-0.7386123901927794,9244 -0.0,0.5039370059967041,0.6741303561283304,-0.7386123901927794,9265 -0.0,0.5039370059967041,0.6741303561283304,-0.7386123901927794,9285 -0.0,0.5039370059967041,0.6741303561283304,-0.7386123901927794,9305 -0.0,0.5039370059967041,0.6741303561283304,-0.7386123901927794,9325 -0.0,0.5039370059967041,0.6618153215021395,-0.749666912851981,9345 -0.0,0.5039370059967041,0.6618153215021395,-0.749666912851981,9388 -0.0,0.5039370059967041,0.6618153215021395,-0.749666912851981,9401 -0.0,0.5039370059967041,0.6618153215021395,-0.749666912851981,9414 -0.0,0.4960629940032959,0.658504607868518,-0.7525766947068778,9424 -0.0,0.4960629940032959,0.658504607868518,-0.7525766947068778,9445 -0.0,0.4960629940032959,0.6551552162877278,-0.7554943034669294,9465 -0.0,0.4960629940032959,0.6551552162877278,-0.7554943034669294,9484 -0.0,0.4960629940032959,0.6551552162877278,-0.7554943034669294,9504 -0.0,0.4960629940032959,0.6551552162877278,-0.7554943034669294,9524 -0.0,0.4960629940032959,0.6551552162877278,-0.7554943034669294,9544 -0.0,0.4960629940032959,0.6517667432879864,-0.7584194830987478,9564 --0.0234375,0.4173228442668915,0.6483387853676998,-0.7613519681382164,9630 --0.0234375,0.4173228442668915,0.6483387853676998,-0.7613519681382164,9643 --0.03125,0.4173228442668915,0.6483387853676998,-0.7613519681382164,9658 --0.03125,0.4173228442668915,0.6483387853676998,-0.7613519681382164,9670 --0.03125,0.4173228442668915,0.6483387853676998,-0.7613519681382164,9686 --0.03125,0.4173228442668915,0.6483387853676998,-0.7613519681382164,9703 --0.03125,0.4173228442668915,0.6483387853676998,-0.7613519681382164,9719 --0.03125,0.4173228442668915,0.6483387853676998,-0.7613519681382164,9733 --0.03125,0.4173228442668915,0.6483387853676998,-0.7613519681382164,9745 --0.03125,0.4173228442668915,0.6483387853676998,-0.7613519681382164,9764 --0.03125,0.4173228442668915,0.6483387853676998,-0.7613519681382164,9784 --0.03125,0.4173228442668915,0.6483387853676998,-0.7613519681382164,9806 --0.03125,0.4173228442668915,0.6483387853676998,-0.7613519681382164,9835 --0.03125,0.4094488322734833,0.6483387853676998,-0.7613519681382164,9850 --0.03125,0.4094488322734833,0.6483387853676998,-0.7613519681382164,9864 --0.03125,0.4094488322734833,0.6483387853676998,-0.7613519681382164,9884 --0.03125,0.4094488322734833,0.6483387853676998,-0.7613519681382164,9905 --0.03125,0.3937007784843445,0.6483387853676998,-0.7613519681382164,9924 --0.03125,0.3937007784843445,0.6483387853676998,-0.7613519681382164,9945 --0.03125,0.3937007784843445,0.6483387853676998,-0.7613519681382164,9964 --0.03125,0.4015747904777527,0.6483387853676998,-0.7613519681382164,9984 --0.03125,0.4015747904777527,0.6483387853676998,-0.7613519681382164,10004 --0.03125,0.3937007784843445,0.6483387853676998,-0.7613519681382164,10024 --0.03125,0.3937007784843445,0.6483387853676998,-0.7613519681382164,10044 --0.03125,0.3937007784843445,0.6483387853676998,-0.7613519681382164,10064 --0.03125,0.3937007784843445,0.6483387853676998,-0.7613519681382164,10094 --0.03125,0.3779527544975281,0.6483387853676998,-0.7613519681382164,10112 --0.03125,0.3779527544975281,0.6483387853676998,-0.7613519681382164,10125 --0.03125,0.3779527544975281,0.6483387853676998,-0.7613519681382164,10145 --0.03125,0.3779527544975281,0.6483387853676998,-0.7613519681382164,10164 --0.03125,0.3779527544975281,0.6483387853676998,-0.7613519681382164,10184 --0.03125,0.3779527544975281,0.6483387853676998,-0.7613519681382164,10204 --0.03125,0.3779527544975281,0.6483387853676998,-0.7613519681382164,10225 --0.03125,0.3858267664909363,0.6483387853676998,-0.7613519681382164,10244 --0.03125,0.3858267664909363,0.6483387853676998,-0.7613519681382164,10265 --0.03125,0.3858267664909363,0.6483387853676998,-0.7613519681382164,10285 --0.03125,0.3858267664909363,0.6448709392097829,-0.7642914835078909,10327 --0.03125,0.3858267664909363,0.6448709392097829,-0.7642914835078909,10339 --0.03125,0.3858267664909363,0.6448709392097829,-0.7642914835078909,10352 --0.03125,0.3779527544975281,0.6448709392097829,-0.7642914835078909,10369 --0.03125,0.3779527544975281,0.6448709392097829,-0.7642914835078909,10385 --0.03125,0.3779527544975281,0.6448709392097829,-0.7642914835078909,10404 --0.03125,0.3779527544975281,0.6448709392097829,-0.7642914835078909,10424 --0.03125,0.3700787425041199,0.6448709392097829,-0.7642914835078909,10445 --0.03125,0.36220473051071167,0.6378139711853661,-0.7701904557710081,10465 --0.03125,0.3385826647281647,0.6305926250944657,-0.7761140001162655,10484 --0.03125,0.31496062874794006,0.6305926250944657,-0.7761140001162655,10504 --0.03125,0.30708661675453186,0.6156440419723151,-0.7880243737245634,10524 --0.03125,0.31496062874794006,0.0234375,-0.109375,10580 --0.0234375,0.25196850299835205,0.0,0.0,10599 --0.0234375,0.25196850299835205,0.0,0.0,10614 --0.0234375,0.21259842813014984,0.0,0.0,10627 --0.0234375,0.10236220806837082,0.0,0.0,10644 --0.0234375,0.10236220806837082,0.0,0.0,10656 --0.0234375,0.10236220806837082,0.0,0.0,10667 -0.0,0.0,0.0,0.0,10684 -0.0,0.0,0.0,0.0,10704 -0.0,0.0,0.0,0.0,10724 -0.0,0.0,0.0,0.0,10744 -0.0,0.0,0.0,0.0,10764 -0.0,0.0,0.0,0.0,10843 -0.0,0.0,0.0,0.0,10855 -0.0,0.0,0.0,0.0,10873 -0.0,0.0,0.0,0.0,10885 -0.0,0.0,0.0,0.0,10896 -0.0,0.0,0.0,0.0,10909 -0.0,0.0,0.0,0.0,10922 -0.0,0.0,0.0,0.0,10935 -0.0,0.0,0.0,0.0,10952 -0.0,0.0,0.0,0.0,10965 -0.0,0.0,0.0,0.0,10984 -0.0,0.0,0.0,0.0,11004 -0.0,0.0,0.0,0.0,11035 -0.0,0.0,0.0,0.0,11049 -0.0,0.0,0.0,0.0,11064 -0.0,0.0,0.0,0.0,11084 -0.0,0.0,0.0,0.0,11104 -0.0,0.0,0.0,0.0,11125 -0.0,0.0,0.0,0.0,11145 -0.0,0.0,0.0,0.0,11165 -0.0,0.0,0.0,0.0,11185 -0.0,0.0,0.0,0.0,11204 -0.0,0.0,0.0,0.0,11225 -0.0,0.0,0.0,0.0,11244 -0.0,0.0,0.0,0.0,11284 -0.0,0.0,0.0,0.0,11296 -0.0,0.0,0.0,0.0,11309 -0.0,0.0,0.0,0.0,11325 -0.0,0.0,0.0,0.0,11344 -0.0,0.0,0.0,0.0,11364 -0.0,0.0,0.0,0.0,11384 -0.0,0.0,0.0,0.0,11405 -0.0,0.0,0.0,0.0,11424 -0.0,0.0,0.0,0.0,11444 -0.0,0.0,0.0,0.0,11464 -0.0,0.0,0.0,0.0,11484 -0.0,0.0,0.0,0.0,11526 -0.0,0.0,0.0,0.0,11540 -0.0,0.0,0.0,0.0,11552 -0.0,0.0,0.0,0.0,11566 -0.0,0.0,0.0,0.0,11584 -0.0,0.0,0.0,0.0,11604 -0.0,0.0,0.0,0.0,11625 -0.0,0.0,0.0,0.0,11644 -0.0,0.0,0.0,0.0,11664 -0.0,0.0,0.0,0.0,11684 -0.0,0.0,0.0,0.0,11705 -0.0,0.0,0.0,0.0,11725 -0.0,0.0,0.0,0.0,11789 -0.0,0.0,0.0,0.0,11802 -0.0,0.0,0.0,0.0,11814 -0.0,0.0,0.0,0.0,11829 -0.0,0.0,0.0,0.0,11842 -0.0,0.0,0.0,0.0,11863 -0.0,0.0,0.0,0.0,11879 -0.0,0.0,0.0,0.0,11892 -0.0,0.0,0.0,0.0,11907 \ No newline at end of file +0.0,0.0,0.0,0.0,3 +0.0,0.0,0.0,0.0,20 +0.0,0.0,0.0,0.0,40 +0.0,0.0,0.0,0.0,60 +0.0,0.0,0.0,0.0,80 +0.0,0.0,0.0,0.0,100 +0.0,0.0,0.0,0.0,120 +0.0,0.0,0.0,0.0,140 +0.0,0.0,0.0,0.0,160 +0.0,0.0,0.0,0.0,180 +0.0,0.0,0.0,0.0,200 +0.0,-0.1015625,0.0,0.0,220 +0.0,-0.125,0.0,0.0,240 +0.0,-0.1953125,0.0,0.0,260 +0.0,-0.203125,0.0,0.0,280 +0.0,-0.203125,0.0,0.0,300 +0.0,-0.203125,0.0,0.0,320 +0.0,-0.203125,0.0,0.0,340 +0.0,-0.203125,0.0,0.0,360 +0.0,-0.203125,0.0,0.0,380 +0.0,-0.203125,0.0,0.0,400 +0.0,-0.203125,0.0,0.0,420 +0.0,-0.203125,0.0,0.0,440 +0.0,-0.203125,0.0,0.0,461 +0.0,-0.21875,0.0,0.0,480 +0.0,-0.234375,0.0,0.0,500 +0.0,-0.2421875,0.0,0.0,521 +0.0,-0.2734375,0.0,0.0,540 +0.0,-0.3046875,0.0,0.0,560 +0.0,-0.34375,0.0,0.0,580 +0.0,-0.34375,0.0,0.0,600 +0.0,-0.359375,0.0,0.0,620 +0.0,-0.3671875,0.0,0.0,640 +0.0,-0.3671875,0.0,0.0,660 +0.0,-0.375,0.0,0.0,680 +0.0,-0.375,0.0,0.0,700 +0.0,-0.375,0.0,0.0,720 +0.0,-0.375,0.0,0.0,740 +0.0,-0.375,0.0,0.0,760 +0.0,-0.375,0.0,0.0,781 +0.0,-0.375,0.0,0.0,800 +0.0,-0.375,0.0,0.0,820 +0.0,-0.375,0.0,0.0,840 +0.0,-0.375,0.0,0.0,860 +0.0,-0.375,0.0,0.0,880 +0.0,-0.375,0.0,0.0,900 +0.0,-0.375,0.0,0.0,920 +0.0,-0.375,0.0,0.0,940 +0.0,-0.375,0.0,0.0,960 +0.0,-0.375,0.0,0.0,980 +0.0,-0.375,0.0,0.0,1000 +0.0,-0.3828125,0.0,0.0,1020 +0.0,-0.3828125,0.0,0.0,1040 +0.0,-0.3828125,0.0,0.0,1060 +0.0,-0.390625,0.0,0.0,1080 +0.0,-0.390625,0.0,0.0,1100 +0.0,-0.3984375,0.0,0.0,1120 +0.0,-0.40625,0.0,0.0,1141 +0.0,-0.40625,0.0,0.0,1160 +0.0,-0.40625,0.0,0.0,1180 +0.0,-0.40625,0.0,0.0,1200 +0.0,-0.40625,0.0,0.0,1220 +0.0,-0.40625,0.0,0.0,1245 +0.0,-0.40625,0.0,0.0,1260 +0.0,-0.40625,0.0,0.0,1280 +0.0,-0.40625,0.0,0.0,1300 +0.0,-0.40625,0.0,0.0,1320 +0.0,-0.40625,0.0,0.0,1340 +0.0,-0.40625,0.0,0.0,1360 +0.0,-0.4140625,0.0,0.0,1380 +0.0,-0.4140625,0.0,0.0,1400 +0.0,-0.4140625,0.0,0.0,1420 +0.0,-0.4140625,0.0,0.0,1440 +0.0,-0.421875,0.0,0.0,1460 +0.0,-0.4296875,0.0,0.0,1480 +0.0,-0.4296875,0.0,0.0,1500 +0.0,-0.4296875,0.0,0.0,1520 +0.0,-0.4296875,0.0,0.0,1540 +0.0,-0.4296875,0.0,0.0,1560 +0.0,-0.4296875,0.0,0.0,1580 +0.0,-0.4296875,0.0,0.0,1600 +0.0,-0.4296875,0.0,0.0,1620 +0.0,-0.4296875,0.0,0.0,1640 +0.0,-0.4296875,0.0,0.0,1660 +0.0,-0.4296875,0.0,0.0,1680 +0.0,-0.4296875,0.0,0.0,1700 +0.0,-0.4296875,0.0,0.0,1720 +0.0,-0.4375,0.0,0.0,1740 +0.0,-0.4375,0.0,0.0,1760 +0.0,-0.4375,0.0,0.0,1780 +0.0,-0.4375,0.0,0.0,1800 +0.0,-0.4375,0.0,0.0,1820 +0.0,-0.4375,0.0,0.0,1841 +0.0,-0.4375,0.0,0.0,1860 +0.0,-0.4375,0.0,0.0,1880 +0.0,-0.4375,0.0,0.0,1900 +0.0,-0.4375,0.0,0.0,1920 +0.0,-0.4453125,0.0,0.0,1940 +0.0,-0.4453125,0.0,0.0,1960 +0.0,-0.4453125,0.0,0.0,1980 +0.0,-0.4453125,0.0,0.0,2001 +0.0,-0.4453125,0.0,0.0,2020 +-0.0078125,-0.4453125,0.0,0.0,2040 +-0.0234375,-0.4453125,0.0,0.0,2060 +-0.015625,-0.4453125,0.0,0.0,2080 +-0.015625,-0.4453125,0.0,0.0,2100 +-0.0234375,-0.4453125,0.0,0.0,2120 +-0.0234375,-0.4453125,0.0,0.0,2140 +-0.0234375,-0.4453125,0.0,0.0,2160 +-0.03125,-0.4453125,0.0,0.0,2180 +-0.0390625,-0.4453125,0.0,0.0,2202 +-0.0390625,-0.4453125,0.0,0.0,2220 +-0.0390625,-0.4453125,0.0,0.0,2240 +-0.0390625,-0.4453125,0.0,0.0,2260 +-0.0546875,-0.4453125,0.0,0.0,2280 +-0.0546875,-0.4453125,0.0,0.0,2300 +-0.0546875,-0.4453125,0.0,0.0,2320 +-0.0546875,-0.4453125,0.0,0.0,2340 +-0.0859375,-0.4453125,0.0,0.0,2360 +-0.1171875,-0.4453125,0.0,0.0,2380 +-0.140625,-0.4453125,0.0,0.0,2400 +-0.15625,-0.4453125,0.0,0.0,2420 +-0.203125,-0.4453125,0.0,0.0,2458 +-0.203125,-0.4453125,0.0,0.0,2476 +-0.203125,-0.4453125,0.0,0.0,2491 +-0.2109375,-0.4453125,0.0,0.0,2508 +-0.2109375,-0.4453125,0.0,0.0,2520 +-0.21875,-0.4453125,0.0,0.0,2540 +-0.21875,-0.4453125,0.0,0.0,2560 +-0.2421875,-0.4453125,0.0,0.0,2580 +-0.2421875,-0.4453125,0.0,0.0,2600 +-0.2421875,-0.4453125,0.0,0.0,2620 +-0.2421875,-0.4453125,0.0,0.0,2640 +-0.2421875,-0.4453125,0.0,0.0,2660 +-0.2421875,-0.453125,0.0,0.0,2680 +-0.2421875,-0.453125,0.0,0.0,2700 +-0.2421875,-0.453125,0.0,0.0,2720 +-0.234375,-0.453125,0.0,0.0,2740 +-0.234375,-0.453125,0.0,0.0,2760 +-0.234375,-0.453125,0.0,0.0,2780 +-0.234375,-0.453125,0.0,0.0,2800 +-0.234375,-0.453125,0.0,0.0,2821 +-0.234375,-0.453125,0.0,0.0,2840 +-0.234375,-0.453125,0.0,0.0,2861 +-0.234375,-0.453125,0.0,0.0,2881 +-0.234375,-0.453125,0.0,0.0,2900 +-0.234375,-0.453125,0.0,0.0,2920 +-0.234375,-0.453125,0.0,0.0,2940 +-0.234375,-0.453125,0.0,0.0,2960 +-0.234375,-0.453125,0.0,0.0,2980 +-0.234375,-0.453125,0.0,0.0,3000 +-0.234375,-0.453125,0.0,0.0,3020 +-0.234375,-0.4375,0.0,0.0,3040 +-0.234375,-0.3984375,0.0,0.0,3060 +-0.234375,-0.375,0.0,0.0,3080 +-0.2421875,-0.3671875,0.0,0.0,3100 +-0.2578125,-0.359375,0.0,0.0,3120 +-0.265625,-0.3515625,0.0,0.0,3140 +-0.28125,-0.34375,0.0,0.0,3160 +-0.28125,-0.34375,0.0,0.0,3180 +-0.28125,-0.34375,0.0,0.0,3200 +-0.2890625,-0.34375,0.0,0.0,3220 +-0.2890625,-0.34375,0.0,0.0,3240 +-0.296875,-0.34375,0.0,0.0,3260 +-0.3046875,-0.34375,0.0,0.0,3280 +-0.3125,-0.34375,0.0,0.0,3300 +-0.3125,-0.34375,0.0,0.0,3320 +-0.3125,-0.34375,0.0,0.0,3340 +-0.3125,-0.34375,0.0,0.0,3362 +-0.3125,-0.34375,0.0,0.0,3380 +-0.328125,-0.34375,0.0,0.0,3400 +-0.3359375,-0.34375,0.0,0.0,3420 +-0.3359375,-0.3515625,0.0,0.0,3440 +-0.3359375,-0.3515625,0.0,0.0,3460 +-0.3359375,-0.3515625,0.0,0.0,3480 +-0.3359375,-0.3515625,0.0,0.0,3500 +-0.34375,-0.3515625,0.0,0.0,3520 +-0.34375,-0.3515625,0.0,0.0,3540 +-0.34375,-0.3515625,0.0,0.0,3560 +-0.34375,-0.3515625,0.0,0.0,3580 +-0.3515625,-0.34375,0.0,0.0,3600 +-0.3671875,-0.328125,0.0,0.0,3620 +-0.390625,-0.3125,0.0,0.0,3695 +-0.3984375,-0.296875,0.0,0.0,3708 +-0.3984375,-0.2890625,0.0,0.0,3721 +-0.3984375,-0.28125,0.0,0.0,3734 +-0.40625,-0.2578125,0.0,0.0,3752 +-0.40625,-0.2578125,0.0,0.0,3763 +-0.40625,-0.2421875,0.0,0.0,3775 +-0.40625,-0.2421875,0.0,0.0,3788 +-0.4140625,-0.2265625,0.0,0.0,3800 +-0.421875,-0.2265625,0.0,0.0,3820 +-0.4375,-0.203125,0.0,0.0,3840 +-0.453125,-0.1953125,0.0,0.0,3860 +-0.4609375,-0.1953125,0.0,0.0,3880 +-0.4609375,-0.1953125,0.0,0.0,3900 +-0.4609375,-0.1875,0.0,0.0,3920 +-0.46875,-0.1875,0.0,0.0,3940 +-0.46875,-0.1796875,0.0,0.0,3960 +-0.46875,-0.171875,0.0,0.0,3980 +-0.4765625,-0.1484375,0.0,0.0,4000 +-0.4765625,-0.140625,0.0,0.0,4020 +-0.484375,-0.140625,0.0,0.0,4040 +-0.4921875,-0.1171875,0.0,0.0,4060 +-0.5,-0.109375,0.0,0.0,4080 +-0.5078125,-0.109375,0.0,0.0,4100 +-0.5078125,-0.109375,0.0,0.0,4120 +-0.5078125,-0.109375,0.0,0.0,4140 +-0.5078125,-0.109375,0.0,0.0,4160 +-0.5078125,-0.1015625,0.0,0.0,4180 +-0.5078125,-0.0859375,0.0,0.0,4200 +-0.5078125,-0.078125,0.0,0.0,4220 +-0.5078125,-0.0703125,0.0,0.0,4240 +-0.5078125,-0.0703125,0.0,0.0,4260 +-0.5078125,-0.0546875,0.0,0.0,4280 +-0.5078125,-0.0546875,0.0,0.0,4300 +-0.5078125,-0.0546875,0.0,0.0,4320 +-0.5078125,-0.0546875,0.0,0.0,4340 +-0.5078125,-0.0390625,0.0,0.0,4360 +-0.5078125,-0.0078125,0.0,0.0,4380 +-0.5078125,0.0,0.0,0.0,4400 +-0.5078125,0.0,0.0,0.0,4421 +-0.484375,0.0,0.0,0.0,4440 +-0.484375,0.0,0.0,0.0,4460 +-0.484375,0.0,0.0,0.0,4480 +-0.484375,0.0,0.0,0.0,4500 +-0.484375,0.0,0.0,0.0,4520 +-0.484375,0.0,0.0,0.0,4540 +-0.46875,0.0,0.0,0.0,4560 +-0.4609375,0.0,0.0,0.0,4581 +-0.4609375,0.0,0.0,0.0,4600 +-0.4609375,0.0,0.0,0.0,4620 +-0.4609375,0.023622047156095505,0.0,0.0,4640 +-0.4609375,0.05511811003088951,0.0,0.0,4660 +-0.4609375,0.06299212574958801,0.0,0.0,4680 +-0.4609375,0.06299212574958801,0.0,0.0,4700 +-0.4609375,0.07086614519357681,0.0,0.0,4720 +-0.4609375,0.08661417663097382,0.0,0.0,4740 +-0.4609375,0.13385826349258423,0.0,0.0,4760 +-0.4609375,0.13385826349258423,0.0,0.0,4780 +-0.4609375,0.14173229038715363,0.0,0.0,4800 +-0.4609375,0.14173229038715363,0.0,0.0,4820 +-0.453125,0.14960630238056183,0.0,0.0,4840 +-0.453125,0.14960630238056183,0.0,0.0,4860 +-0.453125,0.14960630238056183,0.0,0.0,4881 +-0.453125,0.14960630238056183,0.0,0.0,4900 +-0.453125,0.14960630238056183,0.0,0.0,4920 +-0.453125,0.15748031437397003,0.0,0.0,4940 +-0.453125,0.15748031437397003,0.0,0.0,4960 +-0.453125,0.16535432636737823,0.0,0.0,4981 +-0.453125,0.16535432636737823,0.0,0.0,5000 +-0.453125,0.16535432636737823,0.0,0.0,5020 +-0.453125,0.16535432636737823,0.0,0.0,5040 +-0.453125,0.16535432636737823,0.0,0.0,5060 +-0.453125,0.17322835326194763,0.0,0.0,5080 +-0.453125,0.19685038924217224,0.0,0.0,5100 +-0.40625,0.19685038924217224,0.0,0.0,5120 +-0.390625,0.22047244012355804,0.0,0.0,5140 +-0.3828125,0.22047244012355804,0.0,0.0,5160 +-0.3671875,0.22834645211696625,0.0,0.0,5181 +-0.3515625,0.23622047901153564,0.0,0.0,5201 +-0.3515625,0.23622047901153564,0.0,0.0,5220 +-0.3359375,0.25196850299835205,0.0,0.0,5240 +-0.3359375,0.25984251499176025,0.0,0.0,5260 +-0.3359375,0.26771652698516846,0.0,0.0,5280 +-0.3359375,0.26771652698516846,0.0,0.0,5300 +-0.3359375,0.26771652698516846,0.0,0.0,5320 +-0.3359375,0.26771652698516846,0.0,0.0,5340 +-0.328125,0.26771652698516846,0.0,0.0,5361 +-0.328125,0.26771652698516846,0.0,0.0,5381 +-0.3203125,0.28346458077430725,0.0,0.0,5400 +-0.3125,0.28346458077430725,0.0,0.0,5420 +-0.3125,0.29921260476112366,0.0,0.0,5440 +-0.3125,0.29921260476112366,0.0,0.0,5460 +-0.3125,0.29921260476112366,0.0,0.0,5481 +-0.3125,0.29921260476112366,0.0,0.0,5500 +-0.3125,0.29921260476112366,0.0,0.0,5520 +-0.3125,0.29921260476112366,0.0,0.0,5540 +-0.3125,0.29921260476112366,0.0,0.0,5560 +-0.3125,0.29921260476112366,0.0,0.0,5580 +-0.3125,0.29921260476112366,0.0,0.0,5600 +-0.3125,0.30708661675453186,0.0,0.0,5620 +-0.3125,0.30708661675453186,0.0,0.0,5640 +-0.3125,0.30708661675453186,0.0,0.0,5660 +-0.3125,0.30708661675453186,0.0,0.0,5680 +-0.3125,0.30708661675453186,0.0,0.0,5700 +-0.3125,0.30708661675453186,0.0,0.0,5720 +-0.3125,0.30708661675453186,0.0,0.0,5741 +-0.2890625,0.33070865273475647,0.0,0.0,5760 +-0.2890625,0.33070865273475647,0.0,0.0,5780 +-0.2890625,0.33070865273475647,0.0,0.0,5800 +-0.2890625,0.33070865273475647,0.0,0.0,5820 +-0.2890625,0.33070865273475647,0.0,0.0,5840 +-0.2890625,0.33070865273475647,0.0,0.0,5862 +-0.2890625,0.33070865273475647,0.0,0.0,5881 +-0.2890625,0.33070865273475647,0.0,0.0,5900 +-0.2890625,0.33070865273475647,0.0,0.0,5920 +-0.2890625,0.33070865273475647,0.0,0.0,5940 +-0.2890625,0.3385826647281647,0.0,0.0,5960 +-0.2890625,0.3385826647281647,0.0,0.0,5980 +-0.2890625,0.3385826647281647,0.0,0.0,6000 +-0.2734375,0.36220473051071167,0.0,0.0,6020 +-0.2734375,0.36220473051071167,0.0,0.0,6040 +-0.2734375,0.36220473051071167,0.0,0.0,6060 +-0.2734375,0.36220473051071167,0.0,0.0,6080 +-0.2734375,0.36220473051071167,0.0,0.0,6100 +-0.2421875,0.36220473051071167,0.0,0.0,6120 +-0.2421875,0.36220473051071167,0.0,0.0,6140 +-0.2421875,0.36220473051071167,0.0,0.0,6160 +-0.2421875,0.36220473051071167,0.0,0.0,6180 +-0.2421875,0.36220473051071167,0.0,0.0,6200 +-0.2421875,0.36220473051071167,0.0,0.0,6220 +-0.203125,0.36220473051071167,0.0,0.0,6240 +-0.203125,0.36220473051071167,0.0,0.0,6260 +-0.1875,0.36220473051071167,0.0,0.0,6280 +-0.1875,0.36220473051071167,0.0,0.0,6300 +-0.1875,0.36220473051071167,0.0,0.0,6320 +-0.1875,0.36220473051071167,0.0,0.0,6340 +-0.1875,0.36220473051071167,0.0,0.0,6360 +-0.1875,0.36220473051071167,0.0,0.0,6380 +-0.1875,0.36220473051071167,0.0,0.0,6400 +-0.1484375,0.3700787425041199,0.0,0.0,6420 +-0.1484375,0.3700787425041199,0.0,0.0,6440 +-0.1484375,0.3700787425041199,0.0,0.0,6460 +-0.1484375,0.3937007784843445,0.0,0.0,6480 +-0.1484375,0.4015747904777527,0.0,0.0,6500 +-0.1484375,0.4015747904777527,0.0,0.0,6581 +-0.1484375,0.4015747904777527,0.0,0.0,6593 +-0.1484375,0.4015747904777527,0.0,0.0,6606 +-0.1484375,0.4015747904777527,0.0,0.0,6618 +-0.1484375,0.4409448802471161,0.0,0.0,6631 +-0.1484375,0.4409448802471161,0.0,0.0,6644 +-0.1484375,0.4409448802471161,0.0,0.0,6657 +-0.1484375,0.4409448802471161,0.0,0.0,6667 +-0.1484375,0.4409448802471161,0.0,0.0,6680 +-0.1484375,0.4409448802471161,0.0,0.0,6700 +-0.1484375,0.4409448802471161,0.0,0.0,6721 +-0.1484375,0.4409448802471161,0.0,0.0,6740 +-0.1484375,0.4409448802471161,0.0,0.0,6760 +-0.1484375,0.4409448802471161,0.0,0.0,6780 +-0.1484375,0.4409448802471161,0.0,0.0,6800 +-0.1484375,0.4409448802471161,0.0,0.0,6820 +-0.1484375,0.4409448802471161,0.0,0.0,6840 +-0.1484375,0.4409448802471161,0.0,0.0,6860 +-0.1484375,0.4409448802471161,0.0,0.0,6880 +-0.1484375,0.4409448802471161,0.0,0.0,6900 +-0.1484375,0.4409448802471161,0.0,0.0,6920 +-0.1484375,0.4409448802471161,0.0,0.0,6940 +-0.1484375,0.4409448802471161,0.0,0.0,6960 +-0.1484375,0.4409448802471161,0.0,0.0,6980 +-0.1484375,0.4409448802471161,0.0,0.0,7000 +-0.1484375,0.4409448802471161,0.0,0.0,7020 +-0.1484375,0.4409448802471161,0.0,0.0,7040 +-0.1484375,0.4409448802471161,0.0,0.0,7060 +-0.1484375,0.4409448802471161,0.0,0.0,7080 +-0.1484375,0.4409448802471161,0.0,0.0,7100 +-0.1484375,0.4409448802471161,0.0,0.0,7120 +-0.1484375,0.4409448802471161,0.0,0.0,7140 +-0.1484375,0.4409448802471161,0.0,0.0,7160 +-0.1484375,0.4409448802471161,0.0,0.0,7180 +-0.1484375,0.4409448802471161,0.0,0.0,7200 +-0.1484375,0.4409448802471161,0.0,0.0,7220 +-0.1484375,0.4409448802471161,0.0,0.0,7240 +-0.1484375,0.4409448802471161,0.0,0.0,7260 +-0.1484375,0.4409448802471161,0.0,0.0,7280 +-0.1484375,0.4409448802471161,0.0,0.0,7300 +-0.1484375,0.4409448802471161,0.0,0.0,7320 +-0.1484375,0.4409448802471161,0.0,0.0,7340 +-0.1484375,0.4409448802471161,0.0,0.0,7360 +-0.1484375,0.4409448802471161,0.0,0.0,7380 +-0.1484375,0.4409448802471161,0.0,0.0,7400 +-0.1484375,0.4409448802471161,0.0,0.0,7420 +-0.1484375,0.4409448802471161,0.0,0.0,7440 +-0.1484375,0.4409448802471161,0.0,0.0,7460 +-0.1484375,0.4409448802471161,0.0,0.0,7486 +-0.1484375,0.4409448802471161,0.0,0.0,7500 +-0.1484375,0.4409448802471161,0.0,0.0,7520 +-0.1484375,0.4409448802471161,0.0,0.0,7540 +-0.1484375,0.4409448802471161,0.0,0.0,7560 +-0.1484375,0.4409448802471161,0.0,0.0,7580 +-0.1484375,0.4330708682537079,0.0,0.0,7600 +-0.1484375,0.4330708682537079,0.0,0.0,7620 +-0.1484375,0.4330708682537079,0.0,0.0,7640 +-0.1484375,0.4330708682537079,0.0,0.0,7660 +-0.1484375,0.4330708682537079,0.0,0.0,7680 +-0.1484375,0.4094488322734833,0.0,0.0,7700 +-0.1484375,0.3779527544975281,0.0,0.0,7720 +-0.1484375,0.3700787425041199,0.0,0.0,7740 +-0.1484375,0.3700787425041199,0.0,0.0,7760 +-0.15625,0.3700787425041199,0.0,0.0,7780 +-0.171875,0.3700787425041199,0.0,0.0,7800 +-0.1796875,0.3700787425041199,0.0,0.0,7820 +-0.1875,0.3700787425041199,0.0,0.0,7840 +-0.203125,0.3700787425041199,0.0,0.0,7860 +-0.2109375,0.3700787425041199,0.0,0.0,7880 +-0.2265625,0.36220473051071167,0.0,0.0,7900 +-0.2265625,0.36220473051071167,0.0,0.0,7920 +-0.2265625,0.36220473051071167,0.0,0.0,7940 +-0.2265625,0.36220473051071167,0.0,0.0,7960 +-0.2265625,0.36220473051071167,0.0,0.0,7980 +-0.2265625,0.36220473051071167,0.0,0.0,8000 +-0.2421875,0.36220473051071167,0.0,0.0,8021 +-0.2578125,0.36220473051071167,0.0,0.0,8040 +-0.265625,0.35433071851730347,0.0,0.0,8060 +-0.28125,0.35433071851730347,0.0,0.0,8080 +-0.28125,0.33070865273475647,0.0,0.0,8102 +-0.2890625,0.33070865273475647,0.0,0.0,8121 +-0.2890625,0.33070865273475647,0.0,0.0,8140 +-0.296875,0.31496062874794006,0.0,0.0,8160 +-0.3125,0.31496062874794006,0.0,0.0,8180 +-0.3125,0.32283464074134827,0.0,0.0,8200 +-0.3203125,0.29133859276771545,0.0,0.0,8220 +-0.3203125,0.27559053897857666,0.0,0.0,8240 +-0.3203125,0.27559053897857666,0.0,0.0,8260 +-0.3203125,0.25196850299835205,0.0,0.0,8280 +-0.3203125,0.25196850299835205,0.0,0.0,8300 +-0.3203125,0.24409449100494385,0.0,0.0,8320 +-0.3359375,0.22047244012355804,0.0,0.0,8340 +-0.34375,0.19685038924217224,0.0,0.0,8360 +-0.34375,0.19685038924217224,0.0,0.0,8380 +-0.359375,0.19685038924217224,0.0,0.0,8400 +-0.359375,0.19685038924217224,0.0,0.0,8420 +-0.3671875,0.16535432636737823,0.0,0.0,8440 +-0.3671875,0.14960630238056183,0.0,0.0,8460 +-0.3671875,0.11811023950576782,0.0,0.0,8480 +-0.3671875,0.07086614519357681,0.0,0.0,8500 +-0.3671875,0.0,0.0,0.0,8520 +-0.3671875,0.0,0.0,0.0,8540 +-0.3671875,0.0,0.0,0.0,8560 +-0.3671875,0.0,0.0,0.0,8580 +-0.3671875,0.0,0.0,0.0,8600 +-0.3671875,0.0,0.0,0.0,8620 +-0.3671875,0.0,0.0,0.0,8640 +-0.3671875,0.0,0.0,0.0,8660 +-0.3671875,0.0,0.0,0.0,8680 +-0.3671875,0.0,0.0,0.0,8700 +-0.3671875,0.0,0.0,0.0,8720 +-0.3671875,0.0,0.0,0.0,8740 +-0.375,0.0,0.0,0.0,8760 +-0.3828125,0.0,0.0,0.0,8780 +-0.390625,0.0,0.0,0.0,8800 +-0.390625,0.0,0.0,0.0,8820 +-0.3984375,0.0,0.0,0.0,8840 +-0.3984375,0.0,0.0,0.0,8860 +-0.3984375,0.0,0.0,0.0,8880 +-0.3984375,0.0,0.0,0.0,8900 +-0.3984375,-0.0078125,0.0,0.0,8920 +-0.3984375,-0.0234375,0.0,0.0,8940 +-0.3984375,-0.0546875,0.0,0.0,8960 +-0.40625,-0.109375,0.0,0.0,8980 +-0.40625,-0.125,0.0,0.0,9000 +-0.40625,-0.140625,0.0,0.0,9020 +-0.40625,-0.1640625,0.0,0.0,9040 +-0.40625,-0.203125,0.0,0.0,9060 +-0.40625,-0.203125,0.0,0.0,9080 +-0.40625,-0.203125,0.0,0.0,9100 +-0.40625,-0.234375,0.0,0.0,9120 +-0.40625,-0.25,0.0,0.0,9140 +-0.40625,-0.2890625,0.0,0.0,9160 +-0.40625,-0.3046875,0.0,0.0,9180 +-0.40625,-0.3203125,0.0,0.0,9200 +-0.3984375,-0.34375,0.0,0.0,9220 +-0.3984375,-0.34375,0.0,0.0,9240 +-0.3984375,-0.34375,0.0,0.0,9260 +-0.3984375,-0.359375,0.0,0.0,9280 +-0.3984375,-0.359375,0.0,0.0,9300 +-0.3984375,-0.359375,0.0,0.0,9320 +-0.375,-0.359375,0.0,0.0,9340 +-0.3671875,-0.359375,0.0,0.0,9360 +-0.3671875,-0.3671875,0.0,0.0,9380 +-0.3671875,-0.3671875,0.0,0.0,9400 +-0.359375,-0.3671875,0.0,0.0,9420 +-0.3359375,-0.375,0.0,0.0,9440 +-0.3359375,-0.3828125,0.0,0.0,9460 +-0.3359375,-0.3828125,0.0,0.0,9480 +-0.3359375,-0.3828125,0.0,0.0,9500 +-0.328125,-0.3828125,0.0,0.0,9520 +-0.328125,-0.3828125,0.0,0.0,9540 +-0.328125,-0.3828125,0.0,0.0,9560 +-0.3203125,-0.3828125,0.0,0.0,9580 +-0.3203125,-0.3828125,0.0,0.0,9600 +-0.3203125,-0.3828125,0.0,0.0,9620 +-0.3125,-0.3828125,0.0,0.0,9641 +-0.296875,-0.3828125,0.0,0.0,9660 +-0.28125,-0.3828125,0.0,0.0,9680 +-0.2734375,-0.3828125,0.0,0.0,9700 +-0.265625,-0.3828125,0.0,0.0,9720 +-0.203125,-0.375,0.0,0.0,9741 +-0.1875,-0.3671875,0.0,0.0,9760 +-0.1875,-0.3671875,0.0,0.0,9780 +-0.1796875,-0.3671875,0.0,0.0,9800 +-0.15625,-0.3671875,0.0,0.0,9820 +-0.1328125,-0.3671875,0.0,0.0,9840 +-0.125,-0.3671875,0.0,0.0,9860 +-0.1171875,-0.3671875,0.0,0.0,9880 +-0.1171875,-0.3671875,0.0,0.0,9900 +-0.078125,-0.3671875,0.0,0.0,9920 +-0.0625,-0.3671875,0.0,0.0,9940 +-0.0390625,-0.3671875,0.0,0.0,9960 +-0.0234375,-0.3671875,0.0,0.0,9980 +-0.0078125,-0.3671875,0.0,0.0,10000 +-0.0078125,-0.3671875,0.0,0.0,10020 +0.0,-0.3671875,0.0,0.0,10040 +0.0,-0.3671875,0.0,0.0,10060 +-0.0078125,-0.3671875,0.0,0.0,10080 +0.0,-0.3671875,0.0,0.0,10100 +0.0,-0.3671875,0.0,0.0,10120 +0.0,-0.3671875,0.0,0.0,10140 +0.0,-0.3671875,0.0,0.0,10160 +0.0,-0.3671875,0.0,0.0,10180 +0.0,-0.3671875,0.0,0.0,10200 +0.0,-0.3671875,0.0,0.0,10220 +0.0,-0.3671875,0.0,0.0,10240 +0.0,-0.3671875,0.0,0.0,10260 +0.0,-0.3671875,0.0,0.0,10280 +0.0,-0.3671875,0.0,0.0,10300 +0.0,-0.3671875,0.0,0.0,10320 +0.0,-0.3671875,0.0,0.0,10340 +0.0,-0.3671875,0.0,0.0,10360 +0.0,-0.3671875,0.0,0.0,10380 +0.0,-0.3671875,0.0,0.0,10400 +0.0,-0.3671875,0.0,0.0,10420 +0.007874015718698502,-0.3671875,0.0,0.0,10440 +0.031496062874794006,-0.3671875,0.0,0.0,10460 +0.04724409431219101,-0.3671875,0.0,0.0,10480 +0.05511811003088951,-0.3671875,0.0,0.0,10500 +0.05511811003088951,-0.3671875,0.0,0.0,10520 +0.06299212574958801,-0.3671875,0.0,0.0,10540 +0.06299212574958801,-0.3671875,0.0,0.0,10560 +0.06299212574958801,-0.3671875,0.0,0.0,10580 +0.06299212574958801,-0.3671875,0.0,0.0,10600 +0.06299212574958801,-0.3671875,0.0,0.0,10620 +0.06299212574958801,-0.3671875,0.0,0.0,10640 +0.06299212574958801,-0.359375,0.0,0.0,10660 +0.06299212574958801,-0.359375,0.0,0.0,10680 +0.06299212574958801,-0.359375,0.0,0.0,10700 +0.06299212574958801,-0.359375,0.0,0.0,10720 +0.07086614519357681,-0.328125,0.0,0.0,10740 +0.07086614519357681,-0.328125,0.0,0.0,10760 +0.07874015718698502,-0.328125,0.0,0.0,10780 +0.07086614519357681,-0.3203125,0.0,0.0,10800 +0.07874015718698502,-0.3203125,0.0,0.0,10820 +0.07874015718698502,-0.3203125,0.0,0.0,10840 +0.07874015718698502,-0.3203125,0.0,0.0,10860 +0.07874015718698502,-0.3203125,0.0,0.0,10880 +0.09448818862438202,-0.3046875,0.0,0.0,10900 +0.11023622006177902,-0.3046875,0.0,0.0,10920 +0.11811023950576782,-0.3046875,0.0,0.0,10940 +0.11811023950576782,-0.3046875,0.0,0.0,10960 +0.11811023950576782,-0.3046875,0.0,0.0,10980 +0.11811023950576782,-0.3046875,0.0,0.0,11000 +0.11811023950576782,-0.3046875,0.0,0.0,11021 +0.11811023950576782,-0.296875,0.0,0.0,11040 +0.11811023950576782,-0.296875,0.0,0.0,11061 +0.11811023950576782,-0.2890625,0.0,0.0,11085 +0.12598425149917603,-0.2890625,0.0,0.0,11100 +0.12598425149917603,-0.2890625,0.0,0.0,11121 +0.12598425149917603,-0.265625,0.0,0.0,11140 +0.12598425149917603,-0.265625,0.0,0.0,11161 +0.14173229038715363,-0.265625,0.0,0.0,11180 +0.14173229038715363,-0.265625,0.0,0.0,11200 +0.15748031437397003,-0.2265625,0.0,0.0,11220 +0.16535432636737823,-0.2265625,0.0,0.0,11240 +0.16535432636737823,-0.234375,0.0,0.0,11260 +0.16535432636737823,-0.234375,0.0,0.0,11280 +0.16535432636737823,-0.234375,0.0,0.0,11300 +0.16535432636737823,-0.234375,0.0,0.0,11320 +0.16535432636737823,-0.2421875,0.0,0.0,11340 +0.16535432636737823,-0.2421875,0.0,0.0,11360 +0.17322835326194763,-0.2421875,0.0,0.0,11380 +0.17322835326194763,-0.2421875,0.0,0.0,11400 +0.17322835326194763,-0.2421875,0.0,0.0,11420 +0.18110236525535583,-0.25,0.0,0.0,11440 +0.18110236525535583,-0.25,0.0,0.0,11461 +0.18110236525535583,-0.2578125,0.0,0.0,11481 +0.18110236525535583,-0.2578125,0.0,0.0,11500 +0.18110236525535583,-0.2578125,0.0,0.0,11520 +0.18110236525535583,-0.2734375,0.0,0.0,11540 +0.18110236525535583,-0.2890625,0.0,0.0,11560 +0.18110236525535583,-0.2890625,0.0,0.0,11580 +0.18110236525535583,-0.3046875,0.0,0.0,11600 +0.18110236525535583,-0.3125,0.0,0.0,11620 +0.18897637724876404,-0.3203125,0.0,0.0,11640 +0.20472441613674164,-0.328125,0.0,0.0,11660 +0.22834645211696625,-0.34375,0.0,0.0,11680 +0.24409449100494385,-0.34375,0.0,0.0,11700 +0.27559053897857666,-0.34375,0.0,0.0,11725 +0.27559053897857666,-0.34375,0.0,0.0,11740 +0.27559053897857666,-0.34375,0.0,0.0,11760 +0.27559053897857666,-0.34375,0.0,0.0,11780 +0.27559053897857666,-0.34375,0.0,0.0,11800 +0.27559053897857666,-0.34375,0.0,0.0,11820 +0.27559053897857666,-0.34375,0.0,0.0,11840 +0.27559053897857666,-0.34375,0.0,0.0,11860 +0.27559053897857666,-0.34375,0.0,0.0,11880 +0.27559053897857666,-0.34375,0.0,0.0,11900 +0.28346458077430725,-0.34375,0.0,0.0,11920 +0.28346458077430725,-0.34375,0.0,0.0,11940 +0.28346458077430725,-0.296875,0.0,0.0,11960 +0.28346458077430725,-0.25,0.0,0.0,11980 +0.28346458077430725,-0.2109375,0.0,0.0,12000 +0.28346458077430725,-0.1875,0.0,0.0,12020 +0.28346458077430725,-0.15625,0.0,0.0,12041 +0.28346458077430725,-0.140625,0.0,0.0,12060 +0.28346458077430725,-0.125,0.0,0.0,12080 +0.28346458077430725,-0.125,0.0,0.0,12100 +0.28346458077430725,-0.125,0.0,0.0,12120 +0.29133859276771545,-0.125,0.0,0.0,12140 +0.29133859276771545,-0.125,0.0,0.0,12160 +0.29133859276771545,-0.1328125,0.0,0.0,12180 +0.29133859276771545,-0.1328125,0.0,0.0,12200 +0.29921260476112366,-0.1328125,0.0,0.0,12220 +0.30708661675453186,-0.1328125,0.0,0.0,12240 +0.31496062874794006,-0.1328125,0.0,0.0,12260 +0.33070865273475647,-0.1328125,0.0,0.0,12280 +0.35433071851730347,-0.1328125,0.0,0.0,12301 +0.3779527544975281,-0.1328125,0.0,0.0,12320 +0.3858267664909363,-0.1328125,0.0,0.0,12340 +0.4015747904777527,-0.1328125,0.0,0.0,12360 +0.4015747904777527,-0.1328125,0.0,0.0,12380 +0.4015747904777527,-0.1328125,0.0,0.0,12400 +0.4015747904777527,-0.1328125,0.0,0.0,12420 +0.4015747904777527,-0.1328125,0.0,0.0,12440 +0.4015747904777527,-0.1328125,0.0,0.0,12460 +0.4015747904777527,-0.1328125,0.0,0.0,12480 +0.4015747904777527,-0.1328125,0.0,0.0,12500 +0.4015747904777527,-0.1328125,0.0,0.0,12520 +0.4015747904777527,-0.1328125,0.0,0.0,12540 +0.4015747904777527,-0.1328125,0.0,0.0,12560 +0.4015747904777527,-0.1328125,0.0,0.0,12580 +0.4015747904777527,-0.1328125,0.0,0.0,12600 +0.4015747904777527,-0.1328125,0.0,0.0,12620 +0.4015747904777527,-0.1328125,0.0,0.0,12640 +0.4015747904777527,-0.125,0.0,0.0,12660 +0.4015747904777527,-0.125,0.0,0.0,12681 +0.4015747904777527,-0.125,0.0,0.0,12700 +0.4015747904777527,-0.125,0.0,0.0,12720 +0.4015747904777527,-0.125,0.0,0.0,12740 +0.4015747904777527,-0.125,0.0,0.0,12760 +0.4015747904777527,-0.125,0.0,0.0,12780 +0.4015747904777527,-0.1171875,0.0,0.0,12801 +0.4015747904777527,-0.109375,0.0,0.0,12820 +0.4094488322734833,-0.09375,0.0,0.0,12840 +0.4094488322734833,-0.0859375,0.0,0.0,12860 +0.4094488322734833,-0.0703125,0.0,0.0,12880 +0.4251968562602997,-0.0625,0.0,0.0,12900 +0.4251968562602997,-0.0625,0.0,0.0,12920 +0.4330708682537079,-0.0546875,0.0,0.0,12940 +0.4330708682537079,-0.0546875,0.0,0.0,12960 +0.4330708682537079,-0.046875,0.0,0.0,12980 +0.4330708682537079,-0.0390625,0.0,0.0,13000 +0.4330708682537079,-0.015625,0.0,0.0,13020 +0.4330708682537079,0.0,0.0,0.0,13041 +0.4330708682537079,0.0,0.0,0.0,13061 +0.4330708682537079,0.0,0.0,0.0,13080 +0.4330708682537079,0.0,0.0,0.0,13100 +0.4330708682537079,0.0,0.0,0.0,13120 +0.4330708682537079,0.0,0.0,0.0,13140 +0.4330708682537079,0.0,0.0,0.0,13160 +0.4330708682537079,0.0,0.0,0.0,13180 +0.4330708682537079,0.0,0.0,0.0,13201 +0.4330708682537079,0.0,0.0,0.0,13220 +0.4330708682537079,0.0,0.0,0.0,13243 +0.4330708682537079,0.0,0.0,0.0,13260 +0.4330708682537079,0.0,0.0,0.0,13280 +0.4330708682537079,0.0,0.0,0.0,13300 +0.4330708682537079,0.0,0.0,0.0,13320 +0.4330708682537079,0.0,0.0,0.0,13340 +0.4330708682537079,0.0,0.0,0.0,13360 +0.4330708682537079,0.0,0.0,0.0,13380 +0.4330708682537079,0.0,0.0,0.0,13400 +0.4330708682537079,0.0,0.0,0.0,13420 +0.4330708682537079,0.0,0.0,0.0,13440 +0.4330708682537079,0.007874015718698502,0.0,0.0,13460 +0.4330708682537079,0.015748031437397003,0.0,0.0,13480 +0.4330708682537079,0.05511811003088951,0.0,0.0,13500 +0.4330708682537079,0.05511811003088951,0.0,0.0,13520 +0.4330708682537079,0.05511811003088951,0.0,0.0,13540 +0.4330708682537079,0.05511811003088951,0.0,0.0,13560 +0.4330708682537079,0.07086614519357681,0.0,0.0,13580 +0.4330708682537079,0.10236220806837082,0.0,0.0,13600 +0.4330708682537079,0.12598425149917603,0.0,0.0,13620 +0.4330708682537079,0.12598425149917603,0.0,0.0,13640 +0.4330708682537079,0.12598425149917603,0.0,0.0,13660 +0.4330708682537079,0.12598425149917603,0.0,0.0,13680 +0.4330708682537079,0.12598425149917603,0.0,0.0,13700 +0.4330708682537079,0.13385826349258423,0.0,0.0,13720 +0.4330708682537079,0.14173229038715363,0.0,0.0,13740 +0.4330708682537079,0.14173229038715363,0.0,0.0,13760 +0.4330708682537079,0.14173229038715363,0.0,0.0,13780 +0.4330708682537079,0.14173229038715363,0.0,0.0,13800 +0.4330708682537079,0.14960630238056183,0.0,0.0,13820 +0.4330708682537079,0.14960630238056183,0.0,0.0,13840 +0.4330708682537079,0.15748031437397003,0.0,0.0,13860 +0.4330708682537079,0.15748031437397003,0.0,0.0,13880 +0.4330708682537079,0.16535432636737823,0.0,0.0,13900 +0.4330708682537079,0.17322835326194763,0.0,0.0,13920 +0.4330708682537079,0.17322835326194763,0.0,0.0,13940 +0.4330708682537079,0.18897637724876404,0.0,0.0,13960 +0.4330708682537079,0.19685038924217224,0.0,0.0,13980 +0.4330708682537079,0.19685038924217224,0.0,0.0,14001 +0.4330708682537079,0.19685038924217224,0.0,0.0,14021 +0.4330708682537079,0.19685038924217224,0.0,0.0,14040 +0.4330708682537079,0.20472441613674164,0.0,0.0,14060 +0.4330708682537079,0.22047244012355804,0.0,0.0,14080 +0.4330708682537079,0.22047244012355804,0.0,0.0,14100 +0.4330708682537079,0.22834645211696625,0.0,0.0,14120 +0.4330708682537079,0.23622047901153564,0.0,0.0,14140 +0.4330708682537079,0.23622047901153564,0.0,0.0,14160 +0.4330708682537079,0.24409449100494385,0.0,0.0,14180 +0.4330708682537079,0.25196850299835205,0.0,0.0,14204 +0.4330708682537079,0.25196850299835205,0.0,0.0,14220 +0.4330708682537079,0.26771652698516846,0.0,0.0,14242 +0.4330708682537079,0.26771652698516846,0.0,0.0,14260 +0.4330708682537079,0.27559053897857666,0.0,0.0,14280 +0.4330708682537079,0.28346458077430725,0.0,0.0,14300 +0.4251968562602997,0.28346458077430725,0.0,0.0,14320 +0.4094488322734833,0.29133859276771545,0.0,0.0,14340 +0.4094488322734833,0.29133859276771545,0.0,0.0,14360 +0.4015747904777527,0.29133859276771545,0.0,0.0,14380 +0.3937007784843445,0.29133859276771545,0.0,0.0,14400 +0.3937007784843445,0.29133859276771545,0.0,0.0,14420 +0.3937007784843445,0.29133859276771545,0.0,0.0,14440 +0.3937007784843445,0.29133859276771545,0.0,0.0,14460 +0.3937007784843445,0.29133859276771545,0.0,0.0,14481 +0.3937007784843445,0.29133859276771545,0.0,0.0,14500 +0.3858267664909363,0.29133859276771545,0.0,0.0,14520 +0.3779527544975281,0.29921260476112366,0.0,0.0,14540 +0.3779527544975281,0.29921260476112366,0.0,0.0,14560 +0.3779527544975281,0.29921260476112366,0.0,0.0,14580 +0.3779527544975281,0.29921260476112366,0.0,0.0,14600 +0.3779527544975281,0.29921260476112366,0.0,0.0,14620 +0.3779527544975281,0.29921260476112366,0.0,0.0,14640 +0.3779527544975281,0.29921260476112366,0.0,0.0,14660 +0.3779527544975281,0.29921260476112366,0.0,0.0,14680 +0.3779527544975281,0.29921260476112366,0.0,0.0,14700 +0.3779527544975281,0.29921260476112366,0.0,0.0,14720 +0.3779527544975281,0.29921260476112366,0.0,0.0,14740 +0.3779527544975281,0.29921260476112366,0.0,0.0,14760 +0.3779527544975281,0.29921260476112366,0.0,0.0,14780 +0.3779527544975281,0.29921260476112366,0.0,0.0,14800 +0.3779527544975281,0.29921260476112366,0.0,0.0,14820 +0.3779527544975281,0.29921260476112366,0.0,0.0,14840 +0.3779527544975281,0.29921260476112366,0.0,0.0,14860 +0.3779527544975281,0.29921260476112366,0.0,0.0,14880 +0.3779527544975281,0.29921260476112366,0.0,0.0,14900 +0.3779527544975281,0.29921260476112366,0.0,0.0,14920 +0.3779527544975281,0.29921260476112366,0.0,0.0,14940 +0.3779527544975281,0.29921260476112366,0.0,0.0,14961 +0.3779527544975281,0.29921260476112366,0.0,0.0,14980 +0.3779527544975281,0.29921260476112366,0.0,0.0,15000 +0.3779527544975281,0.29921260476112366,0.0,0.0,15020 +0.3779527544975281,0.30708661675453186,0.0,0.0,15040 +0.3779527544975281,0.32283464074134827,0.0,0.0,15060 +0.3700787425041199,0.33070865273475647,0.0,0.0,15080 +0.3700787425041199,0.33070865273475647,0.0,0.0,15100 +0.3700787425041199,0.33070865273475647,0.0,0.0,15120 +0.36220473051071167,0.33070865273475647,0.0,0.0,15140 +0.36220473051071167,0.33070865273475647,0.0,0.0,15160 +0.36220473051071167,0.33070865273475647,0.0,0.0,15180 +0.36220473051071167,0.3385826647281647,0.0,0.0,15200 +0.36220473051071167,0.3385826647281647,0.0,0.0,15220 +0.36220473051071167,0.3385826647281647,0.0,0.0,15240 +0.36220473051071167,0.3385826647281647,0.0,0.0,15260 +0.36220473051071167,0.33070865273475647,0.0,0.0,15280 +0.36220473051071167,0.33070865273475647,0.0,0.0,15300 +0.36220473051071167,0.33070865273475647,0.0,0.0,15320 +0.36220473051071167,0.33070865273475647,0.0,0.0,15340 +0.36220473051071167,0.33070865273475647,0.0,0.0,15360 +0.36220473051071167,0.33070865273475647,0.0,0.0,15380 +0.36220473051071167,0.33070865273475647,0.0,0.0,15400 +0.36220473051071167,0.33070865273475647,0.0,0.0,15420 +0.36220473051071167,0.33070865273475647,0.0,0.0,15440 +0.36220473051071167,0.30708661675453186,0.0,0.0,15460 +0.36220473051071167,0.26771652698516846,0.0,0.0,15480 +0.36220473051071167,0.25984251499176025,0.0,0.0,15500 +0.36220473051071167,0.24409449100494385,0.0,0.0,15520 +0.36220473051071167,0.24409449100494385,0.0,0.0,15540 +0.36220473051071167,0.22834645211696625,0.0,0.0,15560 +0.36220473051071167,0.22047244012355804,0.0,0.0,15580 +0.36220473051071167,0.22047244012355804,0.0,0.0,15600 +0.36220473051071167,0.22047244012355804,0.0,0.0,15621 +0.36220473051071167,0.20472441613674164,0.0,0.0,15640 +0.36220473051071167,0.19685038924217224,0.0,0.0,15660 +0.36220473051071167,0.19685038924217224,0.0,0.0,15681 +0.36220473051071167,0.19685038924217224,0.0,0.0,15700 +0.36220473051071167,0.19685038924217224,0.0,0.0,15720 +0.36220473051071167,0.19685038924217224,0.0,0.0,15740 +0.36220473051071167,0.19685038924217224,0.0,0.0,15760 +0.36220473051071167,0.19685038924217224,0.0,0.0,15780 +0.36220473051071167,0.19685038924217224,0.0,0.0,15800 +0.36220473051071167,0.19685038924217224,0.0,0.0,15821 +0.36220473051071167,0.19685038924217224,0.0,0.0,15841 +0.36220473051071167,0.19685038924217224,0.0,0.0,15860 +0.36220473051071167,0.19685038924217224,0.0,0.0,15880 +0.36220473051071167,0.20472441613674164,0.0,0.0,15900 +0.36220473051071167,0.26771652698516846,0.0,0.0,15922 +0.36220473051071167,0.26771652698516846,0.0,0.0,15940 +0.36220473051071167,0.26771652698516846,0.0,0.0,15960 +0.36220473051071167,0.26771652698516846,0.0,0.0,15980 +0.36220473051071167,0.26771652698516846,0.0,0.0,16000 +0.36220473051071167,0.26771652698516846,0.0,0.0,16021 +0.36220473051071167,0.26771652698516846,0.0,0.0,16040 +0.36220473051071167,0.26771652698516846,0.0,0.0,16061 +0.36220473051071167,0.26771652698516846,0.0,0.0,16080 +0.36220473051071167,0.26771652698516846,0.0,0.0,16100 +0.36220473051071167,0.26771652698516846,0.0,0.0,16120 +0.36220473051071167,0.26771652698516846,0.0,0.0,16140 +0.36220473051071167,0.27559053897857666,0.0,0.0,16160 +0.36220473051071167,0.27559053897857666,0.0,0.0,16180 +0.36220473051071167,0.27559053897857666,0.0,0.0,16200 +0.36220473051071167,0.27559053897857666,0.0,0.0,16220 +0.36220473051071167,0.27559053897857666,0.0,0.0,16240 +0.36220473051071167,0.27559053897857666,0.0,0.0,16260 +0.36220473051071167,0.27559053897857666,0.0,0.0,16280 +0.36220473051071167,0.27559053897857666,0.0,0.0,16300 +0.36220473051071167,0.27559053897857666,0.0,0.0,16320 +0.36220473051071167,0.27559053897857666,0.0,0.0,16340 +0.36220473051071167,0.27559053897857666,0.0,0.0,16362 +0.36220473051071167,0.27559053897857666,0.0,0.0,16380 +0.36220473051071167,0.27559053897857666,0.0,0.0,16400 +0.36220473051071167,0.27559053897857666,0.0,0.0,16420 +0.36220473051071167,0.27559053897857666,0.0,0.0,16440 +0.36220473051071167,0.27559053897857666,0.0,0.0,16460 +0.36220473051071167,0.27559053897857666,0.0,0.0,16480 +0.36220473051071167,0.27559053897857666,0.0,0.0,16500 +0.36220473051071167,0.27559053897857666,0.0,0.0,16520 +0.36220473051071167,0.27559053897857666,0.0,0.0,16540 +0.36220473051071167,0.27559053897857666,0.0,0.0,16560 +0.36220473051071167,0.27559053897857666,0.0,0.0,16580 +0.36220473051071167,0.27559053897857666,0.0,0.0,16600 +0.36220473051071167,0.27559053897857666,0.0,0.0,16620 +0.36220473051071167,0.27559053897857666,0.0,0.0,16640 +0.36220473051071167,0.27559053897857666,0.0,0.0,16660 +0.36220473051071167,0.27559053897857666,0.0,0.0,16680 +0.36220473051071167,0.27559053897857666,0.0,0.0,16700 +0.36220473051071167,0.27559053897857666,0.0,0.0,16720 +0.36220473051071167,0.27559053897857666,0.0,0.0,16740 +0.36220473051071167,0.27559053897857666,0.0,0.0,16760 +0.36220473051071167,0.27559053897857666,0.0,0.0,16780 +0.36220473051071167,0.27559053897857666,0.0,0.0,16800 +0.36220473051071167,0.27559053897857666,0.0,0.0,16820 +0.36220473051071167,0.27559053897857666,0.0,0.0,16840 +0.36220473051071167,0.27559053897857666,0.0,0.0,16860 +0.36220473051071167,0.27559053897857666,0.0,0.0,16880 +0.36220473051071167,0.27559053897857666,0.0,0.0,16900 +0.36220473051071167,0.27559053897857666,0.0,0.0,16920 +0.36220473051071167,0.23622047901153564,0.0,0.0,16940 +0.29133859276771545,0.18110236525535583,0.0,0.0,16960 +0.18897637724876404,0.13385826349258423,0.0,0.0,16980 +0.0,0.0,0.0,0.0,17000 +0.0,0.0,0.0,0.0,17020 +0.0,0.0,0.0,0.0,17040 +0.0,0.0,0.0,0.0,17060 +0.0,0.0,0.0,0.0,17080 +0.0,0.0,0.0,0.0,17100 +0.0,0.0,0.0,0.0,17120 +0.0,0.0,0.0,0.0,17140 +0.0,0.0,0.0,0.0,17160 +0.0,0.0,0.0,0.0,17180 +0.0,0.0,0.0,0.0,17200 +0.0,0.0,0.0,0.0,17220 +0.0,0.0,0.0,0.0,17240 +0.0,0.0,0.0,0.0,17260 +0.0,0.0,0.0,0.0,17280 +0.0,0.0,0.0,0.0,17300 +0.0,0.0,0.0,0.0,17320 +0.0,0.0,0.0,0.0,17341 +0.0,0.0,0.0,0.0,17360 +0.0,0.0,0.0,0.0,17380 +0.0,0.0,0.0,0.0,17400 +0.0,0.0,0.0,0.0,17420 +0.0,0.0,0.0,0.0,17440 +0.0,0.0,0.0,0.0,17460 +0.0,0.0,0.0,0.0,17480 +0.0,0.0,0.0,0.0,17500 +0.0,0.0,0.0,0.0,17520 +0.0,0.0,0.0,0.0,17540 +0.0,0.0,0.0,0.0,17560 +0.0,0.0,0.0,0.0,17580 +0.0,0.0,0.0,0.0,17600 +0.0,0.0,0.0,0.0,17620 +0.0,0.0,0.0,0.0,17642 +0.0,0.0,0.0,0.0,17660 +0.0,0.0,0.0,0.0,17680 +0.0,0.0,0.0,0.0,17700 \ No newline at end of file From b95e32a5ccc3bd0cb8f43e32b0a4f081c7b637d9 Mon Sep 17 00:00:00 2001 From: Abhi <90010729+Abhishrek05@users.noreply.github.com> Date: Thu, 16 Feb 2023 18:59:19 -0700 Subject: [PATCH 15/16] commit (incomplete) --- src/main/java/frc4388/robot/Robot.java | 26 +++++++++++++++++-- .../robot/commands/JoystickRecorder.java | 8 +++++- 2 files changed, 31 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 2d6f348..79d9ade 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -10,6 +10,7 @@ package frc4388.robot; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc4388.robot.commands.JoystickRecorder; import frc4388.utility.RobotTime; /** @@ -25,6 +26,8 @@ public class Robot extends TimedRobot { private RobotTime m_robotTime = RobotTime.getInstance(); private RobotContainer m_robotContainer; + private JoystickRecorder m_joystickRecorder; + Runnable recorder; /** * This function is run when the robot is first started up and should be @@ -93,6 +96,9 @@ public class Robot extends TimedRobot { @Override public void teleopInit() { + + System.out.println("TELEOP INIT STARTING"); + m_robotContainer.m_robotSwerveDrive.resetGyro(); // This makes sure that the autonomous stops running when // teleop starts running. If you want the autonomous to @@ -103,14 +109,30 @@ public class Robot extends TimedRobot { } m_robotContainer.m_robotSwerveDrive.resetGyro(); m_robotTime.startMatchTime(); + + m_joystickRecorder = new JoystickRecorder(m_robotContainer.m_robotSwerveDrive, + () -> m_robotContainer.getDeadbandedDriverController().getLeftX(), + () -> m_robotContainer.getDeadbandedDriverController().getLeftY(), + () -> m_robotContainer.getDeadbandedDriverController().getRightX(), + () -> m_robotContainer.getDeadbandedDriverController().getRightY() + ); + + m_joystickRecorder.initialize(); + + // recorder = () -> { + // m_joystickRecorder.execute(); + // }; + + addPeriodic(() -> { + m_joystickRecorder.execute(); + }, 0.1); } /** * This function is called periodically during operator control. */ @Override - public void teleopPeriodic() { - } + public void teleopPeriodic() {} /** * This function is called periodically during test mode. diff --git a/src/main/java/frc4388/robot/commands/JoystickRecorder.java b/src/main/java/frc4388/robot/commands/JoystickRecorder.java index 2b3fd24..f3b1f53 100644 --- a/src/main/java/frc4388/robot/commands/JoystickRecorder.java +++ b/src/main/java/frc4388/robot/commands/JoystickRecorder.java @@ -45,6 +45,8 @@ public class JoystickRecorder extends CommandBase { this.startTime = System.currentTimeMillis(); outputs.add(new TimedOutput()); + + System.out.println("STARTING RECORDING"); } // Called every time the scheduler runs while the command is scheduled. @@ -62,11 +64,15 @@ public class JoystickRecorder extends CommandBase { swerve.driveWithInput(new Translation2d(inputs.leftX, inputs.leftY), new Translation2d(inputs.rightX, inputs.rightY), true); + + System.out.println("RECORDING IN PROGRESS"); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { + System.out.println("RECORDING ENDED"); + File output = new File("/home/lvuser/JoystickInputs.txt"); try (PrintWriter writer = new PrintWriter(output)) { @@ -85,6 +91,6 @@ public class JoystickRecorder extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return ((System.currentTimeMillis() - this.startTime) > 15000); } } From 06e4c282820cdf4fa1277d834c76d3015465ef98 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 16 Feb 2023 19:07:55 -0700 Subject: [PATCH 16/16] Revert "commit (incomplete)" This reverts commit b95e32a5ccc3bd0cb8f43e32b0a4f081c7b637d9. --- src/main/java/frc4388/robot/Robot.java | 26 ++----------------- .../robot/commands/JoystickRecorder.java | 8 +----- 2 files changed, 3 insertions(+), 31 deletions(-) diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 79d9ade..2d6f348 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -10,7 +10,6 @@ package frc4388.robot; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import frc4388.robot.commands.JoystickRecorder; import frc4388.utility.RobotTime; /** @@ -26,8 +25,6 @@ public class Robot extends TimedRobot { private RobotTime m_robotTime = RobotTime.getInstance(); private RobotContainer m_robotContainer; - private JoystickRecorder m_joystickRecorder; - Runnable recorder; /** * This function is run when the robot is first started up and should be @@ -96,9 +93,6 @@ public class Robot extends TimedRobot { @Override public void teleopInit() { - - System.out.println("TELEOP INIT STARTING"); - m_robotContainer.m_robotSwerveDrive.resetGyro(); // This makes sure that the autonomous stops running when // teleop starts running. If you want the autonomous to @@ -109,30 +103,14 @@ public class Robot extends TimedRobot { } m_robotContainer.m_robotSwerveDrive.resetGyro(); m_robotTime.startMatchTime(); - - m_joystickRecorder = new JoystickRecorder(m_robotContainer.m_robotSwerveDrive, - () -> m_robotContainer.getDeadbandedDriverController().getLeftX(), - () -> m_robotContainer.getDeadbandedDriverController().getLeftY(), - () -> m_robotContainer.getDeadbandedDriverController().getRightX(), - () -> m_robotContainer.getDeadbandedDriverController().getRightY() - ); - - m_joystickRecorder.initialize(); - - // recorder = () -> { - // m_joystickRecorder.execute(); - // }; - - addPeriodic(() -> { - m_joystickRecorder.execute(); - }, 0.1); } /** * This function is called periodically during operator control. */ @Override - public void teleopPeriodic() {} + public void teleopPeriodic() { + } /** * This function is called periodically during test mode. diff --git a/src/main/java/frc4388/robot/commands/JoystickRecorder.java b/src/main/java/frc4388/robot/commands/JoystickRecorder.java index f3b1f53..2b3fd24 100644 --- a/src/main/java/frc4388/robot/commands/JoystickRecorder.java +++ b/src/main/java/frc4388/robot/commands/JoystickRecorder.java @@ -45,8 +45,6 @@ public class JoystickRecorder extends CommandBase { this.startTime = System.currentTimeMillis(); outputs.add(new TimedOutput()); - - System.out.println("STARTING RECORDING"); } // Called every time the scheduler runs while the command is scheduled. @@ -64,15 +62,11 @@ public class JoystickRecorder extends CommandBase { swerve.driveWithInput(new Translation2d(inputs.leftX, inputs.leftY), new Translation2d(inputs.rightX, inputs.rightY), true); - - System.out.println("RECORDING IN PROGRESS"); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - System.out.println("RECORDING ENDED"); - File output = new File("/home/lvuser/JoystickInputs.txt"); try (PrintWriter writer = new PrintWriter(output)) { @@ -91,6 +85,6 @@ public class JoystickRecorder extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - return ((System.currentTimeMillis() - this.startTime) > 15000); + return false; } }