From 3a9b2eca5b7e0a06f55864390899220ed41dcab8 Mon Sep 17 00:00:00 2001 From: Abhi Sachi Date: Mon, 6 Feb 2023 19:49:08 -0700 Subject: [PATCH 01/40] auto incomplete --- src/main/java/frc4388/robot/Constants.java | 5 +++-- src/main/java/frc4388/robot/RobotContainer.java | 2 ++ src/main/java/frc4388/utility/AprilTag.java | 13 +++++++++++++ 3 files changed, 18 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc4388/utility/AprilTag.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 1b94ed1..e063825 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -57,8 +57,8 @@ public final class Constants { new TrapezoidProfile.Constraints(0.0, 0.0) ); - public static final double PATH_MAX_VEL = -1; // TODO: find the actual value - public static final double PATH_MAX_ACC = -1; // TODO: find the actual value + public static final double PATH_MAX_VEL = SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND / 4; // TODO: find the actual value + public static final double PATH_MAX_ACC = 3; // TODO: find the actual value } public static final class Conversions { @@ -106,6 +106,7 @@ public final class Constants { } public static final double MAX_SPEED_FEET_PER_SECOND = 5; // TODO: find the actual value + public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; // TODO: find the actual value // dimensions public static final double WIDTH = 18.5; // TODO: find the actual value diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 97f5db1..c6e01c8 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -7,6 +7,7 @@ package frc4388.robot; +import edu.wpi.first.math.trajectory.TrajectoryConfig; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -91,6 +92,7 @@ public class RobotContainer { * @return the command to run in autonomous */ public Command getAutonomousCommand() { + TrajectoryConfig trajectoryConfig = new TrajectoryConfig(SwerveDriveConstants.AutoConstants.PATH_MAX_VEL, SwerveDriveConstants.AutoConstants.PATH_MAX_ACC).setKinematics(m_robotSwerveDrive.getKinematics()); return new InstantCommand(); } diff --git a/src/main/java/frc4388/utility/AprilTag.java b/src/main/java/frc4388/utility/AprilTag.java new file mode 100644 index 0000000..c2ad269 --- /dev/null +++ b/src/main/java/frc4388/utility/AprilTag.java @@ -0,0 +1,13 @@ +package frc4388.utility; + +// This is a seperate class in case I want to encode rotation or other +// information about the tag +public class AprilTag { + public final double x, y, z; + + public AprilTag(double _x, double _y, double _z) { + x = _x; + y = _y; + z = _z; + } +} From 4bfb26a50a0a5ddf610caf343d3f93548d843ab6 Mon Sep 17 00:00:00 2001 From: Abhi Sachi Date: Tue, 7 Feb 2023 18:50:33 -0700 Subject: [PATCH 02/40] auto (not looked over) --- src/main/java/frc4388/robot/Constants.java | 12 +++-- .../java/frc4388/robot/RobotContainer.java | 46 ++++++++++++++++++- .../frc4388/robot/subsystems/SwerveDrive.java | 9 +++- src/main/java/frc4388/utility/Gains.java | 6 +++ 4 files changed, 64 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index e063825..0420217 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -51,14 +51,16 @@ public final class Constants { } public static final class AutoConstants { - public static final PIDController X_CONTROLLER = new PIDController(0.0, 0.0, 0.0); - public static final PIDController Y_CONTROLLER = new PIDController(0.0, 0.0, 0.0); - public static final ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController(0.0, 0.0, 0.0, - new TrapezoidProfile.Constraints(0.0, 0.0) - ); + public static final Gains X_CONTROLLER = new Gains(0.0, 0.0, 0.0); + public static final Gains Y_CONTROLLER = new Gains(0.0, 0.0, 0.0); + public static final Gains THETA_CONTROLLER = new Gains(0.0, 0.0, 0.0); + public static final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints(0.0, 0.0); public static final double PATH_MAX_VEL = SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND / 4; // TODO: find the actual value public static final double PATH_MAX_ACC = 3; // TODO: find the actual value + + public static final double kPX_CONTROLLER = 1.5; //TODO: find actual value + public static final double kPY_CONTROLLER = 1.5; //TODO: find actual value } public static final class Conversions { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index c6e01c8..08f209f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -7,13 +7,26 @@ package frc4388.robot; +import java.util.List; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.trajectory.Trajectory; import edu.wpi.first.math.trajectory.TrajectoryConfig; +import edu.wpi.first.math.trajectory.TrajectoryGenerator; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; +import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants; +import frc4388.robot.Constants.SwerveDriveConstants.PIDConstants; import frc4388.robot.commands.AutoBalance; import frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.controller.DeadbandedXboxController; @@ -92,9 +105,38 @@ public class RobotContainer { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - TrajectoryConfig trajectoryConfig = new TrajectoryConfig(SwerveDriveConstants.AutoConstants.PATH_MAX_VEL, SwerveDriveConstants.AutoConstants.PATH_MAX_ACC).setKinematics(m_robotSwerveDrive.getKinematics()); + //Create Trajectory Settings + TrajectoryConfig trajectoryConfig = new TrajectoryConfig(SwerveDriveConstants.AutoConstants.PATH_MAX_VEL, + SwerveDriveConstants.AutoConstants.PATH_MAX_ACC).setKinematics(m_robotSwerveDrive.getKinematics()); + //Generate Trajactory + Trajectory trajectory = TrajectoryGenerator.generateTrajectory(new Pose2d(0, 0, new Rotation2d(0)), + List.of(new Translation2d(1, 0), new Translation2d(1, -1)), + new Pose2d(0, 0, Rotation2d.fromDegrees(180)), + trajectoryConfig); - return new InstantCommand(); + //Defining PID Controller for tracking trajectory + PIDController xController = new PIDController(SwerveDriveConstants.AutoConstants.kPX_CONTROLLER, 0, 0); + PIDController yController = new PIDController(SwerveDriveConstants.AutoConstants.kPY_CONTROLLER, 0, 0); + ProfiledPIDController thetaController = new ProfiledPIDController(AutoConstants.THETA_CONTROLLER.kP, + AutoConstants.THETA_CONTROLLER.kI, AutoConstants.THETA_CONTROLLER.kD, AutoConstants.THETA_CONSTRAINTS, 0); + thetaController.enableContinuousInput(-Math.PI, Math.PI); + + //Command to follow trajectory + SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand( + trajectory, + m_robotSwerveDrive::getOdometry, + m_robotSwerveDrive.getKinematics(), + xController, + yController, + thetaController, + m_robotSwerveDrive::setModuleStates, + m_robotSwerveDrive); + + //Init and wrap-up, return everything + return new SequentialCommandGroup( + new InstantCommand(() -> m_robotSwerveDrive.setOdometry(trajectory.getInitialPose())), + swerveControllerCommand, + new InstantCommand(() -> m_robotSwerveDrive.stopModules())); } /** diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 38e7415..6cf7ba8 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -68,14 +68,12 @@ public class SwerveDrive extends SubsystemBase { public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { if (fieldRelative) { - if (rightStick.getNorm() > 0.1) { rotTarget = new Rotation2d(rightStick.getX(), -rightStick.getY()).minus(new Rotation2d(0, 1)); } double rot = rotTarget.minus(gyro.getRotation2d()).getRadians(); - // Use the left joystick to set speed. Apply a quadratic curve and the set max speed. Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); @@ -105,6 +103,13 @@ public class SwerveDrive extends SubsystemBase { } } + public void stopModules() { + leftFront.stop(); + rightFront.stop(); + leftBack.stop(); + rightBack.stop(); + } + public double getGyroAngle() { return gyro.getAngle(); } diff --git a/src/main/java/frc4388/utility/Gains.java b/src/main/java/frc4388/utility/Gains.java index 7cda1e0..7a3a026 100644 --- a/src/main/java/frc4388/utility/Gains.java +++ b/src/main/java/frc4388/utility/Gains.java @@ -54,6 +54,12 @@ public class Gains { this.kMinOutput = -1.0; } + public Gains(double kP, double kI, double kD) { + this.kP = kP; + this.kI = kI; + this.kD = kD; + } + /** * Creates Gains object for PIDs * @param kP The P value. From 4900c397c4506f3dd3a603da0b8339b17306862f Mon Sep 17 00:00:00 2001 From: aarav18 Date: Tue, 7 Feb 2023 23:32:19 -0700 Subject: [PATCH 03/40] some auto work (not working at all) --- src/main/java/frc4388/robot/Constants.java | 16 +++++------ .../java/frc4388/robot/RobotContainer.java | 28 ++++++++++++++----- .../frc4388/robot/subsystems/SwerveDrive.java | 10 +++---- 3 files changed, 34 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 0420217..01f10e6 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -51,16 +51,16 @@ public final class Constants { } public static final class AutoConstants { - public static final Gains X_CONTROLLER = new Gains(0.0, 0.0, 0.0); - public static final Gains Y_CONTROLLER = new Gains(0.0, 0.0, 0.0); - public static final Gains THETA_CONTROLLER = new Gains(0.0, 0.0, 0.0); - public static final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints(0.0, 0.0); + public static final Gains X_CONTROLLER = new Gains(0.8, 0.0, 0.0); + public static final Gains Y_CONTROLLER = new Gains(0.8, 0.0, 0.0); + public static final Gains THETA_CONTROLLER = new Gains(0.8, 0.0, 0.0); + public static final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints(Math.PI/2, Math.PI/2); // TODO: tune - public static final double PATH_MAX_VEL = SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND / 4; // TODO: find the actual value - public static final double PATH_MAX_ACC = 3; // TODO: find the actual value + public static final double PATH_MAX_VEL = 0.3; // TODO: find the actual value + public static final double PATH_MAX_ACC = 0.3; // TODO: find the actual value - public static final double kPX_CONTROLLER = 1.5; //TODO: find actual value - public static final double kPY_CONTROLLER = 1.5; //TODO: find actual value + // public static final double kPX_CONTROLLER = 0.8; //TODO: tune + // public static final double kPY_CONTROLLER = 0.8; //TODO: tune } public static final class Conversions { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 08f209f..37158ad 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -7,6 +7,7 @@ package frc4388.robot; +import java.util.ArrayList; import java.util.List; import edu.wpi.first.math.controller.PIDController; @@ -17,6 +18,7 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.trajectory.Trajectory; import edu.wpi.first.math.trajectory.TrajectoryConfig; import edu.wpi.first.math.trajectory.TrajectoryGenerator; +import edu.wpi.first.math.trajectory.Trajectory.State; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -105,20 +107,32 @@ public class RobotContainer { * @return the command to run in autonomous */ public Command getAutonomousCommand() { + //Create Trajectory Settings TrajectoryConfig trajectoryConfig = new TrajectoryConfig(SwerveDriveConstants.AutoConstants.PATH_MAX_VEL, SwerveDriveConstants.AutoConstants.PATH_MAX_ACC).setKinematics(m_robotSwerveDrive.getKinematics()); - //Generate Trajactory - Trajectory trajectory = TrajectoryGenerator.generateTrajectory(new Pose2d(0, 0, new Rotation2d(0)), - List.of(new Translation2d(1, 0), new Translation2d(1, -1)), - new Pose2d(0, 0, Rotation2d.fromDegrees(180)), + + // generate trajectory + Trajectory trajectory = TrajectoryGenerator.generateTrajectory( + + new Pose2d(0, 0, new Rotation2d(0)), + List.of( + new Translation2d(0, 1)), + new Pose2d(0, 2, Rotation2d.fromDegrees(0)), + trajectoryConfig); + // ArrayList simplePath = new ArrayList(); + // simplePath.add(new Pose2d(0, 0, new Rotation2d(0))); + // simplePath.add(new Pose2d(0, 1, new Rotation2d(0))); + + // Trajectory trajectory = TrajectoryGenerator.generateTrajectory(simplePath, trajectoryConfig); + //Defining PID Controller for tracking trajectory - PIDController xController = new PIDController(SwerveDriveConstants.AutoConstants.kPX_CONTROLLER, 0, 0); - PIDController yController = new PIDController(SwerveDriveConstants.AutoConstants.kPY_CONTROLLER, 0, 0); + PIDController xController = new PIDController(SwerveDriveConstants.AutoConstants.X_CONTROLLER.kP, 0, 0); + PIDController yController = new PIDController(SwerveDriveConstants.AutoConstants.Y_CONTROLLER.kP, 0, 0); ProfiledPIDController thetaController = new ProfiledPIDController(AutoConstants.THETA_CONTROLLER.kP, - AutoConstants.THETA_CONTROLLER.kI, AutoConstants.THETA_CONTROLLER.kD, AutoConstants.THETA_CONSTRAINTS, 0); + AutoConstants.THETA_CONTROLLER.kI, AutoConstants.THETA_CONTROLLER.kD, AutoConstants.THETA_CONSTRAINTS); thetaController.enableContinuousInput(-Math.PI, Math.PI); //Command to follow trajectory diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 6cf7ba8..5790496 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -77,13 +77,10 @@ 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; - } - // 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); } @@ -178,7 +175,10 @@ public class SwerveDrive extends SubsystemBase { updateOdometry(); // SmartDashboard.putNumberArray("Odometry", new double[] {getOdometry().getX(), getOdometry().getY(), getOdometry().getRotation().getDegrees()}); - + SmartDashboard.putNumber("Odo X", getOdometry().getX()); + SmartDashboard.putNumber("Odo Y", getOdometry().getY()); + SmartDashboard.putNumber("Odo Theta", getOdometry().getRotation().getDegrees()); + SmartDashboard.putNumber("Gyro Angle", getGyroAngle()); } From 867e16f0a722944138553fd37eca8d7ce34471c0 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Wed, 8 Feb 2023 00:35:10 -0700 Subject: [PATCH 04/40] 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 5a8af1d071cd52b73d9b085fc698fdf260e5a93e Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 9 Feb 2023 18:25:57 -0700 Subject: [PATCH 05/40] cummit --- src/main/java/frc4388/robot/Constants.java | 7 ++---- .../java/frc4388/robot/RobotContainer.java | 24 ++++++++++--------- .../robot/subsystems/SwerveModule.java | 4 ++-- 3 files changed, 17 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 01f10e6..fbb267e 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -47,20 +47,17 @@ public final class Constants { public static final class PIDConstants { public static final int SWERVE_SLOT_IDX = 0; public static final int SWERVE_PID_LOOP_IDX = 1; - public static final Gains SWERVE_GAINS = new Gains(1.0, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains SWERVE_GAINS = new Gains(0.5, 0.0, 0.0, 0.0, 0, 1.0); } public static final class AutoConstants { public static final Gains X_CONTROLLER = new Gains(0.8, 0.0, 0.0); public static final Gains Y_CONTROLLER = new Gains(0.8, 0.0, 0.0); - public static final Gains THETA_CONTROLLER = new Gains(0.8, 0.0, 0.0); + public static final Gains THETA_CONTROLLER = new Gains(-0.8, 0.0, 0.0); public static final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints(Math.PI/2, Math.PI/2); // TODO: tune public static final double PATH_MAX_VEL = 0.3; // TODO: find the actual value public static final double PATH_MAX_ACC = 0.3; // TODO: find the actual value - - // public static final double kPX_CONTROLLER = 0.8; //TODO: tune - // public static final double kPY_CONTROLLER = 0.8; //TODO: tune } public static final class Conversions { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 37158ad..364b3cc 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -10,6 +10,7 @@ package frc4388.robot; import java.util.ArrayList; import java.util.List; +import edu.wpi.first.math.controller.HolonomicDriveController; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Pose2d; @@ -113,20 +114,20 @@ public class RobotContainer { SwerveDriveConstants.AutoConstants.PATH_MAX_ACC).setKinematics(m_robotSwerveDrive.getKinematics()); // generate trajectory - Trajectory trajectory = TrajectoryGenerator.generateTrajectory( + // Trajectory trajectory = TrajectoryGenerator.generateTrajectory( - new Pose2d(0, 0, new Rotation2d(0)), - List.of( - new Translation2d(0, 1)), - new Pose2d(0, 2, Rotation2d.fromDegrees(0)), + // new Pose2d(0, 0, new Rotation2d(0)), + // List.of( + // new Translation2d(0, 1)), + // new Pose2d(0, 2, Rotation2d.fromDegrees(0)), - trajectoryConfig); + // trajectoryConfig); - // ArrayList simplePath = new ArrayList(); - // simplePath.add(new Pose2d(0, 0, new Rotation2d(0))); - // simplePath.add(new Pose2d(0, 1, new Rotation2d(0))); + ArrayList simplePath = new ArrayList(); + simplePath.add(new Pose2d(0, 0, new Rotation2d(0))); + simplePath.add(new Pose2d(0, 1, new Rotation2d(0))); - // Trajectory trajectory = TrajectoryGenerator.generateTrajectory(simplePath, trajectoryConfig); + Trajectory trajectory = TrajectoryGenerator.generateTrajectory(simplePath, trajectoryConfig); //Defining PID Controller for tracking trajectory PIDController xController = new PIDController(SwerveDriveConstants.AutoConstants.X_CONTROLLER.kP, 0, 0); @@ -148,7 +149,8 @@ public class RobotContainer { //Init and wrap-up, return everything return new SequentialCommandGroup( - new InstantCommand(() -> m_robotSwerveDrive.setOdometry(trajectory.getInitialPose())), + new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(), m_robotSwerveDrive), + // new InstantCommand(() -> m_robotSwerveDrive.setOdometry(trajectory.getInitialPose())), swerveControllerCommand, new InstantCommand(() -> m_robotSwerveDrive.stopModules())); } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 2616b71..a88fca7 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -132,13 +132,13 @@ public class SwerveModule extends SubsystemBase { SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation); // calculate the difference between our current rotational position and our new rotational position - Rotation2d rotationDelta = state.angle.minus(currentRotation); // ? might need to be negative + Rotation2d rotationDelta = state.angle.minus(currentRotation); // calculate the new absolute position of the SwerveModule based on the difference in rotation double deltaTicks = (rotationDelta.getDegrees() / 360.) * SwerveDriveConstants.Conversions.CANCODER_TICKS_PER_ROTATION; // convert the CANCoder from its position reading to ticks - double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient(); // ? why feedback coefficient + double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient(); if (!ignoreAngle) { angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks); From d80b4faccbbd7d3d83ef34641fb87029044684e3 Mon Sep 17 00:00:00 2001 From: Abhi Sachi Date: Thu, 9 Feb 2023 18:48:43 -0700 Subject: [PATCH 06/40] auto incomplete --- .../java/frc4388/robot/RobotContainer.java | 18 ++++++++++++++---- 1 file changed, 14 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 364b3cc..429f3f3 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -10,6 +10,8 @@ package frc4388.robot; import java.util.ArrayList; import java.util.List; +import org.opencv.objdetect.HOGDescriptor; + import edu.wpi.first.math.controller.HolonomicDriveController; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; @@ -21,6 +23,7 @@ import edu.wpi.first.math.trajectory.TrajectoryConfig; import edu.wpi.first.math.trajectory.TrajectoryGenerator; import edu.wpi.first.math.trajectory.Trajectory.State; import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; @@ -125,10 +128,17 @@ public class RobotContainer { ArrayList simplePath = new ArrayList(); simplePath.add(new Pose2d(0, 0, new Rotation2d(0))); - simplePath.add(new Pose2d(0, 1, new Rotation2d(0))); + simplePath.add(new Pose2d(0, -1, new Rotation2d(0))); Trajectory trajectory = TrajectoryGenerator.generateTrajectory(simplePath, trajectoryConfig); + ArrayList states = new ArrayList(); + + for(double i = 0; i <=5.0; i += 0.1) { + Trajectory.State state = trajectory.sample(i); + states.add(new double[] {state.velocityMetersPerSecond, state.curvatureRadPerMeter}); + } + //Defining PID Controller for tracking trajectory PIDController xController = new PIDController(SwerveDriveConstants.AutoConstants.X_CONTROLLER.kP, 0, 0); PIDController yController = new PIDController(SwerveDriveConstants.AutoConstants.Y_CONTROLLER.kP, 0, 0); @@ -136,14 +146,14 @@ public class RobotContainer { AutoConstants.THETA_CONTROLLER.kI, AutoConstants.THETA_CONTROLLER.kD, AutoConstants.THETA_CONSTRAINTS); thetaController.enableContinuousInput(-Math.PI, Math.PI); + HolonomicDriveController holoController = new HolonomicDriveController(xController, yController, thetaController); + //Command to follow trajectory SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand( trajectory, m_robotSwerveDrive::getOdometry, m_robotSwerveDrive.getKinematics(), - xController, - yController, - thetaController, + holoController, m_robotSwerveDrive::setModuleStates, m_robotSwerveDrive); From e7af2c928190d4f5c27593ade3e67e4c68c1d2e9 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 9 Feb 2023 19:34:59 -0700 Subject: [PATCH 07/40] Update RobotContainer.java --- .../java/frc4388/robot/RobotContainer.java | 28 +++++++++++-------- 1 file changed, 16 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 429f3f3..4058926 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -117,27 +117,31 @@ public class RobotContainer { SwerveDriveConstants.AutoConstants.PATH_MAX_ACC).setKinematics(m_robotSwerveDrive.getKinematics()); // generate trajectory - // Trajectory trajectory = TrajectoryGenerator.generateTrajectory( + Trajectory trajectory = TrajectoryGenerator.generateTrajectory( - // new Pose2d(0, 0, new Rotation2d(0)), - // List.of( - // new Translation2d(0, 1)), - // new Pose2d(0, 2, Rotation2d.fromDegrees(0)), + new Pose2d(0, 0, new Rotation2d(0)), + List.of( + new Translation2d(0, 1)), + new Pose2d(0, 2, Rotation2d.fromDegrees(0)), - // trajectoryConfig); + trajectoryConfig); ArrayList simplePath = new ArrayList(); simplePath.add(new Pose2d(0, 0, new Rotation2d(0))); simplePath.add(new Pose2d(0, -1, new Rotation2d(0))); - Trajectory trajectory = TrajectoryGenerator.generateTrajectory(simplePath, trajectoryConfig); + // Trajectory trajectory = TrajectoryGenerator.generateTrajectory(simplePath, trajectoryConfig); + // Trajectory trajectory = TrajectoryGenerator.generateTrajectory( + // new Pose2d(0, 0, new Rotation2d(0), + // List.of(new Translation2d(0, 1)), + // new Pose2d(0, 2, new Rotation2d(0)), + // trajectoryConfig + // ); + + + // ArrayList states = new ArrayList(); - ArrayList states = new ArrayList(); - for(double i = 0; i <=5.0; i += 0.1) { - Trajectory.State state = trajectory.sample(i); - states.add(new double[] {state.velocityMetersPerSecond, state.curvatureRadPerMeter}); - } //Defining PID Controller for tracking trajectory PIDController xController = new PIDController(SwerveDriveConstants.AutoConstants.X_CONTROLLER.kP, 0, 0); From 7c57dd53e8eceace2c400475e5a1ed91845e5fdc Mon Sep 17 00:00:00 2001 From: Abhi <90010729+Abhishrek05@users.noreply.github.com> Date: Fri, 10 Feb 2023 17:12:53 -0700 Subject: [PATCH 08/40] added pose estimator --- .../java/frc4388/robot/RobotContainer.java | 16 +++- .../frc4388/robot/subsystems/SwerveDrive.java | 89 ++++++++++++++----- 2 files changed, 81 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 4058926..14969a8 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -92,8 +92,8 @@ public class RobotContainer { 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)); + // new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(), m_robotSwerveDrive)); // // .onFalse() new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) @@ -153,9 +153,17 @@ public class RobotContainer { HolonomicDriveController holoController = new HolonomicDriveController(xController, yController, thetaController); //Command to follow trajectory + // SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand( + // trajectory, + // m_robotSwerveDrive::getOdometry, + // m_robotSwerveDrive.getKinematics(), + // holoController, + // m_robotSwerveDrive::setModuleStates, + // m_robotSwerveDrive); + SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand( trajectory, - m_robotSwerveDrive::getOdometry, + m_robotSwerveDrive::getPoseEstimator, m_robotSwerveDrive.getKinematics(), holoController, m_robotSwerveDrive::setModuleStates, @@ -163,7 +171,7 @@ public class RobotContainer { //Init and wrap-up, return everything return new SequentialCommandGroup( - new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(), m_robotSwerveDrive), + new InstantCommand(() -> m_robotSwerveDrive.resetPoseEstimator(), m_robotSwerveDrive), // new InstantCommand(() -> m_robotSwerveDrive.setOdometry(trajectory.getInitialPose())), swerveControllerCommand, new InstantCommand(() -> m_robotSwerveDrive.stopModules())); diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 5790496..c174c29 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -5,6 +5,7 @@ package frc4388.robot.subsystems; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -37,7 +38,9 @@ public class SwerveDrive extends SubsystemBase { private RobotGyro gyro; - private SwerveDriveOdometry odometry; + // private SwerveDriveOdometry odometry; + + private SwerveDrivePoseEstimator poseEstimator; public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default public Rotation2d rotTarget = new Rotation2d(); @@ -51,15 +54,28 @@ public class SwerveDrive extends SubsystemBase { this.rightBack = rightBack; this.gyro = gyro; - this.odometry = new SwerveDriveOdometry( + // this.odometry = new SwerveDriveOdometry( + // kinematics, + // gyro.getRotation2d(), + // new SwerveModulePosition[] { + // leftFront.getPosition(), + // rightFront.getPosition(), + // leftBack.getPosition(), + // rightBack.getPosition() + // }, + // getOdometry() + // ); + + this.poseEstimator = new SwerveDrivePoseEstimator( kinematics, - gyro.getRotation2d(), + gyro.getRotation2d(), new SwerveModulePosition[] { leftFront.getPosition(), rightFront.getPosition(), leftBack.getPosition(), rightBack.getPosition() - } + }, + new Pose2d(0,0, new Rotation2d(0)) ); this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack}; @@ -113,15 +129,27 @@ public class SwerveDrive extends SubsystemBase { public void resetGyro() { gyro.reset(); - setOdometry(getOdometry()); + // setOdometry(getOdometry()); rotTarget = new Rotation2d(0); } /** * Updates the odometry of the SwerveDrive. */ - public void updateOdometry() { - odometry.update( + // public void updateOdometry() { + // odometry.update( + // gyro.getRotation2d(), + // new SwerveModulePosition[] { + // leftFront.getPosition(), + // rightFront.getPosition(), + // leftBack.getPosition(), + // rightBack.getPosition() + // } + // ); + // } + + public void updatePoseEstimator() { + poseEstimator.update( gyro.getRotation2d(), new SwerveModulePosition[] { leftFront.getPosition(), @@ -136,16 +164,33 @@ public class SwerveDrive extends SubsystemBase { * Gets the odometry of the SwerveDrive. * @return The odometry of the SwerveDrive as a Pose2d object (xMeters, yMeters, theta). */ - public Pose2d getOdometry() { - return odometry.getPoseMeters(); + // public Pose2d getOdometry() { + // return odometry.getPoseMeters(); + // } + + public Pose2d getPoseEstimator() { + return poseEstimator.getEstimatedPosition(); } /** * Sets the odometry of the SwerveDrive. * @param pose Pose to set the odometry to. */ - public void setOdometry(Pose2d pose) { - odometry.resetPosition( + // public void setOdometry(Pose2d pose) { + // odometry.resetPosition( + // gyro.getRotation2d(), + // new SwerveModulePosition[] { + // leftFront.getPosition(), + // rightFront.getPosition(), + // leftBack.getPosition(), + // rightBack.getPosition() + // }, + // pose + // ); + // } + + public void setPoseEstimator(Pose2d pose) { + poseEstimator.resetPosition( gyro.getRotation2d(), new SwerveModulePosition[] { leftFront.getPosition(), @@ -157,13 +202,17 @@ public class SwerveDrive extends SubsystemBase { ); } + public void resetPoseEstimator() { + setPoseEstimator(new Pose2d()); + } + /** * 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. */ - public void resetOdometry() { - setOdometry(new Pose2d()); - } + // public void resetOdometry() { + // setOdometry(new Pose2d()); + // } public SwerveDriveKinematics getKinematics() { return this.kinematics; @@ -172,14 +221,14 @@ public class SwerveDrive extends SubsystemBase { @Override public void periodic() { // This method will be called once per scheduler run - updateOdometry(); + // updateOdometry(); + updatePoseEstimator(); - // SmartDashboard.putNumberArray("Odometry", new double[] {getOdometry().getX(), getOdometry().getY(), getOdometry().getRotation().getDegrees()}); - SmartDashboard.putNumber("Odo X", getOdometry().getX()); - SmartDashboard.putNumber("Odo Y", getOdometry().getY()); - SmartDashboard.putNumber("Odo Theta", getOdometry().getRotation().getDegrees()); + // SmartDashboard.putNumber("Odo X", getOdometry().getX()); + // SmartDashboard.putNumber("Odo Y", getOdometry().getY()); + // SmartDashboard.putNumber("Odo Theta", getOdometry().getRotation().getDegrees()); - SmartDashboard.putNumber("Gyro Angle", getGyroAngle()); + // SmartDashboard.putNumber("Gyro Angle", getGyroAngle()); } /** From c9a4274c82b009e4dd4c388790c04e4b72b54b48 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 10 Feb 2023 17:33:19 -0700 Subject: [PATCH 09/40] 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 10/40] 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 11/40] 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 12/40] 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 13/40] 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 14/40] 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 15/40] 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 16/40] 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 17/40] 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 18/40] 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 19/40] 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 20/40] 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; } } From 49107c6e143efd1fd40e0c543bd0011df71ed70f Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 17 Feb 2023 20:00:17 -0700 Subject: [PATCH 21/40] fix build issue --- .../frc4388/robot/subsystems/SwerveDrive.java | 17 +++++------------ 1 file changed, 5 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index ddd5300..0b13b6c 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -116,13 +116,6 @@ public class SwerveDrive extends SubsystemBase { } } - public void stopModules() { - leftFront.stop(); - rightFront.stop(); - leftBack.stop(); - rightBack.stop(); - } - public double getGyroAngle() { return gyro.getAngle(); } @@ -231,12 +224,12 @@ public class SwerveDrive extends SubsystemBase { // updateOdometry(); updatePoseEstimator(); - SmartDashboard.putNumber("Odo X (ft)", Units.metersToFeet(this.getOdometry().getX())); - SmartDashboard.putNumber("Odo Y (ft)", Units.metersToFeet(this.getOdometry().getY())); - SmartDashboard.putNumber("Odo Theta", this.getOdometry().getRotation().getDegrees()); + // SmartDashboard.putNumber("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()); + // SmartDashboard.putNumber("Gyro Angle", getGyroAngle()); + // SmartDashboard.putNumber("rotTarget", this.rotTarget.getDegrees()); } From a9d088c7c99089ffd6ef3a9ec41782fb07bf62b9 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 25 Feb 2023 08:52:38 -0700 Subject: [PATCH 22/40] real auto --- .../java/frc4388/robot/RobotContainer.java | 64 +------------------ 1 file changed, 1 insertion(+), 63 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index e4e0b39..2982bf5 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -117,69 +117,7 @@ public class RobotContainer { */ public Command getAutonomousCommand() { - //Create Trajectory Settings - TrajectoryConfig trajectoryConfig = new TrajectoryConfig(SwerveDriveConstants.AutoConstants.PATH_MAX_VEL, - SwerveDriveConstants.AutoConstants.PATH_MAX_ACC).setKinematics(m_robotSwerveDrive.getKinematics()); - - // generate trajectory - Trajectory trajectory = TrajectoryGenerator.generateTrajectory( - - new Pose2d(0, 0, new Rotation2d(0)), - List.of( - new Translation2d(0, 1)), - new Pose2d(0, 2, Rotation2d.fromDegrees(0)), - - trajectoryConfig); - - ArrayList simplePath = new ArrayList(); - simplePath.add(new Pose2d(0, 0, new Rotation2d(0))); - simplePath.add(new Pose2d(0, -1, new Rotation2d(0))); - - // Trajectory trajectory = TrajectoryGenerator.generateTrajectory(simplePath, trajectoryConfig); - // Trajectory trajectory = TrajectoryGenerator.generateTrajectory( - // new Pose2d(0, 0, new Rotation2d(0), - // List.of(new Translation2d(0, 1)), - // new Pose2d(0, 2, new Rotation2d(0)), - // trajectoryConfig - // ); - - - // ArrayList states = new ArrayList(); - - - - //Defining PID Controller for tracking trajectory - PIDController xController = new PIDController(SwerveDriveConstants.AutoConstants.X_CONTROLLER.kP, 0, 0); - PIDController yController = new PIDController(SwerveDriveConstants.AutoConstants.Y_CONTROLLER.kP, 0, 0); - ProfiledPIDController thetaController = new ProfiledPIDController(AutoConstants.THETA_CONTROLLER.kP, - AutoConstants.THETA_CONTROLLER.kI, AutoConstants.THETA_CONTROLLER.kD, AutoConstants.THETA_CONSTRAINTS); - thetaController.enableContinuousInput(-Math.PI, Math.PI); - - HolonomicDriveController holoController = new HolonomicDriveController(xController, yController, thetaController); - - //Command to follow trajectory - // SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand( - // trajectory, - // m_robotSwerveDrive::getOdometry, - // m_robotSwerveDrive.getKinematics(), - // holoController, - // m_robotSwerveDrive::setModuleStates, - // m_robotSwerveDrive); - - SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand( - trajectory, - m_robotSwerveDrive::getPoseEstimator, - m_robotSwerveDrive.getKinematics(), - holoController, - m_robotSwerveDrive::setModuleStates, - m_robotSwerveDrive); - - //Init and wrap-up, return everything - return new SequentialCommandGroup( - new InstantCommand(() -> m_robotSwerveDrive.resetPoseEstimator(), m_robotSwerveDrive), - // new InstantCommand(() -> m_robotSwerveDrive.setOdometry(trajectory.getInitialPose())), - swerveControllerCommand, - new InstantCommand(() -> m_robotSwerveDrive.stopModules())); + return new JoystickPlayback(m_robotSwerveDrive); } public DeadbandedXboxController getDeadbandedDriverController() { From b32a0bf28212c05da0305ed7d592561cd52777aa Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 25 Feb 2023 09:52:03 -0700 Subject: [PATCH 23/40] blueneartodrivestation --- .../commands/BlueNearDriveToChargeStation.txt | 0 .../robot/commands/FarDriveToChargeStation | 0 .../frc4388/robot/commands/JoystickInputs.txt | 887 ------------------ .../robot/commands/JoystickPlayback.java | 2 +- .../robot/commands/JoystickRecorder.java | 2 +- 5 files changed, 2 insertions(+), 889 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/BlueNearDriveToChargeStation.txt create mode 100644 src/main/java/frc4388/robot/commands/FarDriveToChargeStation delete mode 100644 src/main/java/frc4388/robot/commands/JoystickInputs.txt diff --git a/src/main/java/frc4388/robot/commands/BlueNearDriveToChargeStation.txt b/src/main/java/frc4388/robot/commands/BlueNearDriveToChargeStation.txt new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/frc4388/robot/commands/FarDriveToChargeStation b/src/main/java/frc4388/robot/commands/FarDriveToChargeStation new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/frc4388/robot/commands/JoystickInputs.txt b/src/main/java/frc4388/robot/commands/JoystickInputs.txt deleted file mode 100644 index b503e48..0000000 --- a/src/main/java/frc4388/robot/commands/JoystickInputs.txt +++ /dev/null @@ -1,887 +0,0 @@ -0.0,0.0,0.0,0.0,0 -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 diff --git a/src/main/java/frc4388/robot/commands/JoystickPlayback.java b/src/main/java/frc4388/robot/commands/JoystickPlayback.java index 6961d09..f8dfb82 100644 --- a/src/main/java/frc4388/robot/commands/JoystickPlayback.java +++ b/src/main/java/frc4388/robot/commands/JoystickPlayback.java @@ -36,7 +36,7 @@ public class JoystickPlayback extends CommandBase { playbackTime = 0; lastIndex = 0; try { - input = new Scanner(new File("/home/lvuser/JoystickInputs.txt")); + input = new Scanner(new File("/home/lvuser/BlueNearDriveToChargeStation.txt")); String line = ""; while (input.hasNextLine()) { diff --git a/src/main/java/frc4388/robot/commands/JoystickRecorder.java b/src/main/java/frc4388/robot/commands/JoystickRecorder.java index 2b3fd24..0682c72 100644 --- a/src/main/java/frc4388/robot/commands/JoystickRecorder.java +++ b/src/main/java/frc4388/robot/commands/JoystickRecorder.java @@ -67,7 +67,7 @@ public class JoystickRecorder extends CommandBase { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - File output = new File("/home/lvuser/JoystickInputs.txt"); + File output = new File("/home/lvuser/BlueNearDriveToChargeStation.txt"); try (PrintWriter writer = new PrintWriter(output)) { for (var input : outputs) { From 9c003020fdec4c26700eb010477f340355120a5b Mon Sep 17 00:00:00 2001 From: Abhi <90010729+Abhishrek05@users.noreply.github.com> Date: Sat, 25 Feb 2023 09:52:33 -0700 Subject: [PATCH 24/40] yes --- src/main/java/frc4388/robot/commands/AutoBalance.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/AutoBalance.java b/src/main/java/frc4388/robot/commands/AutoBalance.java index ffccee4..714221d 100644 --- a/src/main/java/frc4388/robot/commands/AutoBalance.java +++ b/src/main/java/frc4388/robot/commands/AutoBalance.java @@ -26,7 +26,7 @@ public class AutoBalance extends PelvicInflammatoryDisease { @Override public double getError() { - var pitch = gyro.getPitch(); + var pitch = gyro.getRoll(); SmartDashboard.putNumber("pitch", pitch); return pitch; } @@ -34,8 +34,8 @@ public class AutoBalance extends PelvicInflammatoryDisease { @Override public void runWithOutput(double output) { double out2 = MathUtil.clamp(output / 40, -.5, .5); - if (Math.abs(gyro.getPitch()) < 3) out2 = 0; - drive.driveWithInput(new Translation2d(0, out2), new Translation2d(), false); + if (Math.abs(getError()) < 3) out2 = 0; + drive.driveWithInput(new Translation2d(0, out2), new Translation2d(), true); } @Override From de972faaeed4ddf4102d03cfa688d86acf2b782a Mon Sep 17 00:00:00 2001 From: Aarav Date: Sat, 25 Feb 2023 12:30:20 -0700 Subject: [PATCH 25/40] auto shtuff --- src/main/java/frc4388/robot/Robot.java | 779 ++++++++++++++++++ .../java/frc4388/robot/RobotContainer.java | 7 +- .../frc4388/robot/commands/AutoBalance.java | 2 +- .../commands/BlueNearDriveToChargeStation.txt | 758 +++++++++++++++++ .../robot/commands/JoystickPlayback.java | 16 +- 5 files changed, 1553 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 2d6f348..61a27d3 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -7,6 +7,11 @@ package frc4388.robot; +import java.io.File; +import java.io.IOException; +import java.io.PrintWriter; + +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -35,6 +40,8 @@ public class Robot extends TimedRobot { // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); + + } /** @@ -94,6 +101,778 @@ public class Robot extends TimedRobot { @Override public void teleopInit() { m_robotContainer.m_robotSwerveDrive.resetGyro(); + +// File auto = new File("/home/lvuser/BlueNearDriveToChargeStation.txt"); + +// try (PrintWriter writer = new PrintWriter(auto)) { +// writer.println( +// "0.0,0.0,0.0,0.0,0\n" + +// "0.0,0.0,0.0,0.0,0\n" + +// "0.0,0.0,0.0,0.0,20\n" + +// "0.0,0.0,0.0,0.0,40\n" + +// "0.0,0.0,0.0,0.0,60\n" + +// "0.0,0.0,0.0,0.0,79\n" + +// "0.0,0.0,0.0,0.0,99\n" + +// "0.0,0.0,0.0,0.0,119\n" + +// "0.0,0.0,0.0,0.0,139\n" + +// "0.0,0.0,0.0,0.0,160\n" + +// "0.0,0.0,0.0,0.0,180\n" + +// "0.0,0.0,0.0,0.0,200\n" + +// "0.0,0.0,0.0,0.0,220\n" + +// "0.0,0.0,0.0,0.0,239\n" + +// "0.0,0.0,0.0,0.0,260\n" + +// "0.0,0.0,0.0,0.0,280\n" + +// "0.0,0.0,0.0,0.0,300\n" + +// "0.0,0.0,0.0,0.0,319\n" + +// "0.0,0.0,0.0,0.0,340\n" + +// "0.0,0.0,0.0,0.0,360\n" + +// "0.0,0.0,0.0,0.0,379\n" + +// "0.0,0.0,0.0,0.0,399\n" + +// "0.0,0.0,0.0,0.0,419\n" + +// "0.0,0.0,0.0,0.0,440\n" + +// "0.0,0.0,0.0,0.0,459\n" + +// "0.0,-0.109375,0.0,0.0,479\n" + +// "0.0,-0.109375,0.0,0.0,499\n" + +// "0.0,-0.109375,0.0,0.0,520\n" + +// "0.0,-0.109375,0.0,0.0,540\n" + +// "0.0,-0.1171875,0.0,0.0,559\n" + +// "0.0,-0.1171875,0.0,0.0,580\n" + +// "0.0,-0.1171875,0.0,0.0,599\n" + +// "0.0,-0.1171875,0.0,0.0,621\n" + +// "0.0,-0.1171875,0.0,0.0,639\n" + +// "0.0,-0.1171875,0.0,0.0,661\n" + +// "0.0,-0.1171875,0.0,0.0,680\n" + +// "0.0,-0.1171875,0.0,0.0,699\n" + +// "0.0,-0.1171875,0.0,0.0,721\n" + +// "0.0,-0.1171875,0.0,0.0,769\n" + +// "0.0,-0.125,0.0,0.0,778\n" + +// "0.0,-0.1328125,0.0,0.0,790\n" + +// "0.0,-0.1328125,0.0,0.0,802\n" + +// "0.0,-0.1328125,0.0,0.0,820\n" + +// "0.0,-0.1328125,0.0,0.0,840\n" + +// "0.0,-0.1328125,0.0,0.0,860\n" + +// "0.0,-0.1328125,0.0,0.0,879\n" + +// "0.0,-0.140625,0.0,0.0,899\n" + +// "0.0,-0.171875,0.0,0.0,933\n" + +// "0.0,-0.1875,0.0,0.0,944\n" + +// "0.0,-0.1875,0.0,0.0,959\n" + +// "0.0,-0.203125,0.0,0.0,979\n" + +// "0.0,-0.203125,0.0,0.0,999\n" + +// "0.0,-0.203125,0.0,0.0,1019\n" + +// "0.0,-0.203125,0.0,0.0,1039\n" + +// "0.0,-0.203125,0.0,0.0,1059\n" + +// "0.0,-0.203125,0.0,0.0,1079\n" + +// "0.0,-0.2109375,0.0,0.0,1099\n" + +// "0.0,-0.2109375,0.0,0.0,1120\n" + +// "0.0,-0.2109375,0.0,0.0,1140\n" + +// "0.0,-0.21875,0.0,0.0,1159\n" + +// "0.0,-0.2109375,0.0,0.0,1180\n" + +// "0.0,-0.2265625,0.0,0.0,1199\n" + +// "0.0,-0.234375,0.0,0.0,1219\n" + +// "0.0,-0.234375,0.0,0.0,1239\n" + +// "0.0,-0.234375,0.0,0.0,1259\n" + +// "0.0,-0.2421875,0.0,0.0,1280\n" + +// "0.0,-0.2578125,0.0,0.0,1300\n" + +// "0.0,-0.296875,0.0,0.0,1320\n" + +// "0.0,-0.296875,0.0,0.0,1340\n" + +// "0.0,-0.3046875,0.0,0.0,1359\n" + +// "0.0,-0.3125,0.0,0.0,1379\n" + +// "0.0,-0.3125,0.0,0.0,1399\n" + +// "0.0,-0.3125,0.0,0.0,1419\n" + +// "0.0,-0.3203125,0.0,0.0,1442\n" + +// "0.0,-0.3203125,0.0,0.0,1460\n" + +// "0.0,-0.3203125,0.0,0.0,1479\n" + +// "0.0,-0.3203125,0.0,0.0,1499\n" + +// "0.0,-0.3203125,0.0,0.0,1520\n" + +// "0.0,-0.3203125,0.0,0.0,1540\n" + +// "0.0,-0.3203125,0.0,0.0,1560\n" + +// "0.0,-0.3203125,0.0,0.0,1579\n" + +// "0.0,-0.3203125,0.0,0.0,1599\n" + +// "0.0,-0.328125,0.0,0.0,1619\n" + +// "0.0,-0.34375,0.0,0.0,1640\n" + +// "0.0,-0.3515625,0.0,0.0,1659\n" + +// "0.0,-0.359375,0.0,0.0,1680\n" + +// "0.0,-0.359375,0.0,0.0,1699\n" + +// "0.0,-0.359375,0.0,0.0,1720\n" + +// "0.0,-0.359375,0.0,0.0,1739\n" + +// "0.0,-0.3671875,0.0,0.0,1759\n" + +// "0.0,-0.375,0.0,0.0,1780\n" + +// "0.0,-0.390625,0.0,0.0,1799\n" + +// "0.0,-0.4140625,0.0,0.0,1820\n" + +// "0.0,-0.421875,0.0,0.0,1839\n" + +// "0.0,-0.4296875,0.0,0.0,1859\n" + +// "0.0,-0.4296875,0.0,0.0,1879\n" + +// "0.0,-0.4296875,0.0,0.0,1900\n" + +// "0.0,-0.4296875,0.0,0.0,1919\n" + +// "0.0,-0.4296875,0.0,0.0,1939\n" + +// "0.0,-0.4296875,0.0,0.0,1959\n" + +// "0.0,-0.4296875,0.0,0.0,1979\n" + +// "0.0,-0.4296875,0.0,0.0,2000\n" + +// "0.0,-0.4296875,0.0,0.0,2019\n" + +// "0.0,-0.4296875,0.0,0.0,2039\n" + +// "0.0,-0.4296875,0.0,0.0,2060\n" + +// "0.0,-0.4296875,0.0,0.0,2080\n" + +// "0.0,-0.4296875,0.0,0.0,2099\n" + +// "0.0,-0.4296875,0.0,0.0,2119\n" + +// "0.0,-0.4296875,0.0,0.0,2139\n" + +// "0.0,-0.4296875,0.0,0.0,2160\n" + +// "0.0,-0.4375,0.0,0.0,2179\n" + +// "0.0,-0.4453125,0.0,0.0,2199\n" + +// "0.0,-0.453125,0.0,0.0,2220\n" + +// "0.0,-0.453125,0.0,0.0,2240\n" + +// "0.0,-0.453125,0.0,0.0,2260\n" + +// "0.0,-0.453125,0.0,0.0,2280\n" + +// "0.0,-0.453125,0.0,0.0,2300\n" + +// "0.0,-0.453125,0.0,0.0,2320\n" + +// "0.0,-0.453125,0.0,0.0,2340\n" + +// "0.0,-0.453125,0.0,0.0,2359\n" + +// "0.0,-0.453125,0.0,0.0,2379\n" + +// "0.0,-0.453125,0.0,0.0,2399\n" + +// "0.0,-0.453125,0.0,0.0,2419\n" + +// "0.0,-0.453125,0.0,0.0,2440\n" + +// "0.0,-0.453125,0.0,0.0,2460\n" + +// "0.0,-0.453125,0.0,0.0,2479\n" + +// "0.0,-0.453125,0.0,0.0,2500\n" + +// "0.0,-0.453125,0.0,0.0,2520\n" + +// "0.0,-0.453125,0.0,0.0,2540\n" + +// "0.0,-0.453125,0.0,0.0,2559\n" + +// "0.0,-0.453125,0.0,0.0,2579\n" + +// "0.0,-0.4609375,0.0,0.0,2600\n" + +// "0.0,-0.4609375,0.0,0.0,2620\n" + +// "0.0,-0.4609375,0.0,0.0,2641\n" + +// "0.0,-0.4609375,0.0,0.0,2660\n" + +// "0.0,-0.4609375,0.0,0.0,2679\n" + +// "0.0,-0.4609375,0.0,0.0,2700\n" + +// "0.0,-0.4609375,0.0,0.0,2720\n" + +// "0.0,-0.46875,0.0,0.0,2740\n" + +// "0.0,-0.46875,0.0,0.0,2759\n" + +// "0.0,-0.46875,0.0,0.0,2779\n" + +// "0.0,-0.46875,0.0,0.0,2799\n" + +// "0.0,-0.46875,0.0,0.0,2819\n" + +// "0.0,-0.46875,0.0,0.0,2839\n" + +// "0.0,-0.46875,0.0,0.0,2859\n" + +// "0.0,-0.4765625,0.0,0.0,2880\n" + +// "0.0,-0.5078125,0.0,0.0,2915\n" + +// "0.0,-0.515625,0.0,0.0,2933\n" + +// "0.0,-0.515625,0.0,0.0,2945\n" + +// "0.0,-0.5234375,0.0,0.0,2960\n" + +// "0.0,-0.5234375,0.0,0.0,2979\n" + +// "0.0,-0.5234375,0.0,0.0,2999\n" + +// "0.0,-0.5234375,0.0,0.0,3020\n" + +// "0.0,-0.5234375,0.0,0.0,3040\n" + +// "0.0,-0.5234375,0.0,0.0,3059\n" + +// "0.0,-0.5234375,0.0,0.0,3079\n" + +// "0.0,-0.5234375,0.0,0.0,3099\n" + +// "0.0,-0.5234375,0.0,0.0,3119\n" + +// "0.0,-0.5234375,0.0,0.0,3140\n" + +// "0.0,-0.5234375,0.0,0.0,3159\n" + +// "0.0,-0.5234375,0.0,0.0,3180\n" + +// "0.0,-0.5234375,0.0,0.0,3199\n" + +// "0.0,-0.5234375,0.0,0.0,3219\n" + +// "0.0,-0.5234375,0.0,0.0,3239\n" + +// "0.0,-0.5234375,0.0,0.0,3260\n" + +// "0.0,-0.5234375,0.0,0.0,3279\n" + +// "0.0,-0.5234375,0.0,0.0,3299\n" + +// "0.0,-0.5234375,0.0,0.0,3320\n" + +// "0.0,-0.5234375,0.0,0.0,3339\n" + +// "0.0,-0.5234375,0.0,0.0,3359\n" + +// "0.0,-0.5234375,0.0,0.0,3380\n" + +// "0.0,-0.5234375,0.0,0.0,3400\n" + +// "0.0,-0.5234375,0.0,0.0,3420\n" + +// "0.0,-0.5234375,0.0,0.0,3439\n" + +// "0.0,-0.5234375,0.0,0.0,3459\n" + +// "0.0,-0.5234375,0.0,0.0,3479\n" + +// "0.0,-0.5234375,0.0,0.0,3499\n" + +// "0.0,-0.5234375,0.0,0.0,3520\n" + +// "0.0,-0.5234375,0.0,0.0,3539\n" + +// "0.0,-0.5234375,0.0,0.0,3559\n" + +// "0.0,-0.5234375,0.0,0.0,3579\n" + +// "0.0,-0.5234375,0.0,0.0,3600\n" + +// "0.0,-0.5234375,0.0,0.0,3619\n" + +// "0.0,-0.5234375,0.0,0.0,3640\n" + +// "0.0,-0.5234375,0.0,0.0,3659\n" + +// "0.0,-0.5234375,0.0,0.0,3679\n" + +// "0.0,-0.5234375,0.0,0.0,3699\n" + +// "0.0,-0.5234375,0.0,0.0,3720\n" + +// "0.0,-0.5234375,0.0,0.0,3740\n" + +// "0.0,-0.5234375,0.0,0.0,3759\n" + +// "0.0,-0.5234375,0.0,0.0,3779\n" + +// "0.0,-0.5234375,0.0,0.0,3800\n" + +// "0.0,-0.5234375,0.0,0.0,3820\n" + +// "0.0,-0.5234375,0.0,0.0,3840\n" + +// "0.0,-0.5234375,0.0,0.0,3859\n" + +// "0.0,-0.5234375,0.0,0.0,3879\n" + +// "0.0,-0.5234375,0.0,0.0,3900\n" + +// "0.0,-0.5390625,0.0,0.0,3920\n" + +// "0.0,-0.546875,0.0,0.0,3939\n" + +// "0.0,-0.546875,0.0,0.0,3959\n" + +// "0.0,-0.546875,0.0,0.0,3979\n" + +// "0.0,-0.546875,0.0,0.0,3999\n" + +// "0.0,-0.546875,0.0,0.0,4019\n" + +// "0.0,-0.5546875,0.0,0.0,4039\n" + +// "0.0,-0.5546875,0.0,0.0,4059\n" + +// "0.0,-0.5546875,0.0,0.0,4079\n" + +// "0.0,-0.5546875,0.0,0.0,4134\n" + +// "0.0,-0.5546875,0.0,0.0,4147\n" + +// "0.0,-0.5546875,0.0,0.0,4157\n" + +// "0.0,-0.5546875,0.0,0.0,4170\n" + +// "0.0,-0.5546875,0.0,0.0,4185\n" + +// "0.0,-0.5546875,0.0,0.0,4199\n" + +// "0.0,-0.5546875,0.0,0.0,4219\n" + +// "0.0,-0.5546875,0.0,0.0,4240\n" + +// "-0.0234375,-0.5625,0.0,0.0,4259\n" + +// "-0.03125,-0.5625,0.0,0.0,4279\n" + +// "-0.03125,-0.5625,0.0,0.0,4300\n" + +// "-0.03125,-0.5625,0.0,0.0,4319\n" + +// "-0.03125,-0.5625,0.0,0.0,4339\n" + +// "-0.0234375,-0.5625,0.0,0.0,4360\n" + +// "-0.03125,-0.5625,0.0,0.0,4379\n" + +// "-0.03125,-0.5625,0.0,0.0,4399\n" + +// "-0.03125,-0.5625,0.0,0.0,4419\n" + +// "-0.03125,-0.5625,0.0,0.0,4440\n" + +// "-0.03125,-0.5625,0.0,0.0,4461\n" + +// "-0.0390625,-0.5625,0.0,0.0,4479\n" + +// "-0.046875,-0.5625,0.0,0.0,4499\n" + +// "-0.0546875,-0.5625,0.0,0.0,4520\n" + +// "-0.0625,-0.5625,0.0,0.0,4540\n" + +// "-0.0625,-0.5625,0.0,0.0,4561\n" + +// "-0.0546875,-0.5625,0.0,0.0,4581\n" + +// "-0.0546875,-0.5625,0.0,0.0,4600\n" + +// "-0.0546875,-0.5625,0.0,0.0,4620\n" + +// "-0.0546875,-0.546875,0.0,0.0,4639\n" + +// "-0.0546875,-0.546875,0.0,0.0,4659\n" + +// "-0.0546875,-0.484375,0.0,0.0,4680\n" + +// "-0.0546875,-0.4375,0.0,0.0,4700\n" + +// "-0.0546875,-0.390625,0.0,0.0,4720\n" + +// "-0.0546875,-0.3671875,0.0,0.0,4739\n" + +// "-0.0546875,-0.3359375,0.0,0.0,4759\n" + +// "-0.0546875,-0.3203125,0.0,0.0,4779\n" + +// "-0.0546875,-0.2734375,0.0,0.0,4800\n" + +// "0.0,0.0,0.0,0.0,4820\n" + +// "0.0,0.0,0.0,0.0,4840\n" + +// "0.0,0.0,0.0,0.0,4859\n" + +// "0.0,0.0,0.0,0.0,4879\n" + +// "0.0,0.0,0.0,0.0,4899\n" + +// "0.0,0.0,0.0,0.0,4919\n" + +// "0.0,0.0,0.0,0.0,4939\n" + +// "0.0,0.0,0.0,0.0,4960\n" + +// "0.0,0.0,0.0,0.0,4980\n" + +// "0.0,0.0,0.0,0.0,4999\n" + +// "0.0,0.0,0.0,0.0,5019\n" + +// "0.0,0.0,0.0,0.0,5040\n" + +// "0.0,0.0,0.0,0.0,5061\n" + +// "0.0,0.0,0.0,0.0,5079\n" + +// "0.0,0.0,0.0,0.0,5100\n" + +// "0.0,0.0,0.0,0.0,5119\n" + +// "0.0,0.0,0.0,0.0,5140\n" + +// "0.0,0.0,0.0,0.0,5159\n" + +// "0.0,0.0,0.0,0.0,5180\n" + +// "0.0,0.0,0.0,0.0,5199\n" + +// "0.0,0.0,0.0,0.0,5219\n" + +// "0.0,0.0,0.0,0.0,5239\n" + +// "0.0,0.0,0.0,0.0,5259\n" + +// "0.0,0.0,0.0,0.0,5280\n" + +// "0.0,0.0,0.0,0.0,5299\n" + +// "0.0,0.0,0.0,0.0,5319\n" + +// "0.0,0.0,0.0,0.0,5339\n" + +// "0.0,0.0,0.0,0.0,5360\n" + +// "0.0,0.0,0.0,0.0,5379\n" + +// "0.0,0.0,0.0,0.0,5399\n" + +// "0.0,0.0,0.0,0.0,5419\n" + +// "0.0,0.0,0.0,0.0,5439\n" + +// "0.0,0.0,0.0,0.0,5459\n" + +// "0.0,0.0,0.0,0.0,5480\n" + +// "0.0,0.0,0.0,0.0,5500\n" + +// "0.0,0.0,0.0,0.0,5519\n" + +// "0.0,0.0,0.0,0.0,5540\n" + +// "0.0,0.0,0.0,0.0,5559\n" + +// "0.0,0.0,0.0,0.0,5579\n" + +// "0.0,0.0,0.0,0.0,5599\n" + +// "0.0,0.0,0.0,0.0,5619\n" + +// "0.0,0.0,0.0,0.0,5639\n" + +// "0.0,0.0,0.0,0.0,5659\n" + +// "0.0,0.0,0.0,0.0,5680\n" + +// "0.0,0.0,0.0,0.0,5699\n" + +// "0.0,0.0,0.0,0.0,5721\n" + +// "0.0,0.0,0.0,0.0,5739\n" + +// "0.0,0.0,0.0,0.0,5760\n" + +// "0.0,0.0,0.0,0.0,5779\n" + +// "0.0,0.0,0.0,0.0,5800\n" + +// "0.0,0.0,0.0,0.0,5819\n" + +// "0.0,0.0,0.0,0.0,5839\n" + +// "0.0,0.0,0.0,0.0,5859\n" + +// "0.0,0.0,0.0,0.0,5879\n" + +// "0.0,0.0,0.0,0.0,5900\n" + +// "0.0,0.0,0.0,0.0,5920\n" + +// "0.0,0.0,0.0,0.0,5939\n" + +// "0.0,0.0,0.0,0.0,5959\n" + +// "0.0,0.0,0.0,0.0,5979\n" + +// "-0.03125,0.11811023950576782,0.0,0.0,5999\n" + +// "-0.03125,0.14173229038715363,0.0,0.0,6020\n" + +// "-0.03125,0.14173229038715363,0.0,0.0,6039\n" + +// "-0.03125,0.14173229038715363,0.0,0.0,6059\n" + +// "-0.03125,0.14173229038715363,0.0,0.0,6079\n" + +// "-0.03125,0.14173229038715363,0.0,0.0,6100\n" + +// "-0.03125,0.14173229038715363,0.0,0.0,6119\n" + +// "-0.03125,0.14173229038715363,0.0,0.0,6139\n" + +// "-0.03125,0.14173229038715363,0.0,0.0,6159\n" + +// "-0.03125,0.14173229038715363,0.0,0.0,6179\n" + +// "-0.03125,0.14960630238056183,0.0,0.0,6199\n" + +// "-0.03125,0.14960630238056183,0.0,0.0,6219\n" + +// "-0.03125,0.14960630238056183,0.0,0.0,6239\n" + +// "-0.03125,0.14960630238056183,0.0,0.0,6259\n" + +// "-0.03125,0.14960630238056183,0.0,0.0,6280\n" + +// "-0.03125,0.14960630238056183,0.0,0.0,6299\n" + +// "-0.03125,0.16535432636737823,0.0,0.0,6319\n" + +// "-0.03125,0.19685038924217224,0.0,0.0,6340\n" + +// "-0.03125,0.20472441613674164,0.0,0.0,6360\n" + +// "-0.03125,0.21259842813014984,0.0,0.0,6381\n" + +// "-0.03125,0.21259842813014984,0.0,0.0,6399\n" + +// "-0.03125,0.21259842813014984,0.0,0.0,6419\n" + +// "-0.03125,0.21259842813014984,0.0,0.0,6440\n" + +// "-0.03125,0.21259842813014984,0.0,0.0,6459\n" + +// "-0.03125,0.21259842813014984,0.0,0.0,6479\n" + +// "-0.03125,0.21259842813014984,0.0,0.0,6499\n" + +// "-0.03125,0.21259842813014984,0.0,0.0,6519\n" + +// "-0.03125,0.21259842813014984,0.0,0.0,6540\n" + +// "-0.03125,0.21259842813014984,0.0,0.0,6559\n" + +// "-0.03125,0.21259842813014984,0.0,0.0,6579\n" + +// "-0.03125,0.21259842813014984,0.0,0.0,6599\n" + +// "-0.03125,0.21259842813014984,0.0,0.0,6619\n" + +// "-0.03125,0.21259842813014984,0.0,0.0,6640\n" + +// "-0.03125,0.21259842813014984,0.0,0.0,6660\n" + +// "-0.03125,0.21259842813014984,0.0,0.0,6679\n" + +// "-0.03125,0.33070865273475647,0.0,0.0,6700\n" + +// "-0.03125,0.33070865273475647,0.0,0.0,6720\n" + +// "-0.03125,0.3700787425041199,0.0,0.0,6740\n" + +// "-0.03125,0.3779527544975281,0.0,0.0,6760\n" + +// "-0.03125,0.3779527544975281,0.0,0.0,6780\n" + +// "-0.03125,0.3779527544975281,0.0,0.0,6800\n" + +// "-0.03125,0.3779527544975281,0.0,0.0,6819\n" + +// "-0.03125,0.3779527544975281,0.0,0.0,6839\n" + +// "-0.03125,0.3700787425041199,0.0,0.0,6859\n" + +// "-0.03125,0.3700787425041199,0.0,0.0,6879\n" + +// "-0.03125,0.3700787425041199,0.0,0.0,6899\n" + +// "-0.03125,0.3700787425041199,0.0,0.0,6920\n" + +// "-0.03125,0.3700787425041199,0.0,0.0,6939\n" + +// "-0.03125,0.3700787425041199,0.0,0.0,6959\n" + +// "-0.03125,0.3700787425041199,0.0,0.0,6979\n" + +// "-0.03125,0.3700787425041199,0.0,0.0,6999\n" + +// "-0.03125,0.3700787425041199,0.0,0.0,7019\n" + +// "-0.03125,0.3700787425041199,0.0,0.0,7039\n" + +// "-0.03125,0.3700787425041199,0.0,0.0,7060\n" + +// "-0.03125,0.3700787425041199,0.0,0.0,7080\n" + +// "-0.03125,0.3700787425041199,0.0,0.0,7100\n" + +// "-0.03125,0.3700787425041199,0.0,0.0,7119\n" + +// "-0.03125,0.3700787425041199,0.0,0.0,7139\n" + +// "-0.03125,0.3700787425041199,0.0,0.0,7159\n" + +// "-0.03125,0.3700787425041199,0.0,0.0,7180\n" + +// "-0.03125,0.3700787425041199,0.0,0.0,7199\n" + +// "-0.03125,0.3700787425041199,0.0,0.0,7220\n" + +// "-0.03125,0.3700787425041199,0.0,0.0,7239\n" + +// "-0.03125,0.3700787425041199,0.0,0.0,7260\n" + +// "-0.03125,0.36220473051071167,0.0,0.0,7279\n" + +// "-0.03125,0.34645670652389526,0.0,0.0,7299\n" + +// "0.0,0.33070865273475647,0.0,0.0,7319\n" + +// "0.0,0.33070865273475647,0.0,0.0,7339\n" + +// "0.0,0.3385826647281647,0.0,0.0,7359\n" + +// "0.0,0.3385826647281647,0.0,0.0,7379\n" + +// "0.0,0.3385826647281647,0.0,0.0,7399\n" + +// "0.0,0.3385826647281647,0.0,0.0,7419\n" + +// "0.0,0.3385826647281647,0.0,0.0,7440\n" + +// "0.0,0.33070865273475647,0.0,0.0,7460\n" + +// "0.0,0.31496062874794006,0.0,0.0,7479\n" + +// "0.0,0.31496062874794006,0.0,0.0,7499\n" + +// "0.0,0.29133859276771545,0.0,0.0,7519\n" + +// "0.0,0.24409449100494385,0.0,0.0,7540\n" + +// "0.0,0.24409449100494385,0.0,0.0,7559\n" + +// "0.0,0.24409449100494385,0.0,0.0,7580\n" + +// "0.0,0.24409449100494385,0.0,0.0,7600\n" + +// "0.0,0.24409449100494385,0.0,0.0,7619\n" + +// "0.0,0.24409449100494385,0.0,0.0,7639\n" + +// "0.0,0.24409449100494385,0.0,0.0,7660\n" + +// "0.0,0.24409449100494385,0.0,0.0,7679\n" + +// "0.0,0.24409449100494385,0.0,0.0,7700\n" + +// "0.0,0.25196850299835205,0.0,0.0,7720\n" + +// "0.0,0.33070865273475647,0.0,0.0,7740\n" + +// "0.0,0.3700787425041199,0.0,0.0,7760\n" + +// "0.0,0.4251968562602997,0.0,0.0,7780\n" + +// "0.0,0.4566929042339325,0.0,0.0,7800\n" + +// "0.0,0.4645669162273407,0.0,0.0,7820\n" + +// "0.0,0.4645669162273407,0.0,0.0,7839\n" + +// "0.0,0.4645669162273407,0.0,0.0,7859\n" + +// "0.0,0.4645669162273407,0.0,0.0,7880\n" + +// "0.0,0.4645669162273407,0.0,0.0,7899\n" + +// "0.0,0.4645669162273407,0.0,0.0,7920\n" + +// "0.0,0.4645669162273407,0.0,0.0,7939\n" + +// "0.0,0.4803149700164795,0.0,0.0,7960\n" + +// "0.0,0.5039370059967041,0.0,0.0,7980\n" + +// "-0.0078125,0.5433070659637451,0.0,0.0,7999\n" + +// "-0.0078125,0.5748031735420227,0.0,0.0,8019\n" + +// "-0.015625,0.5748031735420227,0.0,0.0,8039\n" + +// "-0.015625,0.5748031735420227,0.0,0.0,8059\n" + +// "-0.015625,0.5748031735420227,0.0,0.0,8080\n" + +// "-0.015625,0.5511810779571533,0.0,0.0,8099\n" + +// "-0.015625,0.5354330539703369,0.0,0.0,8119\n" + +// "-0.015625,0.5354330539703369,0.0,0.0,8139\n" + +// "-0.015625,0.5354330539703369,0.0,0.0,8159\n" + +// "-0.0078125,0.5354330539703369,0.0,0.0,8180\n" + +// "-0.0078125,0.5354330539703369,0.0,0.0,8199\n" + +// "-0.0078125,0.5354330539703369,0.0,0.0,8220\n" + +// "-0.0078125,0.5354330539703369,0.0,0.0,8239\n" + +// "-0.0078125,0.5354330539703369,0.0,0.0,8259\n" + +// "-0.0078125,0.5354330539703369,0.0,0.0,8279\n" + +// "-0.0078125,0.5354330539703369,0.0,0.0,8299\n" + +// "-0.0078125,0.5354330539703369,0.0,0.0,8319\n" + +// "-0.0078125,0.5354330539703369,0.0,0.0,8340\n" + +// "-0.0078125,0.5275590419769287,0.0,0.0,8359\n" + +// "-0.0078125,0.4803149700164795,0.0,0.0,8379\n" + +// "-0.0078125,0.4803149700164795,0.0,0.0,8399\n" + +// "-0.0078125,0.4803149700164795,0.0,0.0,8421\n" + +// "-0.0078125,0.4803149700164795,0.0,0.0,8440\n" + +// "-0.0078125,0.4803149700164795,0.0,0.0,8460\n" + +// "-0.0078125,0.4803149700164795,0.0,0.0,8479\n" + +// "0.0,0.4803149700164795,0.0,0.0,8499\n" + +// "0.0,0.4803149700164795,0.0,0.0,8520\n" + +// "0.0,0.4803149700164795,0.0,0.0,8540\n" + +// "0.0,0.4803149700164795,0.0,0.0,8559\n" + +// "0.0,0.4803149700164795,0.0,0.0,8579\n" + +// "0.0,0.4803149700164795,0.0,0.0,8600\n" + +// "0.0,0.4803149700164795,0.0,0.0,8619\n" + +// "0.0,0.4803149700164795,0.0,0.0,8639\n" + +// "0.0,0.4803149700164795,0.0,0.0,8659\n" + +// "0.0,0.4724409580230713,0.0,0.0,8679\n" + +// "0.0,0.4409448802471161,0.0,0.0,8699\n" + +// "0.0,0.4173228442668915,0.0,0.0,8719\n" + +// "0.0,0.4094488322734833,0.0,0.0,8739\n" + +// "0.0,0.4094488322734833,0.0,0.0,8759\n" + +// "0.0,0.4094488322734833,0.0,0.0,8779\n" + +// "0.015748031437397003,0.4094488322734833,0.0,0.0,8799\n" + +// "0.04724409431219101,0.4094488322734833,0.0,0.0,8820\n" + +// "0.07874015718698502,0.4094488322734833,0.0,0.0,8840\n" + +// "0.07874015718698502,0.4094488322734833,0.0,0.0,8859\n" + +// "0.07874015718698502,0.4094488322734833,0.0,0.0,8879\n" + +// "0.07874015718698502,0.4251968562602997,0.0,0.0,8899\n" + +// "0.07874015718698502,0.4488188922405243,0.0,0.0,8920\n" + +// "0.07874015718698502,0.4488188922405243,0.0,0.0,8939\n" + +// "0.07874015718698502,0.4488188922405243,0.0,0.0,8959\n" + +// "0.07874015718698502,0.4488188922405243,0.0,0.0,8980\n" + +// "0.05511811003088951,0.4488188922405243,0.0,0.0,8999\n" + +// "0.031496062874794006,0.4488188922405243,0.0,0.0,9019\n" + +// "0.031496062874794006,0.4488188922405243,0.0,0.0,9039\n" + +// "0.031496062874794006,0.4488188922405243,0.0,0.0,9059\n" + +// "0.031496062874794006,0.4488188922405243,0.0,0.0,9079\n" + +// "0.031496062874794006,0.4488188922405243,0.0,0.0,9099\n" + +// "0.031496062874794006,0.4488188922405243,0.0,0.0,9120\n" + +// "0.031496062874794006,0.4488188922405243,0.0,0.0,9139\n" + +// "0.031496062874794006,0.4488188922405243,0.0,0.0,9159\n" + +// "0.031496062874794006,0.4488188922405243,0.0,0.0,9179\n" + +// "0.031496062874794006,0.4488188922405243,0.0,0.0,9199\n" + +// "0.031496062874794006,0.4488188922405243,0.0,0.0,9219\n" + +// "0.031496062874794006,0.4488188922405243,0.0,0.0,9239\n" + +// "0.04724409431219101,0.4488188922405243,0.0,0.0,9260\n" + +// "0.06299212574958801,0.4488188922405243,0.0,0.0,9279\n" + +// "0.06299212574958801,0.4488188922405243,0.0,0.0,9299\n" + +// "0.06299212574958801,0.4488188922405243,0.0,0.0,9319\n" + +// "0.06299212574958801,0.4488188922405243,0.0,0.0,9339\n" + +// "0.07874015718698502,0.4488188922405243,0.0,0.0,9359\n" + +// "0.07874015718698502,0.4488188922405243,0.0,0.0,9380\n" + +// "0.07874015718698502,0.4488188922405243,0.0,0.0,9400\n" + +// "0.07874015718698502,0.4488188922405243,0.0,0.0,9419\n" + +// "0.07874015718698502,0.4488188922405243,0.0,0.0,9439\n" + +// "0.07874015718698502,0.4488188922405243,0.0,0.0,9460\n" + +// "0.07874015718698502,0.4488188922405243,0.0,0.0,9480\n" + +// "0.07874015718698502,0.4488188922405243,0.0,0.0,9499\n" + +// "0.07874015718698502,0.4488188922405243,0.0,0.0,9519\n" + +// "0.07874015718698502,0.4488188922405243,0.0,0.0,9539\n" + +// "0.07874015718698502,0.4488188922405243,0.0,0.0,9559\n" + +// "0.07874015718698502,0.4488188922405243,0.0,0.0,9579\n" + +// "0.07874015718698502,0.4488188922405243,0.0,0.0,9600\n" + +// "0.07874015718698502,0.4488188922405243,0.0,0.0,9619\n" + +// "0.07874015718698502,0.4488188922405243,0.0,0.0,9639\n" + +// "0.07874015718698502,0.4488188922405243,0.0,0.0,9659\n" + +// "0.07874015718698502,0.4488188922405243,0.0,0.0,9679\n" + +// "0.07874015718698502,0.4488188922405243,0.0,0.0,9699\n" + +// "0.07874015718698502,0.4488188922405243,0.0,0.0,9722\n" + +// "0.07874015718698502,0.4488188922405243,0.0,0.0,9739\n" + +// "0.07874015718698502,0.4488188922405243,0.0,0.0,9759\n" + +// "0.08661417663097382,0.4488188922405243,0.0,0.0,9780\n" + +// "0.08661417663097382,0.4488188922405243,0.0,0.0,9800\n" + +// "0.11811023950576782,0.4488188922405243,0.0,0.0,9820\n" + +// "0.14173229038715363,0.4488188922405243,0.0,0.0,9839\n" + +// "0.16535432636737823,0.4488188922405243,0.0,0.0,9859\n" + +// "0.22834645211696625,0.4488188922405243,0.0,0.0,9879\n" + +// "0.23622047901153564,0.4488188922405243,0.0,0.0,9899\n" + +// "0.23622047901153564,0.4409448802471161,0.0,0.0,9919\n" + +// "0.25984251499176025,0.4094488322734833,0.0,0.0,9940\n" + +// "0.29921260476112366,0.3858267664909363,0.0,0.0,9959\n" + +// "0.31496062874794006,0.3779527544975281,0.0,0.0,9979\n" + +// "0.32283464074134827,0.3700787425041199,0.0,0.0,9999\n" + +// "0.32283464074134827,0.3700787425041199,0.0,0.0,10019\n" + +// "0.36220473051071167,0.33070865273475647,0.0,0.0,10039\n" + +// "0.3779527544975281,0.25984251499176025,0.0,0.0,10059\n" + +// "0.3937007784843445,0.21259842813014984,0.0,0.0,10081\n" + +// "0.3937007784843445,0.20472441613674164,0.0,0.0,10100\n" + +// "0.3937007784843445,0.20472441613674164,0.0,0.0,10124\n" + +// "0.3937007784843445,0.20472441613674164,0.0,0.0,10140\n" + +// "0.3937007784843445,0.18897637724876404,0.0,0.0,10159\n" + +// "0.3937007784843445,0.18897637724876404,0.0,0.0,10180\n" + +// "0.3937007784843445,0.16535432636737823,0.0,0.0,10199\n" + +// "0.3937007784843445,0.16535432636737823,0.0,0.0,10221\n" + +// "0.3937007784843445,0.17322835326194763,0.0,0.0,10239\n" + +// "0.3937007784843445,0.16535432636737823,0.0,0.0,10260\n" + +// "0.3937007784843445,0.14960630238056183,0.0,0.0,10279\n" + +// "0.3937007784843445,0.14960630238056183,0.0,0.0,10300\n" + +// "0.3937007784843445,0.14960630238056183,0.0,0.0,10319\n" + +// "0.3937007784843445,0.14960630238056183,0.0,0.0,10340\n" + +// "0.3937007784843445,0.14173229038715363,0.0,0.0,10360\n" + +// "0.3937007784843445,0.14173229038715363,0.0,0.0,10380\n" + +// "0.3937007784843445,0.14173229038715363,0.0,0.0,10399\n" + +// "0.3937007784843445,0.14173229038715363,0.0,0.0,10419\n" + +// "0.3937007784843445,0.14173229038715363,0.0,0.0,10440\n" + +// "0.3937007784843445,0.14173229038715363,0.0,0.0,10459\n" + +// "0.3937007784843445,0.14173229038715363,0.0,0.0,10479\n" + +// "0.3937007784843445,0.14173229038715363,0.0,0.0,10500\n" + +// "0.3937007784843445,0.14173229038715363,0.0,0.0,10519\n" + +// "0.4015747904777527,0.14173229038715363,0.0,0.0,10540\n" + +// "0.4173228442668915,0.14173229038715363,0.0,0.0,10559\n" + +// "0.4251968562602997,0.13385826349258423,0.0,0.0,10580\n" + +// "0.4251968562602997,0.12598425149917603,0.0,0.0,10600\n" + +// "0.4251968562602997,0.10236220806837082,0.0,0.0,10620\n" + +// "0.4251968562602997,0.10236220806837082,0.0,0.0,10639\n" + +// "0.4330708682537079,0.10236220806837082,0.0,0.0,10659\n" + +// "0.4409448802471161,0.10236220806837082,0.0,0.0,10680\n" + +// "0.4488188922405243,0.07086614519357681,0.0,0.0,10699\n" + +// "0.4566929042339325,0.023622047156095505,0.0,0.0,10719\n" + +// "0.4566929042339325,0.0,0.0,0.0,10739\n" + +// "0.4645669162273407,0.0,0.0,0.0,10760\n" + +// "0.4645669162273407,0.0,0.0,0.0,10779\n" + +// "0.4724409580230713,0.0,0.0,0.0,10800\n" + +// "0.4724409580230713,0.0,0.0,0.0,10819\n" + +// "0.4724409580230713,0.0,0.0,0.0,10839\n" + +// "0.4803149700164795,0.0,0.0,0.0,10860\n" + +// "0.4881889820098877,0.0,0.0,0.0,10879\n" + +// "0.5039370059967041,0.0,0.0,0.0,10899\n" + +// "0.5275590419769287,0.0,0.0,0.0,10919\n" + +// "0.5354330539703369,0.0,0.0,0.0,10940\n" + +// "0.5354330539703369,0.0,0.0,0.0,10960\n" + +// "0.5354330539703369,0.0,0.0,0.0,10979\n" + +// "0.5354330539703369,0.0,0.0,0.0,10999\n" + +// "0.5354330539703369,0.0,0.0,0.0,11020\n" + +// "0.5354330539703369,0.0,0.0,0.0,11039\n" + +// "0.5354330539703369,0.0,0.0,0.0,11059\n" + +// "0.5354330539703369,0.0,0.0,0.0,11079\n" + +// "0.5354330539703369,0.0,0.0,0.0,11099\n" + +// "0.5354330539703369,0.0,0.0,0.0,11119\n" + +// "0.5354330539703369,0.0,0.0,0.0,11140\n" + +// "0.5354330539703369,0.0,0.0,0.0,11159\n" + +// "0.5354330539703369,0.0,0.0,0.0,11179\n" + +// "0.5354330539703369,0.0,0.0,0.0,11200\n" + +// "0.5354330539703369,0.0,0.0,0.0,11219\n" + +// "0.5354330539703369,0.0,0.0,0.0,11239\n" + +// "0.5354330539703369,0.0,0.0,0.0,11260\n" + +// "0.5354330539703369,0.0,0.0,0.0,11279\n" + +// "0.5354330539703369,0.0,0.0,0.0,11299\n" + +// "0.5275590419769287,0.0,0.0,0.0,11319\n" + +// "0.5275590419769287,0.0,0.0,0.0,11339\n" + +// "0.5275590419769287,0.0,0.0,0.0,11360\n" + +// "0.5275590419769287,0.0,0.0,0.0,11379\n" + +// "0.5275590419769287,0.0,0.0,0.0,11399\n" + +// "0.5275590419769287,0.0,0.0,0.0,11419\n" + +// "0.5275590419769287,0.0,0.0,0.0,11440\n" + +// "0.5275590419769287,0.0,0.0,0.0,11459\n" + +// "0.5275590419769287,0.0,0.0,0.0,11479\n" + +// "0.5275590419769287,0.0,0.0,0.0,11500\n" + +// "0.5275590419769287,0.0,0.0,0.0,11519\n" + +// "0.5275590419769287,0.0,0.0,0.0,11539\n" + +// "0.5275590419769287,0.0,0.0,0.0,11559\n" + +// "0.5275590419769287,0.0,0.0,0.0,11579\n" + +// "0.5118110179901123,0.0,0.0,0.0,11599\n" + +// "0.5118110179901123,0.0,0.0,0.0,11620\n" + +// "0.5118110179901123,0.0,0.0,0.0,11640\n" + +// "0.5039370059967041,0.0,0.0,0.0,11660\n" + +// "0.4803149700164795,0.0,0.0,0.0,11679\n" + +// "0.4566929042339325,0.0,0.0,0.0,11699\n" + +// "0.4330708682537079,0.0,0.0,0.0,11720\n" + +// "0.4173228442668915,0.0,0.0,0.0,11739\n" + +// "0.4094488322734833,0.0,0.0,0.0,11760\n" + +// "0.4094488322734833,0.0,0.0,0.0,11780\n" + +// "0.4094488322734833,0.0,0.0,0.0,11800\n" + +// "0.4094488322734833,0.0,0.0,0.0,11819\n" + +// "0.4094488322734833,0.0,0.0,0.0,11840\n" + +// "0.3937007784843445,0.0,0.0,0.0,11859\n" + +// "0.25196850299835205,-0.0390625,0.0,0.0,11879\n" + +// "0.11811023950576782,-0.078125,0.0,0.0,11900\n" + +// "0.06299212574958801,-0.078125,0.0,0.0,11920\n" + +// "0.0,0.0,0.0,0.0,11939\n" + +// "0.0,0.0,0.0,0.0,11959\n" + +// "0.0,0.0,0.0,0.0,11979\n" + +// "0.0,0.0,0.0,0.0,12000\n" + +// "0.0,0.0,0.0,0.0,12019\n" + +// "0.0,0.0,0.0,0.0,12040\n" + +// "0.0,0.0,0.0,0.0,12060\n" + +// "0.0,0.0,0.0,0.0,12080\n" + +// "0.0,0.0,0.0,0.0,12099\n" + +// "0.0,0.0,0.0,0.0,12119\n" + +// "0.0,-0.1015625,0.0,0.0,12139\n" + +// "0.0,-0.1015625,0.0,0.0,12159\n" + +// "0.0,-0.109375,0.0,0.0,12179\n" + +// "0.0,-0.1171875,0.0,0.0,12199\n" + +// "0.0,-0.1171875,0.0,0.0,12219\n" + +// "0.0,-0.1171875,0.0,0.0,12240\n" + +// "0.0,-0.1328125,0.0,0.0,12259\n" + +// "0.0,-0.15625,0.0,0.0,12279\n" + +// "0.0,-0.1953125,0.0,0.0,12300\n" + +// "0.0,-0.21875,0.0,0.0,12319\n" + +// "0.0,-0.28125,0.0,0.0,12339\n" + +// "0.0,-0.296875,0.0,0.0,12360\n" + +// "0.0,-0.3046875,0.0,0.0,12379\n" + +// "0.0,-0.3125,0.0,0.0,12399\n" + +// "0.0,-0.3359375,0.0,0.0,12419\n" + +// "0.0,-0.3359375,0.0,0.0,12440\n" + +// "0.0,-0.328125,0.0,0.0,12459\n" + +// "0.0,-0.328125,0.0,0.0,12479\n" + +// "0.0,-0.328125,0.0,0.0,12499\n" + +// "0.0,-0.328125,0.0,0.0,12520\n" + +// "0.0,-0.328125,0.0,0.0,12539\n" + +// "0.0,-0.328125,0.0,0.0,12560\n" + +// "0.0,-0.328125,0.0,0.0,12580\n" + +// "0.0,-0.328125,0.0,0.0,12599\n" + +// "0.0,-0.328125,0.0,0.0,12620\n" + +// "0.0,-0.328125,0.0,0.0,12639\n" + +// "0.0,-0.328125,0.0,0.0,12659\n" + +// "0.0,-0.328125,0.0,0.0,12679\n" + +// "0.0,-0.328125,0.0,0.0,12699\n" + +// "0.0,-0.328125,0.0,0.0,12719\n" + +// "0.0,-0.328125,0.0,0.0,12739\n" + +// "0.0,-0.328125,0.0,0.0,12760\n" + +// "0.0,-0.34375,0.0,0.0,12780\n" + +// "0.0,-0.3515625,0.0,0.0,12799\n" + +// "0.0,-0.359375,0.0,0.0,12819\n" + +// "0.0,-0.359375,0.0,0.0,12840\n" + +// "0.0,-0.359375,0.0,0.0,12860\n" + +// "0.0,-0.359375,0.0,0.0,12880\n" + +// "0.0,-0.359375,0.0,0.0,12900\n" + +// "0.0,-0.3671875,0.0,0.0,12921\n" + +// "0.0,-0.3671875,0.0,0.0,12939\n" + +// "0.0,-0.375,0.0,0.0,12959\n" + +// "0.0,-0.375,0.0,0.0,12979\n" + +// "0.0,-0.390625,0.0,0.0,12999\n" + +// "0.015748031437397003,-0.40625,0.0,0.0,13019\n" + +// "0.023622047156095505,-0.421875,0.0,0.0,13039\n" + +// "0.03937007859349251,-0.4296875,0.0,0.0,13060\n" + +// "0.03937007859349251,-0.4296875,0.0,0.0,13079\n" + +// "0.04724409431219101,-0.453125,0.0,0.0,13099\n" + +// "0.05511811003088951,-0.46875,0.0,0.0,13119\n" + +// "0.05511811003088951,-0.484375,0.0,0.0,13139\n" + +// "0.05511811003088951,-0.5,0.0,0.0,13160\n" + +// "0.05511811003088951,-0.515625,0.0,0.0,13179\n" + +// "0.05511811003088951,-0.515625,0.0,0.0,13199\n" + +// "0.05511811003088951,-0.515625,0.0,0.0,13219\n" + +// "0.05511811003088951,-0.5234375,0.0,0.0,13239\n" + +// "0.05511811003088951,-0.5234375,0.0,0.0,13260\n" + +// "0.05511811003088951,-0.5234375,0.0,0.0,13279\n" + +// "0.05511811003088951,-0.5234375,0.0,0.0,13299\n" + +// "0.05511811003088951,-0.5234375,0.0,0.0,13319\n" + +// "0.04724409431219101,-0.5234375,0.0,0.0,13339\n" + +// "0.04724409431219101,-0.5234375,0.0,0.0,13359\n" + +// "0.04724409431219101,-0.5234375,0.0,0.0,13379\n" + +// "0.04724409431219101,-0.5234375,0.0,0.0,13400\n" + +// "0.04724409431219101,-0.5234375,0.0,0.0,13419\n" + +// "0.04724409431219101,-0.5234375,0.0,0.0,13439\n" + +// "0.04724409431219101,-0.5234375,0.0,0.0,13459\n" + +// "0.04724409431219101,-0.5234375,0.0,0.0,13479\n" + +// "0.04724409431219101,-0.5234375,0.0,0.0,13499\n" + +// "0.04724409431219101,-0.5234375,0.0,0.0,13519\n" + +// "0.04724409431219101,-0.5234375,0.0,0.0,13539\n" + +// "0.04724409431219101,-0.5234375,0.0,0.0,13559\n" + +// "0.04724409431219101,-0.5234375,0.0,0.0,13579\n" + +// "0.04724409431219101,-0.5234375,0.0,0.0,13599\n" + +// "0.04724409431219101,-0.5234375,0.0,0.0,13620\n" + +// "0.04724409431219101,-0.5234375,0.0,0.0,13642\n" + +// "0.04724409431219101,-0.5234375,0.0,0.0,13659\n" + +// "0.04724409431219101,-0.5234375,0.0,0.0,13679\n" + +// "0.04724409431219101,-0.5234375,0.0,0.0,13699\n" + +// "0.04724409431219101,-0.5234375,0.0,0.0,13720\n" + +// "0.04724409431219101,-0.5234375,0.0,0.0,13740\n" + +// "0.04724409431219101,-0.5234375,0.0,0.0,13760\n" + +// "0.04724409431219101,-0.5234375,0.0,0.0,13779\n" + +// "0.04724409431219101,-0.53125,0.0,0.0,13800\n" + +// "0.04724409431219101,-0.546875,0.0,0.0,13819\n" + +// "0.04724409431219101,-0.5625,0.0,0.0,13840\n" + +// "0.04724409431219101,-0.578125,0.0,0.0,13859\n" + +// "0.04724409431219101,-0.59375,0.0,0.0,13880\n" + +// "0.07874015718698502,-0.6875,0.0,0.0,13900\n" + +// "0.07874015718698502,-0.703125,0.0,0.0,13919\n" + +// "0.07874015718698502,-0.7265625,0.0,0.0,13944\n" + +// "0.07874015718698502,-0.7421875,0.0,0.0,13959\n" + +// "0.07874015718698502,-0.765625,0.0,0.0,13980\n" + +// "0.07874015718698502,-0.78125,0.0,0.0,14000\n" + +// "0.07874015718698502,-0.7890625,0.0,0.0,14020\n" + +// "0.07874015718698502,-0.8125,0.0,0.0,14039\n" + +// "0.07874015718698502,-0.8203125,0.0,0.0,14060\n" + +// "0.07874015718698502,-0.828125,0.0,0.0,14079\n" + +// "0.07874015718698502,-0.8359375,0.0,0.0,14099\n" + +// "0.07874015718698502,-0.8359375,0.0,0.0,14119\n" + +// "0.07874015718698502,-0.8359375,0.0,0.0,14139\n" + +// "0.07874015718698502,-0.8359375,0.0,0.0,14159\n" + +// "0.07874015718698502,-0.8359375,0.0,0.0,14179\n" + +// "0.07874015718698502,-0.8203125,0.0,0.0,14199\n" + +// "0.07874015718698502,-0.78125,0.0,0.0,14219\n" + +// "0.007874015718698502,-0.265625,0.0,0.0,14239\n" + +// "0.0,0.0,0.0,0.0,14259\n" + +// "0.0,0.0,0.0,0.0,14279\n" + +// "0.0,-0.1171875,0.0,0.0,14299\n" + +// "0.0,-0.1328125,0.0,0.0,14319\n" + +// "0.0,-0.171875,0.0,0.0,14339\n" + +// "0.0,-0.171875,0.0,0.0,14359\n" + +// "0.0,0.0,0.0,0.0,14379\n" + +// "0.0,0.0,0.0,0.0,14399\n" + +// "0.0,0.0,0.0,0.0,14419\n" + +// "0.0,0.0,0.0,0.0,14439\n" + +// "0.0,0.0,0.0,0.0,14459\n" + +// "0.0,0.0,0.0,0.0,14479\n" + +// "0.0,0.0,0.0,0.0,14499\n" + +// "0.0,0.0,0.0,0.0,14519\n" + +// "0.0,-0.203125,0.0,0.0,14539\n" + +// "0.0,-0.4609375,0.0,0.0,14559\n" + +// "0.031496062874794006,-0.71875,0.0,0.0,14579\n" + +// "0.031496062874794006,-0.9453125,0.0,0.0,14599\n" + +// "0.03148045240972986,-0.999504367732367,0.0,0.0,14619\n" + +// "0.03148045240972986,-0.999504367732367,0.0,0.0,14639\n" + +// "0.03148045240972986,-0.999504367732367,0.0,0.0,14660\n" + +// "0.03148045240972986,-0.999504367732367,0.0,0.0,14679\n" + +// "0.03148045240972986,-0.999504367732367,0.0,0.0,14699\n" + +// "0.03148045240972986,-0.999504367732367,0.0,0.0,14719\n" + +// "0.031496062874794006,-0.9609375,0.0,0.0,14739\n" + +// "0.031496062874794006,-0.9609375,0.0,0.0,14759\n" + +// "0.031496062874794006,-0.9609375,0.0,0.0,14780\n" + +// "0.031496062874794006,-0.9609375,0.0,0.0,14799\n" + +// "0.031496062874794006,-0.953125,0.0,0.0,14820\n" + +// "0.031496062874794006,-0.9609375,0.0,0.0,14839\n" + +// "0.03148045240972986,-0.999504367732367,0.0,0.0,14860\n" + +// "0.03148045240972986,-0.999504367732367,0.0,0.0,14880\n" + +// "0.03148045240972986,-0.999504367732367,0.0,0.0,14900\n" + +// "0.03148045240972986,-0.999504367732367,0.0,0.0,14919\n" + +// "0.031496062874794006,-0.984375,0.0,0.0,14939\n" + +// "0.0,-0.3515625,0.0,0.0,14959\n" + +// "0.0,0.0,0.0,0.0,14980\n" + +// "0.0,0.0,0.0,0.0,14999\n" + +// "0.0,0.0,0.0,0.0,15019\n" + +// "0.0,0.0,0.0,0.0,15039\n" + +// "0.0,0.0,0.0,0.0,15059\n" + +// "0.0,0.0,0.0,0.0,15082\n" + +// "0.0,0.0,0.0,0.0,15099\n" + +// "0.0,0.0,0.0,0.0,15120\n" +// ); + +// writer.close(); + +// System.out.println("Writing Done."); +// } catch (IOException e) { +// e.printStackTrace(); +// } + // 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 diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 2982bf5..8412738 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -105,7 +105,7 @@ public class RobotContainer { .onFalse(new InstantCommand()); new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) - .onTrue(new JoystickPlayback(m_robotSwerveDrive)); + .onTrue(new JoystickPlayback(m_robotSwerveDrive, 1)); // * Operator Buttons } @@ -117,7 +117,9 @@ public class RobotContainer { */ public Command getAutonomousCommand() { - return new JoystickPlayback(m_robotSwerveDrive); + return new JoystickPlayback(m_robotSwerveDrive, 1) + .andThen(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); + // return new InstantCommand(); } public DeadbandedXboxController getDeadbandedDriverController() { @@ -127,4 +129,5 @@ public class RobotContainer { public DeadbandedXboxController getDeadbandedOperatorController() { return this.m_operatorXbox; } + } diff --git a/src/main/java/frc4388/robot/commands/AutoBalance.java b/src/main/java/frc4388/robot/commands/AutoBalance.java index 714221d..a1c5910 100644 --- a/src/main/java/frc4388/robot/commands/AutoBalance.java +++ b/src/main/java/frc4388/robot/commands/AutoBalance.java @@ -35,7 +35,7 @@ public class AutoBalance extends PelvicInflammatoryDisease { public void runWithOutput(double output) { double out2 = MathUtil.clamp(output / 40, -.5, .5); if (Math.abs(getError()) < 3) out2 = 0; - drive.driveWithInput(new Translation2d(0, out2), new Translation2d(), true); + drive.driveWithInput(new Translation2d(0, out2), new Translation2d(), false); } @Override diff --git a/src/main/java/frc4388/robot/commands/BlueNearDriveToChargeStation.txt b/src/main/java/frc4388/robot/commands/BlueNearDriveToChargeStation.txt index e69de29..664216a 100644 --- a/src/main/java/frc4388/robot/commands/BlueNearDriveToChargeStation.txt +++ b/src/main/java/frc4388/robot/commands/BlueNearDriveToChargeStation.txt @@ -0,0 +1,758 @@ +0.0,0.0,0.0,0.0,0 +0.0,0.0,0.0,0.0,0 +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,79 +0.0,0.0,0.0,0.0,99 +0.0,0.0,0.0,0.0,119 +0.0,0.0,0.0,0.0,139 +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.0,0.0,0.0,220 +0.0,0.0,0.0,0.0,239 +0.0,0.0,0.0,0.0,260 +0.0,0.0,0.0,0.0,280 +0.0,0.0,0.0,0.0,300 +0.0,0.0,0.0,0.0,319 +0.0,0.0,0.0,0.0,340 +0.0,0.0,0.0,0.0,360 +0.0,0.0,0.0,0.0,379 +0.0,0.0,0.0,0.0,399 +0.0,0.0,0.0,0.0,419 +0.0,0.0,0.0,0.0,440 +0.0,0.0,0.0,0.0,459 +0.0,-0.109375,0.0,0.0,479 +0.0,-0.109375,0.0,0.0,499 +0.0,-0.109375,0.0,0.0,520 +0.0,-0.109375,0.0,0.0,540 +0.0,-0.1171875,0.0,0.0,559 +0.0,-0.1171875,0.0,0.0,580 +0.0,-0.1171875,0.0,0.0,599 +0.0,-0.1171875,0.0,0.0,621 +0.0,-0.1171875,0.0,0.0,639 +0.0,-0.1171875,0.0,0.0,661 +0.0,-0.1171875,0.0,0.0,680 +0.0,-0.1171875,0.0,0.0,699 +0.0,-0.1171875,0.0,0.0,721 +0.0,-0.1171875,0.0,0.0,769 +0.0,-0.125,0.0,0.0,778 +0.0,-0.1328125,0.0,0.0,790 +0.0,-0.1328125,0.0,0.0,802 +0.0,-0.1328125,0.0,0.0,820 +0.0,-0.1328125,0.0,0.0,840 +0.0,-0.1328125,0.0,0.0,860 +0.0,-0.1328125,0.0,0.0,879 +0.0,-0.140625,0.0,0.0,899 +0.0,-0.171875,0.0,0.0,933 +0.0,-0.1875,0.0,0.0,944 +0.0,-0.1875,0.0,0.0,959 +0.0,-0.203125,0.0,0.0,979 +0.0,-0.203125,0.0,0.0,999 +0.0,-0.203125,0.0,0.0,1019 +0.0,-0.203125,0.0,0.0,1039 +0.0,-0.203125,0.0,0.0,1059 +0.0,-0.203125,0.0,0.0,1079 +0.0,-0.2109375,0.0,0.0,1099 +0.0,-0.2109375,0.0,0.0,1120 +0.0,-0.2109375,0.0,0.0,1140 +0.0,-0.21875,0.0,0.0,1159 +0.0,-0.2109375,0.0,0.0,1180 +0.0,-0.2265625,0.0,0.0,1199 +0.0,-0.234375,0.0,0.0,1219 +0.0,-0.234375,0.0,0.0,1239 +0.0,-0.234375,0.0,0.0,1259 +0.0,-0.2421875,0.0,0.0,1280 +0.0,-0.2578125,0.0,0.0,1300 +0.0,-0.296875,0.0,0.0,1320 +0.0,-0.296875,0.0,0.0,1340 +0.0,-0.3046875,0.0,0.0,1359 +0.0,-0.3125,0.0,0.0,1379 +0.0,-0.3125,0.0,0.0,1399 +0.0,-0.3125,0.0,0.0,1419 +0.0,-0.3203125,0.0,0.0,1442 +0.0,-0.3203125,0.0,0.0,1460 +0.0,-0.3203125,0.0,0.0,1479 +0.0,-0.3203125,0.0,0.0,1499 +0.0,-0.3203125,0.0,0.0,1520 +0.0,-0.3203125,0.0,0.0,1540 +0.0,-0.3203125,0.0,0.0,1560 +0.0,-0.3203125,0.0,0.0,1579 +0.0,-0.3203125,0.0,0.0,1599 +0.0,-0.328125,0.0,0.0,1619 +0.0,-0.34375,0.0,0.0,1640 +0.0,-0.3515625,0.0,0.0,1659 +0.0,-0.359375,0.0,0.0,1680 +0.0,-0.359375,0.0,0.0,1699 +0.0,-0.359375,0.0,0.0,1720 +0.0,-0.359375,0.0,0.0,1739 +0.0,-0.3671875,0.0,0.0,1759 +0.0,-0.375,0.0,0.0,1780 +0.0,-0.390625,0.0,0.0,1799 +0.0,-0.4140625,0.0,0.0,1820 +0.0,-0.421875,0.0,0.0,1839 +0.0,-0.4296875,0.0,0.0,1859 +0.0,-0.4296875,0.0,0.0,1879 +0.0,-0.4296875,0.0,0.0,1900 +0.0,-0.4296875,0.0,0.0,1919 +0.0,-0.4296875,0.0,0.0,1939 +0.0,-0.4296875,0.0,0.0,1959 +0.0,-0.4296875,0.0,0.0,1979 +0.0,-0.4296875,0.0,0.0,2000 +0.0,-0.4296875,0.0,0.0,2019 +0.0,-0.4296875,0.0,0.0,2039 +0.0,-0.4296875,0.0,0.0,2060 +0.0,-0.4296875,0.0,0.0,2080 +0.0,-0.4296875,0.0,0.0,2099 +0.0,-0.4296875,0.0,0.0,2119 +0.0,-0.4296875,0.0,0.0,2139 +0.0,-0.4296875,0.0,0.0,2160 +0.0,-0.4375,0.0,0.0,2179 +0.0,-0.4453125,0.0,0.0,2199 +0.0,-0.453125,0.0,0.0,2220 +0.0,-0.453125,0.0,0.0,2240 +0.0,-0.453125,0.0,0.0,2260 +0.0,-0.453125,0.0,0.0,2280 +0.0,-0.453125,0.0,0.0,2300 +0.0,-0.453125,0.0,0.0,2320 +0.0,-0.453125,0.0,0.0,2340 +0.0,-0.453125,0.0,0.0,2359 +0.0,-0.453125,0.0,0.0,2379 +0.0,-0.453125,0.0,0.0,2399 +0.0,-0.453125,0.0,0.0,2419 +0.0,-0.453125,0.0,0.0,2440 +0.0,-0.453125,0.0,0.0,2460 +0.0,-0.453125,0.0,0.0,2479 +0.0,-0.453125,0.0,0.0,2500 +0.0,-0.453125,0.0,0.0,2520 +0.0,-0.453125,0.0,0.0,2540 +0.0,-0.453125,0.0,0.0,2559 +0.0,-0.453125,0.0,0.0,2579 +0.0,-0.4609375,0.0,0.0,2600 +0.0,-0.4609375,0.0,0.0,2620 +0.0,-0.4609375,0.0,0.0,2641 +0.0,-0.4609375,0.0,0.0,2660 +0.0,-0.4609375,0.0,0.0,2679 +0.0,-0.4609375,0.0,0.0,2700 +0.0,-0.4609375,0.0,0.0,2720 +0.0,-0.46875,0.0,0.0,2740 +0.0,-0.46875,0.0,0.0,2759 +0.0,-0.46875,0.0,0.0,2779 +0.0,-0.46875,0.0,0.0,2799 +0.0,-0.46875,0.0,0.0,2819 +0.0,-0.46875,0.0,0.0,2839 +0.0,-0.46875,0.0,0.0,2859 +0.0,-0.4765625,0.0,0.0,2880 +0.0,-0.5078125,0.0,0.0,2915 +0.0,-0.515625,0.0,0.0,2933 +0.0,-0.515625,0.0,0.0,2945 +0.0,-0.5234375,0.0,0.0,2960 +0.0,-0.5234375,0.0,0.0,2979 +0.0,-0.5234375,0.0,0.0,2999 +0.0,-0.5234375,0.0,0.0,3020 +0.0,-0.5234375,0.0,0.0,3040 +0.0,-0.5234375,0.0,0.0,3059 +0.0,-0.5234375,0.0,0.0,3079 +0.0,-0.5234375,0.0,0.0,3099 +0.0,-0.5234375,0.0,0.0,3119 +0.0,-0.5234375,0.0,0.0,3140 +0.0,-0.5234375,0.0,0.0,3159 +0.0,-0.5234375,0.0,0.0,3180 +0.0,-0.5234375,0.0,0.0,3199 +0.0,-0.5234375,0.0,0.0,3219 +0.0,-0.5234375,0.0,0.0,3239 +0.0,-0.5234375,0.0,0.0,3260 +0.0,-0.5234375,0.0,0.0,3279 +0.0,-0.5234375,0.0,0.0,3299 +0.0,-0.5234375,0.0,0.0,3320 +0.0,-0.5234375,0.0,0.0,3339 +0.0,-0.5234375,0.0,0.0,3359 +0.0,-0.5234375,0.0,0.0,3380 +0.0,-0.5234375,0.0,0.0,3400 +0.0,-0.5234375,0.0,0.0,3420 +0.0,-0.5234375,0.0,0.0,3439 +0.0,-0.5234375,0.0,0.0,3459 +0.0,-0.5234375,0.0,0.0,3479 +0.0,-0.5234375,0.0,0.0,3499 +0.0,-0.5234375,0.0,0.0,3520 +0.0,-0.5234375,0.0,0.0,3539 +0.0,-0.5234375,0.0,0.0,3559 +0.0,-0.5234375,0.0,0.0,3579 +0.0,-0.5234375,0.0,0.0,3600 +0.0,-0.5234375,0.0,0.0,3619 +0.0,-0.5234375,0.0,0.0,3640 +0.0,-0.5234375,0.0,0.0,3659 +0.0,-0.5234375,0.0,0.0,3679 +0.0,-0.5234375,0.0,0.0,3699 +0.0,-0.5234375,0.0,0.0,3720 +0.0,-0.5234375,0.0,0.0,3740 +0.0,-0.5234375,0.0,0.0,3759 +0.0,-0.5234375,0.0,0.0,3779 +0.0,-0.5234375,0.0,0.0,3800 +0.0,-0.5234375,0.0,0.0,3820 +0.0,-0.5234375,0.0,0.0,3840 +0.0,-0.5234375,0.0,0.0,3859 +0.0,-0.5234375,0.0,0.0,3879 +0.0,-0.5234375,0.0,0.0,3900 +0.0,-0.5390625,0.0,0.0,3920 +0.0,-0.546875,0.0,0.0,3939 +0.0,-0.546875,0.0,0.0,3959 +0.0,-0.546875,0.0,0.0,3979 +0.0,-0.546875,0.0,0.0,3999 +0.0,-0.546875,0.0,0.0,4019 +0.0,-0.5546875,0.0,0.0,4039 +0.0,-0.5546875,0.0,0.0,4059 +0.0,-0.5546875,0.0,0.0,4079 +0.0,-0.5546875,0.0,0.0,4134 +0.0,-0.5546875,0.0,0.0,4147 +0.0,-0.5546875,0.0,0.0,4157 +0.0,-0.5546875,0.0,0.0,4170 +0.0,-0.5546875,0.0,0.0,4185 +0.0,-0.5546875,0.0,0.0,4199 +0.0,-0.5546875,0.0,0.0,4219 +0.0,-0.5546875,0.0,0.0,4240 +-0.0234375,-0.5625,0.0,0.0,4259 +-0.03125,-0.5625,0.0,0.0,4279 +-0.03125,-0.5625,0.0,0.0,4300 +-0.03125,-0.5625,0.0,0.0,4319 +-0.03125,-0.5625,0.0,0.0,4339 +-0.0234375,-0.5625,0.0,0.0,4360 +-0.03125,-0.5625,0.0,0.0,4379 +-0.03125,-0.5625,0.0,0.0,4399 +-0.03125,-0.5625,0.0,0.0,4419 +-0.03125,-0.5625,0.0,0.0,4440 +-0.03125,-0.5625,0.0,0.0,4461 +-0.0390625,-0.5625,0.0,0.0,4479 +-0.046875,-0.5625,0.0,0.0,4499 +-0.0546875,-0.5625,0.0,0.0,4520 +-0.0625,-0.5625,0.0,0.0,4540 +-0.0625,-0.5625,0.0,0.0,4561 +-0.0546875,-0.5625,0.0,0.0,4581 +-0.0546875,-0.5625,0.0,0.0,4600 +-0.0546875,-0.5625,0.0,0.0,4620 +-0.0546875,-0.546875,0.0,0.0,4639 +-0.0546875,-0.546875,0.0,0.0,4659 +-0.0546875,-0.484375,0.0,0.0,4680 +-0.0546875,-0.4375,0.0,0.0,4700 +-0.0546875,-0.390625,0.0,0.0,4720 +-0.0546875,-0.3671875,0.0,0.0,4739 +-0.0546875,-0.3359375,0.0,0.0,4759 +-0.0546875,-0.3203125,0.0,0.0,4779 +-0.0546875,-0.2734375,0.0,0.0,4800 +0.0,0.0,0.0,0.0,4820 +0.0,0.0,0.0,0.0,4840 +0.0,0.0,0.0,0.0,4859 +0.0,0.0,0.0,0.0,4879 +0.0,0.0,0.0,0.0,4899 +0.0,0.0,0.0,0.0,4919 +0.0,0.0,0.0,0.0,4939 +0.0,0.0,0.0,0.0,4960 +0.0,0.0,0.0,0.0,4980 +0.0,0.0,0.0,0.0,4999 +0.0,0.0,0.0,0.0,5019 +0.0,0.0,0.0,0.0,5040 +0.0,0.0,0.0,0.0,5061 +0.0,0.0,0.0,0.0,5079 +0.0,0.0,0.0,0.0,5100 +0.0,0.0,0.0,0.0,5119 +0.0,0.0,0.0,0.0,5140 +0.0,0.0,0.0,0.0,5159 +0.0,0.0,0.0,0.0,5180 +0.0,0.0,0.0,0.0,5199 +0.0,0.0,0.0,0.0,5219 +0.0,0.0,0.0,0.0,5239 +0.0,0.0,0.0,0.0,5259 +0.0,0.0,0.0,0.0,5280 +0.0,0.0,0.0,0.0,5299 +0.0,0.0,0.0,0.0,5319 +0.0,0.0,0.0,0.0,5339 +0.0,0.0,0.0,0.0,5360 +0.0,0.0,0.0,0.0,5379 +0.0,0.0,0.0,0.0,5399 +0.0,0.0,0.0,0.0,5419 +0.0,0.0,0.0,0.0,5439 +0.0,0.0,0.0,0.0,5459 +0.0,0.0,0.0,0.0,5480 +0.0,0.0,0.0,0.0,5500 +0.0,0.0,0.0,0.0,5519 +0.0,0.0,0.0,0.0,5540 +0.0,0.0,0.0,0.0,5559 +0.0,0.0,0.0,0.0,5579 +0.0,0.0,0.0,0.0,5599 +0.0,0.0,0.0,0.0,5619 +0.0,0.0,0.0,0.0,5639 +0.0,0.0,0.0,0.0,5659 +0.0,0.0,0.0,0.0,5680 +0.0,0.0,0.0,0.0,5699 +0.0,0.0,0.0,0.0,5721 +0.0,0.0,0.0,0.0,5739 +0.0,0.0,0.0,0.0,5760 +0.0,0.0,0.0,0.0,5779 +0.0,0.0,0.0,0.0,5800 +0.0,0.0,0.0,0.0,5819 +0.0,0.0,0.0,0.0,5839 +0.0,0.0,0.0,0.0,5859 +0.0,0.0,0.0,0.0,5879 +0.0,0.0,0.0,0.0,5900 +0.0,0.0,0.0,0.0,5920 +0.0,0.0,0.0,0.0,5939 +0.0,0.0,0.0,0.0,5959 +0.0,0.0,0.0,0.0,5979 +-0.03125,0.11811023950576782,0.0,0.0,5999 +-0.03125,0.14173229038715363,0.0,0.0,6020 +-0.03125,0.14173229038715363,0.0,0.0,6039 +-0.03125,0.14173229038715363,0.0,0.0,6059 +-0.03125,0.14173229038715363,0.0,0.0,6079 +-0.03125,0.14173229038715363,0.0,0.0,6100 +-0.03125,0.14173229038715363,0.0,0.0,6119 +-0.03125,0.14173229038715363,0.0,0.0,6139 +-0.03125,0.14173229038715363,0.0,0.0,6159 +-0.03125,0.14173229038715363,0.0,0.0,6179 +-0.03125,0.14960630238056183,0.0,0.0,6199 +-0.03125,0.14960630238056183,0.0,0.0,6219 +-0.03125,0.14960630238056183,0.0,0.0,6239 +-0.03125,0.14960630238056183,0.0,0.0,6259 +-0.03125,0.14960630238056183,0.0,0.0,6280 +-0.03125,0.14960630238056183,0.0,0.0,6299 +-0.03125,0.16535432636737823,0.0,0.0,6319 +-0.03125,0.19685038924217224,0.0,0.0,6340 +-0.03125,0.20472441613674164,0.0,0.0,6360 +-0.03125,0.21259842813014984,0.0,0.0,6381 +-0.03125,0.21259842813014984,0.0,0.0,6399 +-0.03125,0.21259842813014984,0.0,0.0,6419 +-0.03125,0.21259842813014984,0.0,0.0,6440 +-0.03125,0.21259842813014984,0.0,0.0,6459 +-0.03125,0.21259842813014984,0.0,0.0,6479 +-0.03125,0.21259842813014984,0.0,0.0,6499 +-0.03125,0.21259842813014984,0.0,0.0,6519 +-0.03125,0.21259842813014984,0.0,0.0,6540 +-0.03125,0.21259842813014984,0.0,0.0,6559 +-0.03125,0.21259842813014984,0.0,0.0,6579 +-0.03125,0.21259842813014984,0.0,0.0,6599 +-0.03125,0.21259842813014984,0.0,0.0,6619 +-0.03125,0.21259842813014984,0.0,0.0,6640 +-0.03125,0.21259842813014984,0.0,0.0,6660 +-0.03125,0.21259842813014984,0.0,0.0,6679 +-0.03125,0.33070865273475647,0.0,0.0,6700 +-0.03125,0.33070865273475647,0.0,0.0,6720 +-0.03125,0.3700787425041199,0.0,0.0,6740 +-0.03125,0.3779527544975281,0.0,0.0,6760 +-0.03125,0.3779527544975281,0.0,0.0,6780 +-0.03125,0.3779527544975281,0.0,0.0,6800 +-0.03125,0.3779527544975281,0.0,0.0,6819 +-0.03125,0.3779527544975281,0.0,0.0,6839 +-0.03125,0.3700787425041199,0.0,0.0,6859 +-0.03125,0.3700787425041199,0.0,0.0,6879 +-0.03125,0.3700787425041199,0.0,0.0,6899 +-0.03125,0.3700787425041199,0.0,0.0,6920 +-0.03125,0.3700787425041199,0.0,0.0,6939 +-0.03125,0.3700787425041199,0.0,0.0,6959 +-0.03125,0.3700787425041199,0.0,0.0,6979 +-0.03125,0.3700787425041199,0.0,0.0,6999 +-0.03125,0.3700787425041199,0.0,0.0,7019 +-0.03125,0.3700787425041199,0.0,0.0,7039 +-0.03125,0.3700787425041199,0.0,0.0,7060 +-0.03125,0.3700787425041199,0.0,0.0,7080 +-0.03125,0.3700787425041199,0.0,0.0,7100 +-0.03125,0.3700787425041199,0.0,0.0,7119 +-0.03125,0.3700787425041199,0.0,0.0,7139 +-0.03125,0.3700787425041199,0.0,0.0,7159 +-0.03125,0.3700787425041199,0.0,0.0,7180 +-0.03125,0.3700787425041199,0.0,0.0,7199 +-0.03125,0.3700787425041199,0.0,0.0,7220 +-0.03125,0.3700787425041199,0.0,0.0,7239 +-0.03125,0.3700787425041199,0.0,0.0,7260 +-0.03125,0.36220473051071167,0.0,0.0,7279 +-0.03125,0.34645670652389526,0.0,0.0,7299 +0.0,0.33070865273475647,0.0,0.0,7319 +0.0,0.33070865273475647,0.0,0.0,7339 +0.0,0.3385826647281647,0.0,0.0,7359 +0.0,0.3385826647281647,0.0,0.0,7379 +0.0,0.3385826647281647,0.0,0.0,7399 +0.0,0.3385826647281647,0.0,0.0,7419 +0.0,0.3385826647281647,0.0,0.0,7440 +0.0,0.33070865273475647,0.0,0.0,7460 +0.0,0.31496062874794006,0.0,0.0,7479 +0.0,0.31496062874794006,0.0,0.0,7499 +0.0,0.29133859276771545,0.0,0.0,7519 +0.0,0.24409449100494385,0.0,0.0,7540 +0.0,0.24409449100494385,0.0,0.0,7559 +0.0,0.24409449100494385,0.0,0.0,7580 +0.0,0.24409449100494385,0.0,0.0,7600 +0.0,0.24409449100494385,0.0,0.0,7619 +0.0,0.24409449100494385,0.0,0.0,7639 +0.0,0.24409449100494385,0.0,0.0,7660 +0.0,0.24409449100494385,0.0,0.0,7679 +0.0,0.24409449100494385,0.0,0.0,7700 +0.0,0.25196850299835205,0.0,0.0,7720 +0.0,0.33070865273475647,0.0,0.0,7740 +0.0,0.3700787425041199,0.0,0.0,7760 +0.0,0.4251968562602997,0.0,0.0,7780 +0.0,0.4566929042339325,0.0,0.0,7800 +0.0,0.4645669162273407,0.0,0.0,7820 +0.0,0.4645669162273407,0.0,0.0,7839 +0.0,0.4645669162273407,0.0,0.0,7859 +0.0,0.4645669162273407,0.0,0.0,7880 +0.0,0.4645669162273407,0.0,0.0,7899 +0.0,0.4645669162273407,0.0,0.0,7920 +0.0,0.4645669162273407,0.0,0.0,7939 +0.0,0.4803149700164795,0.0,0.0,7960 +0.0,0.5039370059967041,0.0,0.0,7980 +-0.0078125,0.5433070659637451,0.0,0.0,7999 +-0.0078125,0.5748031735420227,0.0,0.0,8019 +-0.015625,0.5748031735420227,0.0,0.0,8039 +-0.015625,0.5748031735420227,0.0,0.0,8059 +-0.015625,0.5748031735420227,0.0,0.0,8080 +-0.015625,0.5511810779571533,0.0,0.0,8099 +-0.015625,0.5354330539703369,0.0,0.0,8119 +-0.015625,0.5354330539703369,0.0,0.0,8139 +-0.015625,0.5354330539703369,0.0,0.0,8159 +-0.0078125,0.5354330539703369,0.0,0.0,8180 +-0.0078125,0.5354330539703369,0.0,0.0,8199 +-0.0078125,0.5354330539703369,0.0,0.0,8220 +-0.0078125,0.5354330539703369,0.0,0.0,8239 +-0.0078125,0.5354330539703369,0.0,0.0,8259 +-0.0078125,0.5354330539703369,0.0,0.0,8279 +-0.0078125,0.5354330539703369,0.0,0.0,8299 +-0.0078125,0.5354330539703369,0.0,0.0,8319 +-0.0078125,0.5354330539703369,0.0,0.0,8340 +-0.0078125,0.5275590419769287,0.0,0.0,8359 +-0.0078125,0.4803149700164795,0.0,0.0,8379 +-0.0078125,0.4803149700164795,0.0,0.0,8399 +-0.0078125,0.4803149700164795,0.0,0.0,8421 +-0.0078125,0.4803149700164795,0.0,0.0,8440 +-0.0078125,0.4803149700164795,0.0,0.0,8460 +-0.0078125,0.4803149700164795,0.0,0.0,8479 +0.0,0.4803149700164795,0.0,0.0,8499 +0.0,0.4803149700164795,0.0,0.0,8520 +0.0,0.4803149700164795,0.0,0.0,8540 +0.0,0.4803149700164795,0.0,0.0,8559 +0.0,0.4803149700164795,0.0,0.0,8579 +0.0,0.4803149700164795,0.0,0.0,8600 +0.0,0.4803149700164795,0.0,0.0,8619 +0.0,0.4803149700164795,0.0,0.0,8639 +0.0,0.4803149700164795,0.0,0.0,8659 +0.0,0.4724409580230713,0.0,0.0,8679 +0.0,0.4409448802471161,0.0,0.0,8699 +0.0,0.4173228442668915,0.0,0.0,8719 +0.0,0.4094488322734833,0.0,0.0,8739 +0.0,0.4094488322734833,0.0,0.0,8759 +0.0,0.4094488322734833,0.0,0.0,8779 +0.015748031437397003,0.4094488322734833,0.0,0.0,8799 +0.04724409431219101,0.4094488322734833,0.0,0.0,8820 +0.07874015718698502,0.4094488322734833,0.0,0.0,8840 +0.07874015718698502,0.4094488322734833,0.0,0.0,8859 +0.07874015718698502,0.4094488322734833,0.0,0.0,8879 +0.07874015718698502,0.4251968562602997,0.0,0.0,8899 +0.07874015718698502,0.4488188922405243,0.0,0.0,8920 +0.07874015718698502,0.4488188922405243,0.0,0.0,8939 +0.07874015718698502,0.4488188922405243,0.0,0.0,8959 +0.07874015718698502,0.4488188922405243,0.0,0.0,8980 +0.05511811003088951,0.4488188922405243,0.0,0.0,8999 +0.031496062874794006,0.4488188922405243,0.0,0.0,9019 +0.031496062874794006,0.4488188922405243,0.0,0.0,9039 +0.031496062874794006,0.4488188922405243,0.0,0.0,9059 +0.031496062874794006,0.4488188922405243,0.0,0.0,9079 +0.031496062874794006,0.4488188922405243,0.0,0.0,9099 +0.031496062874794006,0.4488188922405243,0.0,0.0,9120 +0.031496062874794006,0.4488188922405243,0.0,0.0,9139 +0.031496062874794006,0.4488188922405243,0.0,0.0,9159 +0.031496062874794006,0.4488188922405243,0.0,0.0,9179 +0.031496062874794006,0.4488188922405243,0.0,0.0,9199 +0.031496062874794006,0.4488188922405243,0.0,0.0,9219 +0.031496062874794006,0.4488188922405243,0.0,0.0,9239 +0.04724409431219101,0.4488188922405243,0.0,0.0,9260 +0.06299212574958801,0.4488188922405243,0.0,0.0,9279 +0.06299212574958801,0.4488188922405243,0.0,0.0,9299 +0.06299212574958801,0.4488188922405243,0.0,0.0,9319 +0.06299212574958801,0.4488188922405243,0.0,0.0,9339 +0.07874015718698502,0.4488188922405243,0.0,0.0,9359 +0.07874015718698502,0.4488188922405243,0.0,0.0,9380 +0.07874015718698502,0.4488188922405243,0.0,0.0,9400 +0.07874015718698502,0.4488188922405243,0.0,0.0,9419 +0.07874015718698502,0.4488188922405243,0.0,0.0,9439 +0.07874015718698502,0.4488188922405243,0.0,0.0,9460 +0.07874015718698502,0.4488188922405243,0.0,0.0,9480 +0.07874015718698502,0.4488188922405243,0.0,0.0,9499 +0.07874015718698502,0.4488188922405243,0.0,0.0,9519 +0.07874015718698502,0.4488188922405243,0.0,0.0,9539 +0.07874015718698502,0.4488188922405243,0.0,0.0,9559 +0.07874015718698502,0.4488188922405243,0.0,0.0,9579 +0.07874015718698502,0.4488188922405243,0.0,0.0,9600 +0.07874015718698502,0.4488188922405243,0.0,0.0,9619 +0.07874015718698502,0.4488188922405243,0.0,0.0,9639 +0.07874015718698502,0.4488188922405243,0.0,0.0,9659 +0.07874015718698502,0.4488188922405243,0.0,0.0,9679 +0.07874015718698502,0.4488188922405243,0.0,0.0,9699 +0.07874015718698502,0.4488188922405243,0.0,0.0,9722 +0.07874015718698502,0.4488188922405243,0.0,0.0,9739 +0.07874015718698502,0.4488188922405243,0.0,0.0,9759 +0.08661417663097382,0.4488188922405243,0.0,0.0,9780 +0.08661417663097382,0.4488188922405243,0.0,0.0,9800 +0.11811023950576782,0.4488188922405243,0.0,0.0,9820 +0.14173229038715363,0.4488188922405243,0.0,0.0,9839 +0.16535432636737823,0.4488188922405243,0.0,0.0,9859 +0.22834645211696625,0.4488188922405243,0.0,0.0,9879 +0.23622047901153564,0.4488188922405243,0.0,0.0,9899 +0.23622047901153564,0.4409448802471161,0.0,0.0,9919 +0.25984251499176025,0.4094488322734833,0.0,0.0,9940 +0.29921260476112366,0.3858267664909363,0.0,0.0,9959 +0.31496062874794006,0.3779527544975281,0.0,0.0,9979 +0.32283464074134827,0.3700787425041199,0.0,0.0,9999 +0.32283464074134827,0.3700787425041199,0.0,0.0,10019 +0.36220473051071167,0.33070865273475647,0.0,0.0,10039 +0.3779527544975281,0.25984251499176025,0.0,0.0,10059 +0.3937007784843445,0.21259842813014984,0.0,0.0,10081 +0.3937007784843445,0.20472441613674164,0.0,0.0,10100 +0.3937007784843445,0.20472441613674164,0.0,0.0,10124 +0.3937007784843445,0.20472441613674164,0.0,0.0,10140 +0.3937007784843445,0.18897637724876404,0.0,0.0,10159 +0.3937007784843445,0.18897637724876404,0.0,0.0,10180 +0.3937007784843445,0.16535432636737823,0.0,0.0,10199 +0.3937007784843445,0.16535432636737823,0.0,0.0,10221 +0.3937007784843445,0.17322835326194763,0.0,0.0,10239 +0.3937007784843445,0.16535432636737823,0.0,0.0,10260 +0.3937007784843445,0.14960630238056183,0.0,0.0,10279 +0.3937007784843445,0.14960630238056183,0.0,0.0,10300 +0.3937007784843445,0.14960630238056183,0.0,0.0,10319 +0.3937007784843445,0.14960630238056183,0.0,0.0,10340 +0.3937007784843445,0.14173229038715363,0.0,0.0,10360 +0.3937007784843445,0.14173229038715363,0.0,0.0,10380 +0.3937007784843445,0.14173229038715363,0.0,0.0,10399 +0.3937007784843445,0.14173229038715363,0.0,0.0,10419 +0.3937007784843445,0.14173229038715363,0.0,0.0,10440 +0.3937007784843445,0.14173229038715363,0.0,0.0,10459 +0.3937007784843445,0.14173229038715363,0.0,0.0,10479 +0.3937007784843445,0.14173229038715363,0.0,0.0,10500 +0.3937007784843445,0.14173229038715363,0.0,0.0,10519 +0.4015747904777527,0.14173229038715363,0.0,0.0,10540 +0.4173228442668915,0.14173229038715363,0.0,0.0,10559 +0.4251968562602997,0.13385826349258423,0.0,0.0,10580 +0.4251968562602997,0.12598425149917603,0.0,0.0,10600 +0.4251968562602997,0.10236220806837082,0.0,0.0,10620 +0.4251968562602997,0.10236220806837082,0.0,0.0,10639 +0.4330708682537079,0.10236220806837082,0.0,0.0,10659 +0.4409448802471161,0.10236220806837082,0.0,0.0,10680 +0.4488188922405243,0.07086614519357681,0.0,0.0,10699 +0.4566929042339325,0.023622047156095505,0.0,0.0,10719 +0.4566929042339325,0.0,0.0,0.0,10739 +0.4645669162273407,0.0,0.0,0.0,10760 +0.4645669162273407,0.0,0.0,0.0,10779 +0.4724409580230713,0.0,0.0,0.0,10800 +0.4724409580230713,0.0,0.0,0.0,10819 +0.4724409580230713,0.0,0.0,0.0,10839 +0.4803149700164795,0.0,0.0,0.0,10860 +0.4881889820098877,0.0,0.0,0.0,10879 +0.5039370059967041,0.0,0.0,0.0,10899 +0.5275590419769287,0.0,0.0,0.0,10919 +0.5354330539703369,0.0,0.0,0.0,10940 +0.5354330539703369,0.0,0.0,0.0,10960 +0.5354330539703369,0.0,0.0,0.0,10979 +0.5354330539703369,0.0,0.0,0.0,10999 +0.5354330539703369,0.0,0.0,0.0,11020 +0.5354330539703369,0.0,0.0,0.0,11039 +0.5354330539703369,0.0,0.0,0.0,11059 +0.5354330539703369,0.0,0.0,0.0,11079 +0.5354330539703369,0.0,0.0,0.0,11099 +0.5354330539703369,0.0,0.0,0.0,11119 +0.5354330539703369,0.0,0.0,0.0,11140 +0.5354330539703369,0.0,0.0,0.0,11159 +0.5354330539703369,0.0,0.0,0.0,11179 +0.5354330539703369,0.0,0.0,0.0,11200 +0.5354330539703369,0.0,0.0,0.0,11219 +0.5354330539703369,0.0,0.0,0.0,11239 +0.5354330539703369,0.0,0.0,0.0,11260 +0.5354330539703369,0.0,0.0,0.0,11279 +0.5354330539703369,0.0,0.0,0.0,11299 +0.5275590419769287,0.0,0.0,0.0,11319 +0.5275590419769287,0.0,0.0,0.0,11339 +0.5275590419769287,0.0,0.0,0.0,11360 +0.5275590419769287,0.0,0.0,0.0,11379 +0.5275590419769287,0.0,0.0,0.0,11399 +0.5275590419769287,0.0,0.0,0.0,11419 +0.5275590419769287,0.0,0.0,0.0,11440 +0.5275590419769287,0.0,0.0,0.0,11459 +0.5275590419769287,0.0,0.0,0.0,11479 +0.5275590419769287,0.0,0.0,0.0,11500 +0.5275590419769287,0.0,0.0,0.0,11519 +0.5275590419769287,0.0,0.0,0.0,11539 +0.5275590419769287,0.0,0.0,0.0,11559 +0.5275590419769287,0.0,0.0,0.0,11579 +0.5118110179901123,0.0,0.0,0.0,11599 +0.5118110179901123,0.0,0.0,0.0,11620 +0.5118110179901123,0.0,0.0,0.0,11640 +0.5039370059967041,0.0,0.0,0.0,11660 +0.4803149700164795,0.0,0.0,0.0,11679 +0.4566929042339325,0.0,0.0,0.0,11699 +0.4330708682537079,0.0,0.0,0.0,11720 +0.4173228442668915,0.0,0.0,0.0,11739 +0.4094488322734833,0.0,0.0,0.0,11760 +0.4094488322734833,0.0,0.0,0.0,11780 +0.4094488322734833,0.0,0.0,0.0,11800 +0.4094488322734833,0.0,0.0,0.0,11819 +0.4094488322734833,0.0,0.0,0.0,11840 +0.3937007784843445,0.0,0.0,0.0,11859 +0.25196850299835205,-0.0390625,0.0,0.0,11879 +0.11811023950576782,-0.078125,0.0,0.0,11900 +0.06299212574958801,-0.078125,0.0,0.0,11920 +0.0,0.0,0.0,0.0,11939 +0.0,0.0,0.0,0.0,11959 +0.0,0.0,0.0,0.0,11979 +0.0,0.0,0.0,0.0,12000 +0.0,0.0,0.0,0.0,12019 +0.0,0.0,0.0,0.0,12040 +0.0,0.0,0.0,0.0,12060 +0.0,0.0,0.0,0.0,12080 +0.0,0.0,0.0,0.0,12099 +0.0,0.0,0.0,0.0,12119 +0.0,-0.1015625,0.0,0.0,12139 +0.0,-0.1015625,0.0,0.0,12159 +0.0,-0.109375,0.0,0.0,12179 +0.0,-0.1171875,0.0,0.0,12199 +0.0,-0.1171875,0.0,0.0,12219 +0.0,-0.1171875,0.0,0.0,12240 +0.0,-0.1328125,0.0,0.0,12259 +0.0,-0.15625,0.0,0.0,12279 +0.0,-0.1953125,0.0,0.0,12300 +0.0,-0.21875,0.0,0.0,12319 +0.0,-0.28125,0.0,0.0,12339 +0.0,-0.296875,0.0,0.0,12360 +0.0,-0.3046875,0.0,0.0,12379 +0.0,-0.3125,0.0,0.0,12399 +0.0,-0.3359375,0.0,0.0,12419 +0.0,-0.3359375,0.0,0.0,12440 +0.0,-0.328125,0.0,0.0,12459 +0.0,-0.328125,0.0,0.0,12479 +0.0,-0.328125,0.0,0.0,12499 +0.0,-0.328125,0.0,0.0,12520 +0.0,-0.328125,0.0,0.0,12539 +0.0,-0.328125,0.0,0.0,12560 +0.0,-0.328125,0.0,0.0,12580 +0.0,-0.328125,0.0,0.0,12599 +0.0,-0.328125,0.0,0.0,12620 +0.0,-0.328125,0.0,0.0,12639 +0.0,-0.328125,0.0,0.0,12659 +0.0,-0.328125,0.0,0.0,12679 +0.0,-0.328125,0.0,0.0,12699 +0.0,-0.328125,0.0,0.0,12719 +0.0,-0.328125,0.0,0.0,12739 +0.0,-0.328125,0.0,0.0,12760 +0.0,-0.34375,0.0,0.0,12780 +0.0,-0.3515625,0.0,0.0,12799 +0.0,-0.359375,0.0,0.0,12819 +0.0,-0.359375,0.0,0.0,12840 +0.0,-0.359375,0.0,0.0,12860 +0.0,-0.359375,0.0,0.0,12880 +0.0,-0.359375,0.0,0.0,12900 +0.0,-0.3671875,0.0,0.0,12921 +0.0,-0.3671875,0.0,0.0,12939 +0.0,-0.375,0.0,0.0,12959 +0.0,-0.375,0.0,0.0,12979 +0.0,-0.390625,0.0,0.0,12999 +0.015748031437397003,-0.40625,0.0,0.0,13019 +0.023622047156095505,-0.421875,0.0,0.0,13039 +0.03937007859349251,-0.4296875,0.0,0.0,13060 +0.03937007859349251,-0.4296875,0.0,0.0,13079 +0.04724409431219101,-0.453125,0.0,0.0,13099 +0.05511811003088951,-0.46875,0.0,0.0,13119 +0.05511811003088951,-0.484375,0.0,0.0,13139 +0.05511811003088951,-0.5,0.0,0.0,13160 +0.05511811003088951,-0.515625,0.0,0.0,13179 +0.05511811003088951,-0.515625,0.0,0.0,13199 +0.05511811003088951,-0.515625,0.0,0.0,13219 +0.05511811003088951,-0.5234375,0.0,0.0,13239 +0.05511811003088951,-0.5234375,0.0,0.0,13260 +0.05511811003088951,-0.5234375,0.0,0.0,13279 +0.05511811003088951,-0.5234375,0.0,0.0,13299 +0.05511811003088951,-0.5234375,0.0,0.0,13319 +0.04724409431219101,-0.5234375,0.0,0.0,13339 +0.04724409431219101,-0.5234375,0.0,0.0,13359 +0.04724409431219101,-0.5234375,0.0,0.0,13379 +0.04724409431219101,-0.5234375,0.0,0.0,13400 +0.04724409431219101,-0.5234375,0.0,0.0,13419 +0.04724409431219101,-0.5234375,0.0,0.0,13439 +0.04724409431219101,-0.5234375,0.0,0.0,13459 +0.04724409431219101,-0.5234375,0.0,0.0,13479 +0.04724409431219101,-0.5234375,0.0,0.0,13499 +0.04724409431219101,-0.5234375,0.0,0.0,13519 +0.04724409431219101,-0.5234375,0.0,0.0,13539 +0.04724409431219101,-0.5234375,0.0,0.0,13559 +0.04724409431219101,-0.5234375,0.0,0.0,13579 +0.04724409431219101,-0.5234375,0.0,0.0,13599 +0.04724409431219101,-0.5234375,0.0,0.0,13620 +0.04724409431219101,-0.5234375,0.0,0.0,13642 +0.04724409431219101,-0.5234375,0.0,0.0,13659 +0.04724409431219101,-0.5234375,0.0,0.0,13679 +0.04724409431219101,-0.5234375,0.0,0.0,13699 +0.04724409431219101,-0.5234375,0.0,0.0,13720 +0.04724409431219101,-0.5234375,0.0,0.0,13740 +0.04724409431219101,-0.5234375,0.0,0.0,13760 +0.04724409431219101,-0.5234375,0.0,0.0,13779 +0.04724409431219101,-0.53125,0.0,0.0,13800 +0.04724409431219101,-0.546875,0.0,0.0,13819 +0.04724409431219101,-0.5625,0.0,0.0,13840 +0.04724409431219101,-0.578125,0.0,0.0,13859 +0.04724409431219101,-0.59375,0.0,0.0,13880 +0.07874015718698502,-0.6875,0.0,0.0,13900 +0.07874015718698502,-0.703125,0.0,0.0,13919 +0.07874015718698502,-0.7265625,0.0,0.0,13944 +0.07874015718698502,-0.7421875,0.0,0.0,13959 +0.07874015718698502,-0.765625,0.0,0.0,13980 +0.07874015718698502,-0.78125,0.0,0.0,14000 +0.07874015718698502,-0.7890625,0.0,0.0,14020 +0.07874015718698502,-0.8125,0.0,0.0,14039 +0.07874015718698502,-0.8203125,0.0,0.0,14060 +0.07874015718698502,-0.828125,0.0,0.0,14079 +0.07874015718698502,-0.8359375,0.0,0.0,14099 +0.07874015718698502,-0.8359375,0.0,0.0,14119 +0.07874015718698502,-0.8359375,0.0,0.0,14139 +0.07874015718698502,-0.8359375,0.0,0.0,14159 +0.07874015718698502,-0.8359375,0.0,0.0,14179 +0.07874015718698502,-0.8203125,0.0,0.0,14199 +0.07874015718698502,-0.78125,0.0,0.0,14219 +0.007874015718698502,-0.265625,0.0,0.0,14239 +0.0,0.0,0.0,0.0,14259 +0.0,0.0,0.0,0.0,14279 +0.0,-0.1171875,0.0,0.0,14299 +0.0,-0.1328125,0.0,0.0,14319 +0.0,-0.171875,0.0,0.0,14339 +0.0,-0.171875,0.0,0.0,14359 +0.0,0.0,0.0,0.0,14379 +0.0,0.0,0.0,0.0,14399 +0.0,0.0,0.0,0.0,14419 +0.0,0.0,0.0,0.0,14439 +0.0,0.0,0.0,0.0,14459 +0.0,0.0,0.0,0.0,14479 +0.0,0.0,0.0,0.0,14499 +0.0,0.0,0.0,0.0,14519 +0.0,-0.203125,0.0,0.0,14539 +0.0,-0.4609375,0.0,0.0,14559 +0.031496062874794006,-0.71875,0.0,0.0,14579 +0.031496062874794006,-0.9453125,0.0,0.0,14599 +0.03148045240972986,-0.999504367732367,0.0,0.0,14619 +0.03148045240972986,-0.999504367732367,0.0,0.0,14639 +0.03148045240972986,-0.999504367732367,0.0,0.0,14660 +0.03148045240972986,-0.999504367732367,0.0,0.0,14679 +0.03148045240972986,-0.999504367732367,0.0,0.0,14699 +0.03148045240972986,-0.999504367732367,0.0,0.0,14719 +0.031496062874794006,-0.9609375,0.0,0.0,14739 +0.031496062874794006,-0.9609375,0.0,0.0,14759 +0.031496062874794006,-0.9609375,0.0,0.0,14780 +0.031496062874794006,-0.9609375,0.0,0.0,14799 +0.031496062874794006,-0.953125,0.0,0.0,14820 +0.031496062874794006,-0.9609375,0.0,0.0,14839 +0.03148045240972986,-0.999504367732367,0.0,0.0,14860 +0.03148045240972986,-0.999504367732367,0.0,0.0,14880 +0.03148045240972986,-0.999504367732367,0.0,0.0,14900 +0.03148045240972986,-0.999504367732367,0.0,0.0,14919 +0.031496062874794006,-0.984375,0.0,0.0,14939 +0.0,-0.3515625,0.0,0.0,14959 +0.0,0.0,0.0,0.0,14980 +0.0,0.0,0.0,0.0,14999 +0.0,0.0,0.0,0.0,15019 +0.0,0.0,0.0,0.0,15039 +0.0,0.0,0.0,0.0,15059 +0.0,0.0,0.0,0.0,15082 +0.0,0.0,0.0,0.0,15099 +0.0,0.0,0.0,0.0,15120 \ 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 f8dfb82..0d7facc 100644 --- a/src/main/java/frc4388/robot/commands/JoystickPlayback.java +++ b/src/main/java/frc4388/robot/commands/JoystickPlayback.java @@ -22,10 +22,12 @@ public class JoystickPlayback extends CommandBase { private long playbackTime = 0; private int lastIndex; private boolean m_finished = false; // ! find a better way + private int m_mult = 1; /** Creates a new JoystickPlayback. */ - public JoystickPlayback(SwerveDrive swerve) { + public JoystickPlayback(SwerveDrive swerve, int mult) { this.swerve = swerve; + m_mult = mult; addRequirements(this.swerve); } @@ -41,11 +43,16 @@ public class JoystickPlayback extends CommandBase { String line = ""; while (input.hasNextLine()) { line = input.nextLine(); + + if (line.isEmpty() || line.isBlank() || line.equals("\n")) { + continue; + } String[] values = line.split(","); + System.out.println("values: " + values[0] + " " + values[1] + " " + values[2] + " " + values[3]); var out = new TimedOutput(); - out.leftX = Double.parseDouble(values[0]); + out.leftX = Double.parseDouble(values[0]) * m_mult; out.leftY = Double.parseDouble(values[1]); out.rightX = Double.parseDouble(values[2]); out.rightY = Double.parseDouble(values[3]); @@ -59,8 +66,6 @@ public class JoystickPlayback extends CommandBase { } catch (FileNotFoundException e) { e.printStackTrace(); } - - System.out.println("STARTING PLAYBACK"); } // Called every time the scheduler runs while the command is scheduled. @@ -106,8 +111,7 @@ public class JoystickPlayback extends CommandBase { this.swerve.driveWithInput( new Translation2d(lerpLX, lerpLY), new Translation2d(lerpRX, lerpRY), true); - - System.out.println("PLAYING"); + counter++; } From 0469e17da30af25fc3a87191e039e4e95287f89b Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 25 Feb 2023 12:51:23 -0700 Subject: [PATCH 26/40] autoChooser + playback filename --- .../java/frc4388/robot/RobotContainer.java | 33 ++++++++++++++++--- .../robot/commands/JoystickPlayback.java | 18 +++++++--- 2 files changed, 42 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 8412738..d2b28fe 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -23,6 +23,7 @@ import edu.wpi.first.math.trajectory.TrajectoryConfig; import edu.wpi.first.math.trajectory.TrajectoryGenerator; import edu.wpi.first.math.trajectory.Trajectory.State; import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -62,6 +63,20 @@ public class RobotContainer { private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); + /* Autos */ + private SendableChooser chooser = new SendableChooser<>(); + + private Command noAuto = new InstantCommand(); + + private Command balance = new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive); + + private Command blue1Path = new JoystickPlayback(m_robotSwerveDrive, "BlueNearDriveToChargeStation.txt"); + private Command blue1PathWithBalance = blue1Path.andThen(balance); + + private Command red1Path = new JoystickPlayback(m_robotSwerveDrive, "BlueNearDriveToChargeStation.txt", -1); + private Command red1PathWithBalance = red1Path.andThen(balance); + + /** * The container for the robot. Contains subsystems, OI devices, and commands. */ @@ -75,6 +90,17 @@ public class RobotContainer { true); }, m_robotSwerveDrive) .withName("SwerveDrive DefaultCommand")); + + // * Auto Commands + chooser.setDefaultOption("NoAuto", noAuto); + + chooser.addOption("Blue1Path", blue1Path); + chooser.addOption("Blue1PathWithBalance", blue1PathWithBalance); + + chooser.addOption("Red1Path", red1Path); + chooser.addOption("Red1PathWithBalance", red1PathWithBalance); + + SmartDashboard.putData(chooser); } @@ -104,8 +130,8 @@ public class RobotContainer { () -> getDeadbandedDriverController().getRightY())) .onFalse(new InstantCommand()); - new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) - .onTrue(new JoystickPlayback(m_robotSwerveDrive, 1)); + // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) + // .onTrue(new JoystickPlayback(m_robotSwerveDrive, 1)); // * Operator Buttons } @@ -117,8 +143,7 @@ public class RobotContainer { */ public Command getAutonomousCommand() { - return new JoystickPlayback(m_robotSwerveDrive, 1) - .andThen(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); + return chooser.getSelected(); // return new InstantCommand(); } diff --git a/src/main/java/frc4388/robot/commands/JoystickPlayback.java b/src/main/java/frc4388/robot/commands/JoystickPlayback.java index 0d7facc..c844abd 100644 --- a/src/main/java/frc4388/robot/commands/JoystickPlayback.java +++ b/src/main/java/frc4388/robot/commands/JoystickPlayback.java @@ -15,6 +15,8 @@ import frc4388.utility.UtilityStructs.TimedOutput; public class JoystickPlayback extends CommandBase { private final SwerveDrive swerve; + private String filename; + private int mult = 1; private Scanner input; private final ArrayList outputs = new ArrayList<>(); private int counter = 0; @@ -22,15 +24,21 @@ public class JoystickPlayback extends CommandBase { private long playbackTime = 0; private int lastIndex; private boolean m_finished = false; // ! find a better way - private int m_mult = 1; /** Creates a new JoystickPlayback. */ - public JoystickPlayback(SwerveDrive swerve, int mult) { + public JoystickPlayback(SwerveDrive swerve, String filename, int mult) { this.swerve = swerve; - m_mult = mult; + this.filename = filename; + this.mult = mult; + addRequirements(this.swerve); } + /** Creates a new JoystickPlayback. */ + public JoystickPlayback(SwerveDrive swerve, String filename) { + this(swerve, filename, 1); + } + // Called when the command is initially scheduled. @Override public void initialize() { @@ -38,7 +46,7 @@ public class JoystickPlayback extends CommandBase { playbackTime = 0; lastIndex = 0; try { - input = new Scanner(new File("/home/lvuser/BlueNearDriveToChargeStation.txt")); + input = new Scanner(new File("/home/lvuser/" + filename)); String line = ""; while (input.hasNextLine()) { @@ -52,7 +60,7 @@ public class JoystickPlayback extends CommandBase { System.out.println("values: " + values[0] + " " + values[1] + " " + values[2] + " " + values[3]); var out = new TimedOutput(); - out.leftX = Double.parseDouble(values[0]) * m_mult; + out.leftX = Double.parseDouble(values[0]) * mult; out.leftY = Double.parseDouble(values[1]); out.rightX = Double.parseDouble(values[2]); out.rightY = Double.parseDouble(values[3]); From 2031ace20404a60b0cc5d812a7eeeaf63dfb7fca Mon Sep 17 00:00:00 2001 From: Aarav Date: Sat, 25 Feb 2023 13:29:52 -0700 Subject: [PATCH 27/40] auto chooser changes --- .../java/frc4388/robot/RobotContainer.java | 18 +- .../java/frc4388/robot/commands/Blue1Path.txt | 150 ++++ .../commands/BlueNearDriveToChargeStation.txt | 758 ------------------ .../robot/commands/FarDriveToChargeStation | 0 .../robot/commands/JoystickRecorder.java | 7 +- src/main/java/frc4388/robot/commands/Taxi.txt | 356 ++++++++ 6 files changed, 522 insertions(+), 767 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/Blue1Path.txt delete mode 100644 src/main/java/frc4388/robot/commands/BlueNearDriveToChargeStation.txt delete mode 100644 src/main/java/frc4388/robot/commands/FarDriveToChargeStation create mode 100644 src/main/java/frc4388/robot/commands/Taxi.txt diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index d2b28fe..279be06 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -70,12 +70,13 @@ public class RobotContainer { private Command balance = new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive); - private Command blue1Path = new JoystickPlayback(m_robotSwerveDrive, "BlueNearDriveToChargeStation.txt"); - private Command blue1PathWithBalance = blue1Path.andThen(balance); + private Command blue1Path = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt"); + private Command blue1PathWithBalance = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt").andThen(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); - private Command red1Path = new JoystickPlayback(m_robotSwerveDrive, "BlueNearDriveToChargeStation.txt", -1); - private Command red1PathWithBalance = red1Path.andThen(balance); + private Command red1Path = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt", -1); + private Command red1PathWithBalance = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt", -1).andThen(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); + private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"); /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -99,6 +100,8 @@ public class RobotContainer { chooser.addOption("Red1Path", red1Path); chooser.addOption("Red1PathWithBalance", red1PathWithBalance); + + chooser.addOption("Taxi", taxi); SmartDashboard.putData(chooser); } @@ -127,11 +130,12 @@ public class RobotContainer { () -> getDeadbandedDriverController().getLeftX(), () -> getDeadbandedDriverController().getLeftY(), () -> getDeadbandedDriverController().getRightX(), - () -> getDeadbandedDriverController().getRightY())) + () -> getDeadbandedDriverController().getRightY(), + "Blue1Path.txt")) .onFalse(new InstantCommand()); - // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) - // .onTrue(new JoystickPlayback(m_robotSwerveDrive, 1)); + new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) + .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt")); // * Operator Buttons } diff --git a/src/main/java/frc4388/robot/commands/Blue1Path.txt b/src/main/java/frc4388/robot/commands/Blue1Path.txt new file mode 100644 index 0000000..70d9ee7 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Blue1Path.txt @@ -0,0 +1,150 @@ +0.0,0.0,0.0,0.0,0 +0.0,0.0,0.0,0.0,2 +0.0,0.0,0.0,0.0,18 +0.0,0.0,0.0,0.0,31 +0.0,0.0,0.0,0.0,61 +0.0,0.0,0.0,0.0,72 +0.0,0.0,0.0,0.0,85 +0.0,0.0,0.0,0.0,102 +0.0,0.0,0.0,0.0,116 +0.0,0.0,0.0,0.0,163 +0.0,0.0,0.0,0.0,175 +0.0,0.0,0.0,0.0,188 +0.0,0.0,0.0,0.0,205 +0.0,0.0,0.0,0.0,217 +0.0,0.0,0.0,0.0,229 +0.0,0.0,0.0,0.0,241 +0.0,0.0,0.0,0.0,261 +0.0,0.0,0.0,0.0,281 +0.0,0.0,0.0,0.0,302 +0.0,0.0,0.0,0.0,322 +0.0,0.0,0.0,0.0,341 +0.0,0.0,0.0,0.0,361 +0.0,-0.1328125,0.0,0.0,397 +0.0,-0.1328125,0.0,0.0,412 +0.0,-0.1484375,0.0,0.0,425 +0.0,-0.1796875,0.0,0.0,441 +0.0,-0.1875,0.0,0.0,461 +0.0,-0.1953125,0.0,0.0,481 +0.0,-0.234375,0.0,0.0,502 +0.0,-0.2890625,0.0,0.0,521 +0.0,-0.3125,0.0,0.0,541 +0.0,-0.328125,0.0,0.0,561 +0.0,-0.3671875,0.0,0.0,582 +0.0,-0.390625,0.0,0.0,601 +0.0,-0.4609375,0.0,0.0,642 +0.0,-0.4765625,0.0,0.0,653 +0.0,-0.4765625,0.0,0.0,666 +0.0,-0.4765625,0.0,0.0,681 +0.0,-0.4765625,0.0,0.0,702 +0.0,-0.4765625,0.0,0.0,721 +0.0,-0.4765625,0.0,0.0,741 +0.0,-0.4765625,0.0,0.0,762 +0.0,-0.4765625,0.0,0.0,782 +0.0,-0.484375,0.0,0.0,803 +0.0,-0.484375,0.0,0.0,821 +0.0,-0.4921875,0.0,0.0,842 +0.0,-0.5078125,0.0,0.0,878 +0.0,-0.5234375,0.0,0.0,893 +0.0,-0.5234375,0.0,0.0,906 +0.0,-0.53125,0.0,0.0,922 +0.0,-0.53125,0.0,0.0,942 +0.0,-0.53125,0.0,0.0,962 +0.0,-0.53125,0.0,0.0,982 +0.0,-0.5390625,0.0,0.0,1001 +0.0,-0.5390625,0.0,0.0,1022 +0.0,-0.546875,0.0,0.0,1042 +0.0,-0.546875,0.0,0.0,1061 +0.0,-0.546875,0.0,0.0,1082 +0.0,-0.546875,0.0,0.0,1164 +0.0,-0.546875,0.0,0.0,1176 +0.0,-0.546875,0.0,0.0,1190 +0.0,-0.546875,0.0,0.0,1202 +0.0,-0.546875,0.0,0.0,1219 +0.0,-0.546875,0.0,0.0,1230 +0.0,-0.546875,0.0,0.0,1244 +0.0,-0.546875,0.0,0.0,1258 +0.0,-0.546875,0.0,0.0,1268 +0.0,-0.546875,0.0,0.0,1281 +0.0,-0.546875,0.0,0.0,1301 +0.0,-0.546875,0.0,0.0,1321 +0.0,-0.546875,0.0,0.0,1363 +0.0,-0.546875,0.0,0.0,1376 +0.0,-0.546875,0.0,0.0,1388 +0.0,-0.546875,0.0,0.0,1402 +0.0,-0.546875,0.0,0.0,1422 +0.0,-0.546875,0.0,0.0,1441 +0.0,-0.546875,0.0,0.0,1461 +0.0,-0.546875,0.0,0.0,1482 +0.0,-0.546875,0.0,0.0,1502 +0.0,-0.546875,0.0,0.0,1521 +0.0,-0.546875,0.0,0.0,1542 +0.0,-0.546875,0.0,0.0,1562 +0.0,-0.546875,0.0,0.0,1598 +0.0,-0.546875,0.0,0.0,1609 +0.0,-0.546875,0.0,0.0,1621 +0.0,-0.546875,0.0,0.0,1642 +0.0,-0.546875,0.0,0.0,1661 +0.0,-0.546875,0.0,0.0,1682 +0.0,-0.546875,0.0,0.0,1702 +0.0,-0.546875,0.0,0.0,1722 +0.0,-0.546875,0.0,0.0,1742 +0.0,-0.546875,0.0,0.0,1762 +0.0,-0.546875,0.0,0.0,1781 +0.0,-0.5390625,0.0,0.0,1801 +0.0,-0.5390625,0.0,0.0,1835 +0.0,-0.5390625,0.0,0.0,1847 +0.0,-0.5390625,0.0,0.0,1861 +0.0,-0.5390625,0.0,0.0,1882 +0.0,-0.5390625,0.0,0.0,1901 +0.0,-0.5390625,0.0,0.0,1921 +0.0,-0.5390625,0.0,0.0,1941 +0.0,-0.5390625,0.0,0.0,1962 +0.0,-0.5390625,0.0,0.0,1983 +0.0,-0.5390625,0.0,0.0,2001 +0.0,-0.5390625,0.0,0.0,2022 +0.0,-0.5390625,0.0,0.0,2042 +0.0,-0.5390625,0.0,0.0,2085 +0.0,-0.5390625,0.0,0.0,2097 +0.0,-0.5390625,0.0,0.0,2113 +0.0,-0.5390625,0.0,0.0,2124 +0.0,-0.5390625,0.0,0.0,2141 +0.0,-0.5390625,0.0,0.0,2161 +0.0,-0.5390625,0.0,0.0,2181 +0.0,-0.5390625,0.0,0.0,2201 +0.0,-0.5390625,0.0,0.0,2221 +0.0,-0.5390625,0.0,0.0,2241 +0.0,-0.5390625,0.0,0.0,2262 +0.0,-0.5390625,0.0,0.0,2283 +0.0,-0.2265625,0.0,0.0,2377 +0.0,-0.2265625,0.0,0.0,2390 +0.0,0.0,0.0,0.0,2402 +0.0,0.0,0.0,0.0,2417 +0.0,0.0,0.0,0.0,2428 +0.0,0.0,0.0,0.0,2440 +0.0,0.0,0.0,0.0,2451 +0.0,0.0,0.0,0.0,2463 +0.0,0.0,0.0,0.0,2475 +0.0,0.0,0.0,0.0,2493 +0.0,0.0,0.0,0.0,2505 +0.0,0.0,0.0,0.0,2521 +0.0,0.0,0.0,0.0,2558 +0.0,0.0,0.0,0.0,2576 +0.0,0.0,0.0,0.0,2592 +0.0,0.0,0.0,0.0,2603 +0.0,0.0,0.0,0.0,2621 +0.0,0.0,0.0,0.0,2641 +0.0,0.0,0.0,0.0,2661 +0.0,0.0,0.0,0.0,2681 +0.0,0.0,0.0,0.0,2702 +0.0,0.0,0.0,0.0,2721 +0.0,0.0,0.0,0.0,2742 +0.0,0.0,0.0,0.0,2762 +0.0,0.0,0.0,0.0,2805 +0.0,0.0,0.0,0.0,2814 +0.0,0.0,0.0,0.0,2825 +0.0,0.0,0.0,0.0,2841 +0.0,0.0,0.0,0.0,2861 +0.0,0.0,0.0,0.0,2882 +0.0,0.0,0.0,0.0,2901 +0.0,0.0,0.0,0.0,2922 \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/BlueNearDriveToChargeStation.txt b/src/main/java/frc4388/robot/commands/BlueNearDriveToChargeStation.txt deleted file mode 100644 index 664216a..0000000 --- a/src/main/java/frc4388/robot/commands/BlueNearDriveToChargeStation.txt +++ /dev/null @@ -1,758 +0,0 @@ -0.0,0.0,0.0,0.0,0 -0.0,0.0,0.0,0.0,0 -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,79 -0.0,0.0,0.0,0.0,99 -0.0,0.0,0.0,0.0,119 -0.0,0.0,0.0,0.0,139 -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.0,0.0,0.0,220 -0.0,0.0,0.0,0.0,239 -0.0,0.0,0.0,0.0,260 -0.0,0.0,0.0,0.0,280 -0.0,0.0,0.0,0.0,300 -0.0,0.0,0.0,0.0,319 -0.0,0.0,0.0,0.0,340 -0.0,0.0,0.0,0.0,360 -0.0,0.0,0.0,0.0,379 -0.0,0.0,0.0,0.0,399 -0.0,0.0,0.0,0.0,419 -0.0,0.0,0.0,0.0,440 -0.0,0.0,0.0,0.0,459 -0.0,-0.109375,0.0,0.0,479 -0.0,-0.109375,0.0,0.0,499 -0.0,-0.109375,0.0,0.0,520 -0.0,-0.109375,0.0,0.0,540 -0.0,-0.1171875,0.0,0.0,559 -0.0,-0.1171875,0.0,0.0,580 -0.0,-0.1171875,0.0,0.0,599 -0.0,-0.1171875,0.0,0.0,621 -0.0,-0.1171875,0.0,0.0,639 -0.0,-0.1171875,0.0,0.0,661 -0.0,-0.1171875,0.0,0.0,680 -0.0,-0.1171875,0.0,0.0,699 -0.0,-0.1171875,0.0,0.0,721 -0.0,-0.1171875,0.0,0.0,769 -0.0,-0.125,0.0,0.0,778 -0.0,-0.1328125,0.0,0.0,790 -0.0,-0.1328125,0.0,0.0,802 -0.0,-0.1328125,0.0,0.0,820 -0.0,-0.1328125,0.0,0.0,840 -0.0,-0.1328125,0.0,0.0,860 -0.0,-0.1328125,0.0,0.0,879 -0.0,-0.140625,0.0,0.0,899 -0.0,-0.171875,0.0,0.0,933 -0.0,-0.1875,0.0,0.0,944 -0.0,-0.1875,0.0,0.0,959 -0.0,-0.203125,0.0,0.0,979 -0.0,-0.203125,0.0,0.0,999 -0.0,-0.203125,0.0,0.0,1019 -0.0,-0.203125,0.0,0.0,1039 -0.0,-0.203125,0.0,0.0,1059 -0.0,-0.203125,0.0,0.0,1079 -0.0,-0.2109375,0.0,0.0,1099 -0.0,-0.2109375,0.0,0.0,1120 -0.0,-0.2109375,0.0,0.0,1140 -0.0,-0.21875,0.0,0.0,1159 -0.0,-0.2109375,0.0,0.0,1180 -0.0,-0.2265625,0.0,0.0,1199 -0.0,-0.234375,0.0,0.0,1219 -0.0,-0.234375,0.0,0.0,1239 -0.0,-0.234375,0.0,0.0,1259 -0.0,-0.2421875,0.0,0.0,1280 -0.0,-0.2578125,0.0,0.0,1300 -0.0,-0.296875,0.0,0.0,1320 -0.0,-0.296875,0.0,0.0,1340 -0.0,-0.3046875,0.0,0.0,1359 -0.0,-0.3125,0.0,0.0,1379 -0.0,-0.3125,0.0,0.0,1399 -0.0,-0.3125,0.0,0.0,1419 -0.0,-0.3203125,0.0,0.0,1442 -0.0,-0.3203125,0.0,0.0,1460 -0.0,-0.3203125,0.0,0.0,1479 -0.0,-0.3203125,0.0,0.0,1499 -0.0,-0.3203125,0.0,0.0,1520 -0.0,-0.3203125,0.0,0.0,1540 -0.0,-0.3203125,0.0,0.0,1560 -0.0,-0.3203125,0.0,0.0,1579 -0.0,-0.3203125,0.0,0.0,1599 -0.0,-0.328125,0.0,0.0,1619 -0.0,-0.34375,0.0,0.0,1640 -0.0,-0.3515625,0.0,0.0,1659 -0.0,-0.359375,0.0,0.0,1680 -0.0,-0.359375,0.0,0.0,1699 -0.0,-0.359375,0.0,0.0,1720 -0.0,-0.359375,0.0,0.0,1739 -0.0,-0.3671875,0.0,0.0,1759 -0.0,-0.375,0.0,0.0,1780 -0.0,-0.390625,0.0,0.0,1799 -0.0,-0.4140625,0.0,0.0,1820 -0.0,-0.421875,0.0,0.0,1839 -0.0,-0.4296875,0.0,0.0,1859 -0.0,-0.4296875,0.0,0.0,1879 -0.0,-0.4296875,0.0,0.0,1900 -0.0,-0.4296875,0.0,0.0,1919 -0.0,-0.4296875,0.0,0.0,1939 -0.0,-0.4296875,0.0,0.0,1959 -0.0,-0.4296875,0.0,0.0,1979 -0.0,-0.4296875,0.0,0.0,2000 -0.0,-0.4296875,0.0,0.0,2019 -0.0,-0.4296875,0.0,0.0,2039 -0.0,-0.4296875,0.0,0.0,2060 -0.0,-0.4296875,0.0,0.0,2080 -0.0,-0.4296875,0.0,0.0,2099 -0.0,-0.4296875,0.0,0.0,2119 -0.0,-0.4296875,0.0,0.0,2139 -0.0,-0.4296875,0.0,0.0,2160 -0.0,-0.4375,0.0,0.0,2179 -0.0,-0.4453125,0.0,0.0,2199 -0.0,-0.453125,0.0,0.0,2220 -0.0,-0.453125,0.0,0.0,2240 -0.0,-0.453125,0.0,0.0,2260 -0.0,-0.453125,0.0,0.0,2280 -0.0,-0.453125,0.0,0.0,2300 -0.0,-0.453125,0.0,0.0,2320 -0.0,-0.453125,0.0,0.0,2340 -0.0,-0.453125,0.0,0.0,2359 -0.0,-0.453125,0.0,0.0,2379 -0.0,-0.453125,0.0,0.0,2399 -0.0,-0.453125,0.0,0.0,2419 -0.0,-0.453125,0.0,0.0,2440 -0.0,-0.453125,0.0,0.0,2460 -0.0,-0.453125,0.0,0.0,2479 -0.0,-0.453125,0.0,0.0,2500 -0.0,-0.453125,0.0,0.0,2520 -0.0,-0.453125,0.0,0.0,2540 -0.0,-0.453125,0.0,0.0,2559 -0.0,-0.453125,0.0,0.0,2579 -0.0,-0.4609375,0.0,0.0,2600 -0.0,-0.4609375,0.0,0.0,2620 -0.0,-0.4609375,0.0,0.0,2641 -0.0,-0.4609375,0.0,0.0,2660 -0.0,-0.4609375,0.0,0.0,2679 -0.0,-0.4609375,0.0,0.0,2700 -0.0,-0.4609375,0.0,0.0,2720 -0.0,-0.46875,0.0,0.0,2740 -0.0,-0.46875,0.0,0.0,2759 -0.0,-0.46875,0.0,0.0,2779 -0.0,-0.46875,0.0,0.0,2799 -0.0,-0.46875,0.0,0.0,2819 -0.0,-0.46875,0.0,0.0,2839 -0.0,-0.46875,0.0,0.0,2859 -0.0,-0.4765625,0.0,0.0,2880 -0.0,-0.5078125,0.0,0.0,2915 -0.0,-0.515625,0.0,0.0,2933 -0.0,-0.515625,0.0,0.0,2945 -0.0,-0.5234375,0.0,0.0,2960 -0.0,-0.5234375,0.0,0.0,2979 -0.0,-0.5234375,0.0,0.0,2999 -0.0,-0.5234375,0.0,0.0,3020 -0.0,-0.5234375,0.0,0.0,3040 -0.0,-0.5234375,0.0,0.0,3059 -0.0,-0.5234375,0.0,0.0,3079 -0.0,-0.5234375,0.0,0.0,3099 -0.0,-0.5234375,0.0,0.0,3119 -0.0,-0.5234375,0.0,0.0,3140 -0.0,-0.5234375,0.0,0.0,3159 -0.0,-0.5234375,0.0,0.0,3180 -0.0,-0.5234375,0.0,0.0,3199 -0.0,-0.5234375,0.0,0.0,3219 -0.0,-0.5234375,0.0,0.0,3239 -0.0,-0.5234375,0.0,0.0,3260 -0.0,-0.5234375,0.0,0.0,3279 -0.0,-0.5234375,0.0,0.0,3299 -0.0,-0.5234375,0.0,0.0,3320 -0.0,-0.5234375,0.0,0.0,3339 -0.0,-0.5234375,0.0,0.0,3359 -0.0,-0.5234375,0.0,0.0,3380 -0.0,-0.5234375,0.0,0.0,3400 -0.0,-0.5234375,0.0,0.0,3420 -0.0,-0.5234375,0.0,0.0,3439 -0.0,-0.5234375,0.0,0.0,3459 -0.0,-0.5234375,0.0,0.0,3479 -0.0,-0.5234375,0.0,0.0,3499 -0.0,-0.5234375,0.0,0.0,3520 -0.0,-0.5234375,0.0,0.0,3539 -0.0,-0.5234375,0.0,0.0,3559 -0.0,-0.5234375,0.0,0.0,3579 -0.0,-0.5234375,0.0,0.0,3600 -0.0,-0.5234375,0.0,0.0,3619 -0.0,-0.5234375,0.0,0.0,3640 -0.0,-0.5234375,0.0,0.0,3659 -0.0,-0.5234375,0.0,0.0,3679 -0.0,-0.5234375,0.0,0.0,3699 -0.0,-0.5234375,0.0,0.0,3720 -0.0,-0.5234375,0.0,0.0,3740 -0.0,-0.5234375,0.0,0.0,3759 -0.0,-0.5234375,0.0,0.0,3779 -0.0,-0.5234375,0.0,0.0,3800 -0.0,-0.5234375,0.0,0.0,3820 -0.0,-0.5234375,0.0,0.0,3840 -0.0,-0.5234375,0.0,0.0,3859 -0.0,-0.5234375,0.0,0.0,3879 -0.0,-0.5234375,0.0,0.0,3900 -0.0,-0.5390625,0.0,0.0,3920 -0.0,-0.546875,0.0,0.0,3939 -0.0,-0.546875,0.0,0.0,3959 -0.0,-0.546875,0.0,0.0,3979 -0.0,-0.546875,0.0,0.0,3999 -0.0,-0.546875,0.0,0.0,4019 -0.0,-0.5546875,0.0,0.0,4039 -0.0,-0.5546875,0.0,0.0,4059 -0.0,-0.5546875,0.0,0.0,4079 -0.0,-0.5546875,0.0,0.0,4134 -0.0,-0.5546875,0.0,0.0,4147 -0.0,-0.5546875,0.0,0.0,4157 -0.0,-0.5546875,0.0,0.0,4170 -0.0,-0.5546875,0.0,0.0,4185 -0.0,-0.5546875,0.0,0.0,4199 -0.0,-0.5546875,0.0,0.0,4219 -0.0,-0.5546875,0.0,0.0,4240 --0.0234375,-0.5625,0.0,0.0,4259 --0.03125,-0.5625,0.0,0.0,4279 --0.03125,-0.5625,0.0,0.0,4300 --0.03125,-0.5625,0.0,0.0,4319 --0.03125,-0.5625,0.0,0.0,4339 --0.0234375,-0.5625,0.0,0.0,4360 --0.03125,-0.5625,0.0,0.0,4379 --0.03125,-0.5625,0.0,0.0,4399 --0.03125,-0.5625,0.0,0.0,4419 --0.03125,-0.5625,0.0,0.0,4440 --0.03125,-0.5625,0.0,0.0,4461 --0.0390625,-0.5625,0.0,0.0,4479 --0.046875,-0.5625,0.0,0.0,4499 --0.0546875,-0.5625,0.0,0.0,4520 --0.0625,-0.5625,0.0,0.0,4540 --0.0625,-0.5625,0.0,0.0,4561 --0.0546875,-0.5625,0.0,0.0,4581 --0.0546875,-0.5625,0.0,0.0,4600 --0.0546875,-0.5625,0.0,0.0,4620 --0.0546875,-0.546875,0.0,0.0,4639 --0.0546875,-0.546875,0.0,0.0,4659 --0.0546875,-0.484375,0.0,0.0,4680 --0.0546875,-0.4375,0.0,0.0,4700 --0.0546875,-0.390625,0.0,0.0,4720 --0.0546875,-0.3671875,0.0,0.0,4739 --0.0546875,-0.3359375,0.0,0.0,4759 --0.0546875,-0.3203125,0.0,0.0,4779 --0.0546875,-0.2734375,0.0,0.0,4800 -0.0,0.0,0.0,0.0,4820 -0.0,0.0,0.0,0.0,4840 -0.0,0.0,0.0,0.0,4859 -0.0,0.0,0.0,0.0,4879 -0.0,0.0,0.0,0.0,4899 -0.0,0.0,0.0,0.0,4919 -0.0,0.0,0.0,0.0,4939 -0.0,0.0,0.0,0.0,4960 -0.0,0.0,0.0,0.0,4980 -0.0,0.0,0.0,0.0,4999 -0.0,0.0,0.0,0.0,5019 -0.0,0.0,0.0,0.0,5040 -0.0,0.0,0.0,0.0,5061 -0.0,0.0,0.0,0.0,5079 -0.0,0.0,0.0,0.0,5100 -0.0,0.0,0.0,0.0,5119 -0.0,0.0,0.0,0.0,5140 -0.0,0.0,0.0,0.0,5159 -0.0,0.0,0.0,0.0,5180 -0.0,0.0,0.0,0.0,5199 -0.0,0.0,0.0,0.0,5219 -0.0,0.0,0.0,0.0,5239 -0.0,0.0,0.0,0.0,5259 -0.0,0.0,0.0,0.0,5280 -0.0,0.0,0.0,0.0,5299 -0.0,0.0,0.0,0.0,5319 -0.0,0.0,0.0,0.0,5339 -0.0,0.0,0.0,0.0,5360 -0.0,0.0,0.0,0.0,5379 -0.0,0.0,0.0,0.0,5399 -0.0,0.0,0.0,0.0,5419 -0.0,0.0,0.0,0.0,5439 -0.0,0.0,0.0,0.0,5459 -0.0,0.0,0.0,0.0,5480 -0.0,0.0,0.0,0.0,5500 -0.0,0.0,0.0,0.0,5519 -0.0,0.0,0.0,0.0,5540 -0.0,0.0,0.0,0.0,5559 -0.0,0.0,0.0,0.0,5579 -0.0,0.0,0.0,0.0,5599 -0.0,0.0,0.0,0.0,5619 -0.0,0.0,0.0,0.0,5639 -0.0,0.0,0.0,0.0,5659 -0.0,0.0,0.0,0.0,5680 -0.0,0.0,0.0,0.0,5699 -0.0,0.0,0.0,0.0,5721 -0.0,0.0,0.0,0.0,5739 -0.0,0.0,0.0,0.0,5760 -0.0,0.0,0.0,0.0,5779 -0.0,0.0,0.0,0.0,5800 -0.0,0.0,0.0,0.0,5819 -0.0,0.0,0.0,0.0,5839 -0.0,0.0,0.0,0.0,5859 -0.0,0.0,0.0,0.0,5879 -0.0,0.0,0.0,0.0,5900 -0.0,0.0,0.0,0.0,5920 -0.0,0.0,0.0,0.0,5939 -0.0,0.0,0.0,0.0,5959 -0.0,0.0,0.0,0.0,5979 --0.03125,0.11811023950576782,0.0,0.0,5999 --0.03125,0.14173229038715363,0.0,0.0,6020 --0.03125,0.14173229038715363,0.0,0.0,6039 --0.03125,0.14173229038715363,0.0,0.0,6059 --0.03125,0.14173229038715363,0.0,0.0,6079 --0.03125,0.14173229038715363,0.0,0.0,6100 --0.03125,0.14173229038715363,0.0,0.0,6119 --0.03125,0.14173229038715363,0.0,0.0,6139 --0.03125,0.14173229038715363,0.0,0.0,6159 --0.03125,0.14173229038715363,0.0,0.0,6179 --0.03125,0.14960630238056183,0.0,0.0,6199 --0.03125,0.14960630238056183,0.0,0.0,6219 --0.03125,0.14960630238056183,0.0,0.0,6239 --0.03125,0.14960630238056183,0.0,0.0,6259 --0.03125,0.14960630238056183,0.0,0.0,6280 --0.03125,0.14960630238056183,0.0,0.0,6299 --0.03125,0.16535432636737823,0.0,0.0,6319 --0.03125,0.19685038924217224,0.0,0.0,6340 --0.03125,0.20472441613674164,0.0,0.0,6360 --0.03125,0.21259842813014984,0.0,0.0,6381 --0.03125,0.21259842813014984,0.0,0.0,6399 --0.03125,0.21259842813014984,0.0,0.0,6419 --0.03125,0.21259842813014984,0.0,0.0,6440 --0.03125,0.21259842813014984,0.0,0.0,6459 --0.03125,0.21259842813014984,0.0,0.0,6479 --0.03125,0.21259842813014984,0.0,0.0,6499 --0.03125,0.21259842813014984,0.0,0.0,6519 --0.03125,0.21259842813014984,0.0,0.0,6540 --0.03125,0.21259842813014984,0.0,0.0,6559 --0.03125,0.21259842813014984,0.0,0.0,6579 --0.03125,0.21259842813014984,0.0,0.0,6599 --0.03125,0.21259842813014984,0.0,0.0,6619 --0.03125,0.21259842813014984,0.0,0.0,6640 --0.03125,0.21259842813014984,0.0,0.0,6660 --0.03125,0.21259842813014984,0.0,0.0,6679 --0.03125,0.33070865273475647,0.0,0.0,6700 --0.03125,0.33070865273475647,0.0,0.0,6720 --0.03125,0.3700787425041199,0.0,0.0,6740 --0.03125,0.3779527544975281,0.0,0.0,6760 --0.03125,0.3779527544975281,0.0,0.0,6780 --0.03125,0.3779527544975281,0.0,0.0,6800 --0.03125,0.3779527544975281,0.0,0.0,6819 --0.03125,0.3779527544975281,0.0,0.0,6839 --0.03125,0.3700787425041199,0.0,0.0,6859 --0.03125,0.3700787425041199,0.0,0.0,6879 --0.03125,0.3700787425041199,0.0,0.0,6899 --0.03125,0.3700787425041199,0.0,0.0,6920 --0.03125,0.3700787425041199,0.0,0.0,6939 --0.03125,0.3700787425041199,0.0,0.0,6959 --0.03125,0.3700787425041199,0.0,0.0,6979 --0.03125,0.3700787425041199,0.0,0.0,6999 --0.03125,0.3700787425041199,0.0,0.0,7019 --0.03125,0.3700787425041199,0.0,0.0,7039 --0.03125,0.3700787425041199,0.0,0.0,7060 --0.03125,0.3700787425041199,0.0,0.0,7080 --0.03125,0.3700787425041199,0.0,0.0,7100 --0.03125,0.3700787425041199,0.0,0.0,7119 --0.03125,0.3700787425041199,0.0,0.0,7139 --0.03125,0.3700787425041199,0.0,0.0,7159 --0.03125,0.3700787425041199,0.0,0.0,7180 --0.03125,0.3700787425041199,0.0,0.0,7199 --0.03125,0.3700787425041199,0.0,0.0,7220 --0.03125,0.3700787425041199,0.0,0.0,7239 --0.03125,0.3700787425041199,0.0,0.0,7260 --0.03125,0.36220473051071167,0.0,0.0,7279 --0.03125,0.34645670652389526,0.0,0.0,7299 -0.0,0.33070865273475647,0.0,0.0,7319 -0.0,0.33070865273475647,0.0,0.0,7339 -0.0,0.3385826647281647,0.0,0.0,7359 -0.0,0.3385826647281647,0.0,0.0,7379 -0.0,0.3385826647281647,0.0,0.0,7399 -0.0,0.3385826647281647,0.0,0.0,7419 -0.0,0.3385826647281647,0.0,0.0,7440 -0.0,0.33070865273475647,0.0,0.0,7460 -0.0,0.31496062874794006,0.0,0.0,7479 -0.0,0.31496062874794006,0.0,0.0,7499 -0.0,0.29133859276771545,0.0,0.0,7519 -0.0,0.24409449100494385,0.0,0.0,7540 -0.0,0.24409449100494385,0.0,0.0,7559 -0.0,0.24409449100494385,0.0,0.0,7580 -0.0,0.24409449100494385,0.0,0.0,7600 -0.0,0.24409449100494385,0.0,0.0,7619 -0.0,0.24409449100494385,0.0,0.0,7639 -0.0,0.24409449100494385,0.0,0.0,7660 -0.0,0.24409449100494385,0.0,0.0,7679 -0.0,0.24409449100494385,0.0,0.0,7700 -0.0,0.25196850299835205,0.0,0.0,7720 -0.0,0.33070865273475647,0.0,0.0,7740 -0.0,0.3700787425041199,0.0,0.0,7760 -0.0,0.4251968562602997,0.0,0.0,7780 -0.0,0.4566929042339325,0.0,0.0,7800 -0.0,0.4645669162273407,0.0,0.0,7820 -0.0,0.4645669162273407,0.0,0.0,7839 -0.0,0.4645669162273407,0.0,0.0,7859 -0.0,0.4645669162273407,0.0,0.0,7880 -0.0,0.4645669162273407,0.0,0.0,7899 -0.0,0.4645669162273407,0.0,0.0,7920 -0.0,0.4645669162273407,0.0,0.0,7939 -0.0,0.4803149700164795,0.0,0.0,7960 -0.0,0.5039370059967041,0.0,0.0,7980 --0.0078125,0.5433070659637451,0.0,0.0,7999 --0.0078125,0.5748031735420227,0.0,0.0,8019 --0.015625,0.5748031735420227,0.0,0.0,8039 --0.015625,0.5748031735420227,0.0,0.0,8059 --0.015625,0.5748031735420227,0.0,0.0,8080 --0.015625,0.5511810779571533,0.0,0.0,8099 --0.015625,0.5354330539703369,0.0,0.0,8119 --0.015625,0.5354330539703369,0.0,0.0,8139 --0.015625,0.5354330539703369,0.0,0.0,8159 --0.0078125,0.5354330539703369,0.0,0.0,8180 --0.0078125,0.5354330539703369,0.0,0.0,8199 --0.0078125,0.5354330539703369,0.0,0.0,8220 --0.0078125,0.5354330539703369,0.0,0.0,8239 --0.0078125,0.5354330539703369,0.0,0.0,8259 --0.0078125,0.5354330539703369,0.0,0.0,8279 --0.0078125,0.5354330539703369,0.0,0.0,8299 --0.0078125,0.5354330539703369,0.0,0.0,8319 --0.0078125,0.5354330539703369,0.0,0.0,8340 --0.0078125,0.5275590419769287,0.0,0.0,8359 --0.0078125,0.4803149700164795,0.0,0.0,8379 --0.0078125,0.4803149700164795,0.0,0.0,8399 --0.0078125,0.4803149700164795,0.0,0.0,8421 --0.0078125,0.4803149700164795,0.0,0.0,8440 --0.0078125,0.4803149700164795,0.0,0.0,8460 --0.0078125,0.4803149700164795,0.0,0.0,8479 -0.0,0.4803149700164795,0.0,0.0,8499 -0.0,0.4803149700164795,0.0,0.0,8520 -0.0,0.4803149700164795,0.0,0.0,8540 -0.0,0.4803149700164795,0.0,0.0,8559 -0.0,0.4803149700164795,0.0,0.0,8579 -0.0,0.4803149700164795,0.0,0.0,8600 -0.0,0.4803149700164795,0.0,0.0,8619 -0.0,0.4803149700164795,0.0,0.0,8639 -0.0,0.4803149700164795,0.0,0.0,8659 -0.0,0.4724409580230713,0.0,0.0,8679 -0.0,0.4409448802471161,0.0,0.0,8699 -0.0,0.4173228442668915,0.0,0.0,8719 -0.0,0.4094488322734833,0.0,0.0,8739 -0.0,0.4094488322734833,0.0,0.0,8759 -0.0,0.4094488322734833,0.0,0.0,8779 -0.015748031437397003,0.4094488322734833,0.0,0.0,8799 -0.04724409431219101,0.4094488322734833,0.0,0.0,8820 -0.07874015718698502,0.4094488322734833,0.0,0.0,8840 -0.07874015718698502,0.4094488322734833,0.0,0.0,8859 -0.07874015718698502,0.4094488322734833,0.0,0.0,8879 -0.07874015718698502,0.4251968562602997,0.0,0.0,8899 -0.07874015718698502,0.4488188922405243,0.0,0.0,8920 -0.07874015718698502,0.4488188922405243,0.0,0.0,8939 -0.07874015718698502,0.4488188922405243,0.0,0.0,8959 -0.07874015718698502,0.4488188922405243,0.0,0.0,8980 -0.05511811003088951,0.4488188922405243,0.0,0.0,8999 -0.031496062874794006,0.4488188922405243,0.0,0.0,9019 -0.031496062874794006,0.4488188922405243,0.0,0.0,9039 -0.031496062874794006,0.4488188922405243,0.0,0.0,9059 -0.031496062874794006,0.4488188922405243,0.0,0.0,9079 -0.031496062874794006,0.4488188922405243,0.0,0.0,9099 -0.031496062874794006,0.4488188922405243,0.0,0.0,9120 -0.031496062874794006,0.4488188922405243,0.0,0.0,9139 -0.031496062874794006,0.4488188922405243,0.0,0.0,9159 -0.031496062874794006,0.4488188922405243,0.0,0.0,9179 -0.031496062874794006,0.4488188922405243,0.0,0.0,9199 -0.031496062874794006,0.4488188922405243,0.0,0.0,9219 -0.031496062874794006,0.4488188922405243,0.0,0.0,9239 -0.04724409431219101,0.4488188922405243,0.0,0.0,9260 -0.06299212574958801,0.4488188922405243,0.0,0.0,9279 -0.06299212574958801,0.4488188922405243,0.0,0.0,9299 -0.06299212574958801,0.4488188922405243,0.0,0.0,9319 -0.06299212574958801,0.4488188922405243,0.0,0.0,9339 -0.07874015718698502,0.4488188922405243,0.0,0.0,9359 -0.07874015718698502,0.4488188922405243,0.0,0.0,9380 -0.07874015718698502,0.4488188922405243,0.0,0.0,9400 -0.07874015718698502,0.4488188922405243,0.0,0.0,9419 -0.07874015718698502,0.4488188922405243,0.0,0.0,9439 -0.07874015718698502,0.4488188922405243,0.0,0.0,9460 -0.07874015718698502,0.4488188922405243,0.0,0.0,9480 -0.07874015718698502,0.4488188922405243,0.0,0.0,9499 -0.07874015718698502,0.4488188922405243,0.0,0.0,9519 -0.07874015718698502,0.4488188922405243,0.0,0.0,9539 -0.07874015718698502,0.4488188922405243,0.0,0.0,9559 -0.07874015718698502,0.4488188922405243,0.0,0.0,9579 -0.07874015718698502,0.4488188922405243,0.0,0.0,9600 -0.07874015718698502,0.4488188922405243,0.0,0.0,9619 -0.07874015718698502,0.4488188922405243,0.0,0.0,9639 -0.07874015718698502,0.4488188922405243,0.0,0.0,9659 -0.07874015718698502,0.4488188922405243,0.0,0.0,9679 -0.07874015718698502,0.4488188922405243,0.0,0.0,9699 -0.07874015718698502,0.4488188922405243,0.0,0.0,9722 -0.07874015718698502,0.4488188922405243,0.0,0.0,9739 -0.07874015718698502,0.4488188922405243,0.0,0.0,9759 -0.08661417663097382,0.4488188922405243,0.0,0.0,9780 -0.08661417663097382,0.4488188922405243,0.0,0.0,9800 -0.11811023950576782,0.4488188922405243,0.0,0.0,9820 -0.14173229038715363,0.4488188922405243,0.0,0.0,9839 -0.16535432636737823,0.4488188922405243,0.0,0.0,9859 -0.22834645211696625,0.4488188922405243,0.0,0.0,9879 -0.23622047901153564,0.4488188922405243,0.0,0.0,9899 -0.23622047901153564,0.4409448802471161,0.0,0.0,9919 -0.25984251499176025,0.4094488322734833,0.0,0.0,9940 -0.29921260476112366,0.3858267664909363,0.0,0.0,9959 -0.31496062874794006,0.3779527544975281,0.0,0.0,9979 -0.32283464074134827,0.3700787425041199,0.0,0.0,9999 -0.32283464074134827,0.3700787425041199,0.0,0.0,10019 -0.36220473051071167,0.33070865273475647,0.0,0.0,10039 -0.3779527544975281,0.25984251499176025,0.0,0.0,10059 -0.3937007784843445,0.21259842813014984,0.0,0.0,10081 -0.3937007784843445,0.20472441613674164,0.0,0.0,10100 -0.3937007784843445,0.20472441613674164,0.0,0.0,10124 -0.3937007784843445,0.20472441613674164,0.0,0.0,10140 -0.3937007784843445,0.18897637724876404,0.0,0.0,10159 -0.3937007784843445,0.18897637724876404,0.0,0.0,10180 -0.3937007784843445,0.16535432636737823,0.0,0.0,10199 -0.3937007784843445,0.16535432636737823,0.0,0.0,10221 -0.3937007784843445,0.17322835326194763,0.0,0.0,10239 -0.3937007784843445,0.16535432636737823,0.0,0.0,10260 -0.3937007784843445,0.14960630238056183,0.0,0.0,10279 -0.3937007784843445,0.14960630238056183,0.0,0.0,10300 -0.3937007784843445,0.14960630238056183,0.0,0.0,10319 -0.3937007784843445,0.14960630238056183,0.0,0.0,10340 -0.3937007784843445,0.14173229038715363,0.0,0.0,10360 -0.3937007784843445,0.14173229038715363,0.0,0.0,10380 -0.3937007784843445,0.14173229038715363,0.0,0.0,10399 -0.3937007784843445,0.14173229038715363,0.0,0.0,10419 -0.3937007784843445,0.14173229038715363,0.0,0.0,10440 -0.3937007784843445,0.14173229038715363,0.0,0.0,10459 -0.3937007784843445,0.14173229038715363,0.0,0.0,10479 -0.3937007784843445,0.14173229038715363,0.0,0.0,10500 -0.3937007784843445,0.14173229038715363,0.0,0.0,10519 -0.4015747904777527,0.14173229038715363,0.0,0.0,10540 -0.4173228442668915,0.14173229038715363,0.0,0.0,10559 -0.4251968562602997,0.13385826349258423,0.0,0.0,10580 -0.4251968562602997,0.12598425149917603,0.0,0.0,10600 -0.4251968562602997,0.10236220806837082,0.0,0.0,10620 -0.4251968562602997,0.10236220806837082,0.0,0.0,10639 -0.4330708682537079,0.10236220806837082,0.0,0.0,10659 -0.4409448802471161,0.10236220806837082,0.0,0.0,10680 -0.4488188922405243,0.07086614519357681,0.0,0.0,10699 -0.4566929042339325,0.023622047156095505,0.0,0.0,10719 -0.4566929042339325,0.0,0.0,0.0,10739 -0.4645669162273407,0.0,0.0,0.0,10760 -0.4645669162273407,0.0,0.0,0.0,10779 -0.4724409580230713,0.0,0.0,0.0,10800 -0.4724409580230713,0.0,0.0,0.0,10819 -0.4724409580230713,0.0,0.0,0.0,10839 -0.4803149700164795,0.0,0.0,0.0,10860 -0.4881889820098877,0.0,0.0,0.0,10879 -0.5039370059967041,0.0,0.0,0.0,10899 -0.5275590419769287,0.0,0.0,0.0,10919 -0.5354330539703369,0.0,0.0,0.0,10940 -0.5354330539703369,0.0,0.0,0.0,10960 -0.5354330539703369,0.0,0.0,0.0,10979 -0.5354330539703369,0.0,0.0,0.0,10999 -0.5354330539703369,0.0,0.0,0.0,11020 -0.5354330539703369,0.0,0.0,0.0,11039 -0.5354330539703369,0.0,0.0,0.0,11059 -0.5354330539703369,0.0,0.0,0.0,11079 -0.5354330539703369,0.0,0.0,0.0,11099 -0.5354330539703369,0.0,0.0,0.0,11119 -0.5354330539703369,0.0,0.0,0.0,11140 -0.5354330539703369,0.0,0.0,0.0,11159 -0.5354330539703369,0.0,0.0,0.0,11179 -0.5354330539703369,0.0,0.0,0.0,11200 -0.5354330539703369,0.0,0.0,0.0,11219 -0.5354330539703369,0.0,0.0,0.0,11239 -0.5354330539703369,0.0,0.0,0.0,11260 -0.5354330539703369,0.0,0.0,0.0,11279 -0.5354330539703369,0.0,0.0,0.0,11299 -0.5275590419769287,0.0,0.0,0.0,11319 -0.5275590419769287,0.0,0.0,0.0,11339 -0.5275590419769287,0.0,0.0,0.0,11360 -0.5275590419769287,0.0,0.0,0.0,11379 -0.5275590419769287,0.0,0.0,0.0,11399 -0.5275590419769287,0.0,0.0,0.0,11419 -0.5275590419769287,0.0,0.0,0.0,11440 -0.5275590419769287,0.0,0.0,0.0,11459 -0.5275590419769287,0.0,0.0,0.0,11479 -0.5275590419769287,0.0,0.0,0.0,11500 -0.5275590419769287,0.0,0.0,0.0,11519 -0.5275590419769287,0.0,0.0,0.0,11539 -0.5275590419769287,0.0,0.0,0.0,11559 -0.5275590419769287,0.0,0.0,0.0,11579 -0.5118110179901123,0.0,0.0,0.0,11599 -0.5118110179901123,0.0,0.0,0.0,11620 -0.5118110179901123,0.0,0.0,0.0,11640 -0.5039370059967041,0.0,0.0,0.0,11660 -0.4803149700164795,0.0,0.0,0.0,11679 -0.4566929042339325,0.0,0.0,0.0,11699 -0.4330708682537079,0.0,0.0,0.0,11720 -0.4173228442668915,0.0,0.0,0.0,11739 -0.4094488322734833,0.0,0.0,0.0,11760 -0.4094488322734833,0.0,0.0,0.0,11780 -0.4094488322734833,0.0,0.0,0.0,11800 -0.4094488322734833,0.0,0.0,0.0,11819 -0.4094488322734833,0.0,0.0,0.0,11840 -0.3937007784843445,0.0,0.0,0.0,11859 -0.25196850299835205,-0.0390625,0.0,0.0,11879 -0.11811023950576782,-0.078125,0.0,0.0,11900 -0.06299212574958801,-0.078125,0.0,0.0,11920 -0.0,0.0,0.0,0.0,11939 -0.0,0.0,0.0,0.0,11959 -0.0,0.0,0.0,0.0,11979 -0.0,0.0,0.0,0.0,12000 -0.0,0.0,0.0,0.0,12019 -0.0,0.0,0.0,0.0,12040 -0.0,0.0,0.0,0.0,12060 -0.0,0.0,0.0,0.0,12080 -0.0,0.0,0.0,0.0,12099 -0.0,0.0,0.0,0.0,12119 -0.0,-0.1015625,0.0,0.0,12139 -0.0,-0.1015625,0.0,0.0,12159 -0.0,-0.109375,0.0,0.0,12179 -0.0,-0.1171875,0.0,0.0,12199 -0.0,-0.1171875,0.0,0.0,12219 -0.0,-0.1171875,0.0,0.0,12240 -0.0,-0.1328125,0.0,0.0,12259 -0.0,-0.15625,0.0,0.0,12279 -0.0,-0.1953125,0.0,0.0,12300 -0.0,-0.21875,0.0,0.0,12319 -0.0,-0.28125,0.0,0.0,12339 -0.0,-0.296875,0.0,0.0,12360 -0.0,-0.3046875,0.0,0.0,12379 -0.0,-0.3125,0.0,0.0,12399 -0.0,-0.3359375,0.0,0.0,12419 -0.0,-0.3359375,0.0,0.0,12440 -0.0,-0.328125,0.0,0.0,12459 -0.0,-0.328125,0.0,0.0,12479 -0.0,-0.328125,0.0,0.0,12499 -0.0,-0.328125,0.0,0.0,12520 -0.0,-0.328125,0.0,0.0,12539 -0.0,-0.328125,0.0,0.0,12560 -0.0,-0.328125,0.0,0.0,12580 -0.0,-0.328125,0.0,0.0,12599 -0.0,-0.328125,0.0,0.0,12620 -0.0,-0.328125,0.0,0.0,12639 -0.0,-0.328125,0.0,0.0,12659 -0.0,-0.328125,0.0,0.0,12679 -0.0,-0.328125,0.0,0.0,12699 -0.0,-0.328125,0.0,0.0,12719 -0.0,-0.328125,0.0,0.0,12739 -0.0,-0.328125,0.0,0.0,12760 -0.0,-0.34375,0.0,0.0,12780 -0.0,-0.3515625,0.0,0.0,12799 -0.0,-0.359375,0.0,0.0,12819 -0.0,-0.359375,0.0,0.0,12840 -0.0,-0.359375,0.0,0.0,12860 -0.0,-0.359375,0.0,0.0,12880 -0.0,-0.359375,0.0,0.0,12900 -0.0,-0.3671875,0.0,0.0,12921 -0.0,-0.3671875,0.0,0.0,12939 -0.0,-0.375,0.0,0.0,12959 -0.0,-0.375,0.0,0.0,12979 -0.0,-0.390625,0.0,0.0,12999 -0.015748031437397003,-0.40625,0.0,0.0,13019 -0.023622047156095505,-0.421875,0.0,0.0,13039 -0.03937007859349251,-0.4296875,0.0,0.0,13060 -0.03937007859349251,-0.4296875,0.0,0.0,13079 -0.04724409431219101,-0.453125,0.0,0.0,13099 -0.05511811003088951,-0.46875,0.0,0.0,13119 -0.05511811003088951,-0.484375,0.0,0.0,13139 -0.05511811003088951,-0.5,0.0,0.0,13160 -0.05511811003088951,-0.515625,0.0,0.0,13179 -0.05511811003088951,-0.515625,0.0,0.0,13199 -0.05511811003088951,-0.515625,0.0,0.0,13219 -0.05511811003088951,-0.5234375,0.0,0.0,13239 -0.05511811003088951,-0.5234375,0.0,0.0,13260 -0.05511811003088951,-0.5234375,0.0,0.0,13279 -0.05511811003088951,-0.5234375,0.0,0.0,13299 -0.05511811003088951,-0.5234375,0.0,0.0,13319 -0.04724409431219101,-0.5234375,0.0,0.0,13339 -0.04724409431219101,-0.5234375,0.0,0.0,13359 -0.04724409431219101,-0.5234375,0.0,0.0,13379 -0.04724409431219101,-0.5234375,0.0,0.0,13400 -0.04724409431219101,-0.5234375,0.0,0.0,13419 -0.04724409431219101,-0.5234375,0.0,0.0,13439 -0.04724409431219101,-0.5234375,0.0,0.0,13459 -0.04724409431219101,-0.5234375,0.0,0.0,13479 -0.04724409431219101,-0.5234375,0.0,0.0,13499 -0.04724409431219101,-0.5234375,0.0,0.0,13519 -0.04724409431219101,-0.5234375,0.0,0.0,13539 -0.04724409431219101,-0.5234375,0.0,0.0,13559 -0.04724409431219101,-0.5234375,0.0,0.0,13579 -0.04724409431219101,-0.5234375,0.0,0.0,13599 -0.04724409431219101,-0.5234375,0.0,0.0,13620 -0.04724409431219101,-0.5234375,0.0,0.0,13642 -0.04724409431219101,-0.5234375,0.0,0.0,13659 -0.04724409431219101,-0.5234375,0.0,0.0,13679 -0.04724409431219101,-0.5234375,0.0,0.0,13699 -0.04724409431219101,-0.5234375,0.0,0.0,13720 -0.04724409431219101,-0.5234375,0.0,0.0,13740 -0.04724409431219101,-0.5234375,0.0,0.0,13760 -0.04724409431219101,-0.5234375,0.0,0.0,13779 -0.04724409431219101,-0.53125,0.0,0.0,13800 -0.04724409431219101,-0.546875,0.0,0.0,13819 -0.04724409431219101,-0.5625,0.0,0.0,13840 -0.04724409431219101,-0.578125,0.0,0.0,13859 -0.04724409431219101,-0.59375,0.0,0.0,13880 -0.07874015718698502,-0.6875,0.0,0.0,13900 -0.07874015718698502,-0.703125,0.0,0.0,13919 -0.07874015718698502,-0.7265625,0.0,0.0,13944 -0.07874015718698502,-0.7421875,0.0,0.0,13959 -0.07874015718698502,-0.765625,0.0,0.0,13980 -0.07874015718698502,-0.78125,0.0,0.0,14000 -0.07874015718698502,-0.7890625,0.0,0.0,14020 -0.07874015718698502,-0.8125,0.0,0.0,14039 -0.07874015718698502,-0.8203125,0.0,0.0,14060 -0.07874015718698502,-0.828125,0.0,0.0,14079 -0.07874015718698502,-0.8359375,0.0,0.0,14099 -0.07874015718698502,-0.8359375,0.0,0.0,14119 -0.07874015718698502,-0.8359375,0.0,0.0,14139 -0.07874015718698502,-0.8359375,0.0,0.0,14159 -0.07874015718698502,-0.8359375,0.0,0.0,14179 -0.07874015718698502,-0.8203125,0.0,0.0,14199 -0.07874015718698502,-0.78125,0.0,0.0,14219 -0.007874015718698502,-0.265625,0.0,0.0,14239 -0.0,0.0,0.0,0.0,14259 -0.0,0.0,0.0,0.0,14279 -0.0,-0.1171875,0.0,0.0,14299 -0.0,-0.1328125,0.0,0.0,14319 -0.0,-0.171875,0.0,0.0,14339 -0.0,-0.171875,0.0,0.0,14359 -0.0,0.0,0.0,0.0,14379 -0.0,0.0,0.0,0.0,14399 -0.0,0.0,0.0,0.0,14419 -0.0,0.0,0.0,0.0,14439 -0.0,0.0,0.0,0.0,14459 -0.0,0.0,0.0,0.0,14479 -0.0,0.0,0.0,0.0,14499 -0.0,0.0,0.0,0.0,14519 -0.0,-0.203125,0.0,0.0,14539 -0.0,-0.4609375,0.0,0.0,14559 -0.031496062874794006,-0.71875,0.0,0.0,14579 -0.031496062874794006,-0.9453125,0.0,0.0,14599 -0.03148045240972986,-0.999504367732367,0.0,0.0,14619 -0.03148045240972986,-0.999504367732367,0.0,0.0,14639 -0.03148045240972986,-0.999504367732367,0.0,0.0,14660 -0.03148045240972986,-0.999504367732367,0.0,0.0,14679 -0.03148045240972986,-0.999504367732367,0.0,0.0,14699 -0.03148045240972986,-0.999504367732367,0.0,0.0,14719 -0.031496062874794006,-0.9609375,0.0,0.0,14739 -0.031496062874794006,-0.9609375,0.0,0.0,14759 -0.031496062874794006,-0.9609375,0.0,0.0,14780 -0.031496062874794006,-0.9609375,0.0,0.0,14799 -0.031496062874794006,-0.953125,0.0,0.0,14820 -0.031496062874794006,-0.9609375,0.0,0.0,14839 -0.03148045240972986,-0.999504367732367,0.0,0.0,14860 -0.03148045240972986,-0.999504367732367,0.0,0.0,14880 -0.03148045240972986,-0.999504367732367,0.0,0.0,14900 -0.03148045240972986,-0.999504367732367,0.0,0.0,14919 -0.031496062874794006,-0.984375,0.0,0.0,14939 -0.0,-0.3515625,0.0,0.0,14959 -0.0,0.0,0.0,0.0,14980 -0.0,0.0,0.0,0.0,14999 -0.0,0.0,0.0,0.0,15019 -0.0,0.0,0.0,0.0,15039 -0.0,0.0,0.0,0.0,15059 -0.0,0.0,0.0,0.0,15082 -0.0,0.0,0.0,0.0,15099 -0.0,0.0,0.0,0.0,15120 \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/FarDriveToChargeStation b/src/main/java/frc4388/robot/commands/FarDriveToChargeStation deleted file mode 100644 index e69de29..0000000 diff --git a/src/main/java/frc4388/robot/commands/JoystickRecorder.java b/src/main/java/frc4388/robot/commands/JoystickRecorder.java index 0682c72..76e2464 100644 --- a/src/main/java/frc4388/robot/commands/JoystickRecorder.java +++ b/src/main/java/frc4388/robot/commands/JoystickRecorder.java @@ -22,19 +22,22 @@ public class JoystickRecorder extends CommandBase { public final Supplier leftY; public final Supplier rightX; public final Supplier rightY; + private String filename; public final ArrayList outputs = new ArrayList<>(); private long startTime = -1; /** Creates a new JoystickRecorder. */ public JoystickRecorder(SwerveDrive swerve, Supplier leftX, Supplier leftY, - Supplier rightX, Supplier rightY) + Supplier rightX, Supplier rightY, + String filename) { this.swerve = swerve; this.leftX = leftX; this.leftY = leftY; this.rightX = rightX; this.rightY = rightY; + this.filename = filename; addRequirements(this.swerve); } @@ -67,7 +70,7 @@ public class JoystickRecorder extends CommandBase { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - File output = new File("/home/lvuser/BlueNearDriveToChargeStation.txt"); + File output = new File("/home/lvuser/" + filename); try (PrintWriter writer = new PrintWriter(output)) { for (var input : outputs) { diff --git a/src/main/java/frc4388/robot/commands/Taxi.txt b/src/main/java/frc4388/robot/commands/Taxi.txt new file mode 100644 index 0000000..f49eded --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Taxi.txt @@ -0,0 +1,356 @@ +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,20 +0.0,0.0,0.0,0.0,70 +0.0,0.0,0.0,0.0,85 +0.0,0.0,0.0,0.0,97 +0.0,0.0,0.0,0.0,109 +0.0,0.0,0.0,0.0,122 +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.0,0.0,0.0,220 +0.0,0.0,0.0,0.0,240 +0.0,0.0,0.0,0.0,260 +0.0,0.0,0.0,0.0,305 +0.0,0.0,0.0,0.0,314 +0.0,0.0,0.0,0.0,327 +0.0,0.0,0.0,0.0,340 +0.0,-0.1171875,0.0,0.0,360 +0.0,-0.125,0.0,0.0,380 +0.0,-0.15625,0.0,0.0,400 +0.0,-0.1796875,0.0,0.0,420 +0.0,-0.1796875,0.0,0.0,441 +0.0,-0.2109375,0.0,0.0,460 +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,541 +0.0,-0.2421875,0.0,0.0,552 +0.0,-0.2578125,0.0,0.0,565 +0.0,-0.2890625,0.0,0.0,584 +0.0,-0.2890625,0.0,0.0,600 +0.0,-0.328125,0.0,0.0,620 +0.0,-0.3515625,0.0,0.0,640 +0.0,-0.375,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,829 +0.0,-0.375,0.0,0.0,841 +0.0,-0.375,0.0,0.0,852 +0.0,-0.375,0.0,0.0,865 +0.0,-0.375,0.0,0.0,878 +0.0,-0.375,0.0,0.0,890 +0.0,-0.375,0.0,0.0,901 +0.0,-0.375,0.0,0.0,913 +0.0,-0.375,0.0,0.0,925 +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.3828125,0.0,0.0,1026 +0.0,-0.3828125,0.0,0.0,1042 +0.0,-0.3828125,0.0,0.0,1053 +0.0,-0.3828125,0.0,0.0,1066 +0.0,-0.390625,0.0,0.0,1084 +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.4140625,0.0,0.0,1180 +0.0,-0.421875,0.0,0.0,1200 +0.0,-0.453125,0.0,0.0,1222 +0.0,-0.4609375,0.0,0.0,1260 +0.0,-0.4609375,0.0,0.0,1273 +0.0,-0.4609375,0.0,0.0,1288 +0.0,-0.4609375,0.0,0.0,1301 +0.0,-0.4609375,0.0,0.0,1320 +0.0,-0.4609375,0.0,0.0,1340 +0.0,-0.4609375,0.0,0.0,1360 +0.0,-0.4609375,0.0,0.0,1380 +0.0,-0.4609375,0.0,0.0,1400 +0.0,-0.4609375,0.0,0.0,1420 +0.0,-0.4609375,0.0,0.0,1440 +0.0,-0.4609375,0.0,0.0,1460 +0.0,-0.4609375,0.0,0.0,1503 +0.0,-0.4609375,0.0,0.0,1513 +0.0,-0.4609375,0.0,0.0,1524 +0.0,-0.4609375,0.0,0.0,1540 +0.0,-0.4609375,0.0,0.0,1560 +0.0,-0.4609375,0.0,0.0,1580 +0.0,-0.4609375,0.0,0.0,1600 +0.0,-0.4609375,0.0,0.0,1620 +0.0,-0.4609375,0.0,0.0,1640 +0.0,-0.4609375,0.0,0.0,1660 +0.0,-0.4609375,0.0,0.0,1680 +0.0,-0.4609375,0.0,0.0,1700 +0.0,-0.4609375,0.0,0.0,1745 +0.0,-0.4609375,0.0,0.0,1760 +0.0,-0.4609375,0.0,0.0,1772 +0.0,-0.4609375,0.0,0.0,1783 +0.0,-0.4609375,0.0,0.0,1800 +0.0,-0.4609375,0.0,0.0,1820 +0.0,-0.4609375,0.0,0.0,1840 +0.0,-0.4609375,0.0,0.0,1860 +0.0,-0.4609375,0.0,0.0,1880 +0.0,-0.4609375,0.0,0.0,1900 +0.0,-0.4609375,0.0,0.0,1920 +0.0,-0.4609375,0.0,0.0,1941 +0.0,-0.4609375,0.0,0.0,2018 +0.0,-0.4609375,0.0,0.0,2030 +0.0,-0.4609375,0.0,0.0,2040 +0.0,-0.4609375,0.0,0.0,2052 +0.0,-0.4609375,0.0,0.0,2064 +0.0,-0.4609375,0.0,0.0,2083 +0.0,-0.4609375,0.0,0.0,2092 +0.0,-0.4609375,0.0,0.0,2105 +0.0,-0.4609375,0.0,0.0,2120 +0.0,-0.4609375,0.0,0.0,2140 +0.0,-0.4609375,0.0,0.0,2160 +0.0,-0.4609375,0.0,0.0,2180 +0.0,-0.4609375,0.0,0.0,2221 +0.0,-0.4609375,0.0,0.0,2235 +0.0,-0.4609375,0.0,0.0,2253 +0.0,-0.4609375,0.0,0.0,2269 +0.0,-0.4609375,0.0,0.0,2283 +0.0,-0.4609375,0.0,0.0,2301 +0.0,-0.4609375,0.0,0.0,2321 +0.0,-0.4609375,0.0,0.0,2340 +0.0,-0.4609375,0.0,0.0,2360 +0.0,-0.4609375,0.0,0.0,2380 +0.0,-0.4609375,0.0,0.0,2400 +0.0,-0.4609375,0.0,0.0,2423 +0.0,-0.4609375,0.0,0.0,2463 +0.0,-0.4609375,0.0,0.0,2474 +0.0,-0.4609375,0.0,0.0,2487 +0.0,-0.4609375,0.0,0.0,2499 +0.0,-0.4609375,0.0,0.0,2520 +0.0,-0.4609375,0.0,0.0,2540 +0.0,-0.4609375,0.0,0.0,2560 +0.0,-0.4609375,0.0,0.0,2580 +0.0,-0.4609375,0.0,0.0,2600 +0.0,-0.4609375,0.0,0.0,2620 +0.0,-0.4609375,0.0,0.0,2640 +0.0,-0.4609375,0.0,0.0,2660 +0.0,-0.4609375,0.0,0.0,2702 +0.0,-0.4609375,0.0,0.0,2716 +0.0,-0.4609375,0.0,0.0,2728 +0.0,-0.4609375,0.0,0.0,2740 +0.0,-0.4609375,0.0,0.0,2760 +0.0,-0.4609375,0.0,0.0,2780 +0.0,-0.4609375,0.0,0.0,2801 +0.0,-0.453125,0.0,0.0,2820 +0.0,-0.4296875,0.0,0.0,2840 +0.0,-0.3984375,0.0,0.0,2861 +0.0,-0.3828125,0.0,0.0,2880 +0.0,-0.234375,0.0,0.0,2900 +0.0,0.0,0.0,0.0,2937 +0.0,0.0,0.0,0.0,2953 +0.0,0.0,0.0,0.0,2965 +0.0,0.0,0.0,0.0,2980 +0.0,0.0,0.0,0.0,3000 +0.0,0.0,0.0,0.0,3020 +0.0,0.0,0.0,0.0,3040 +0.0,0.0,0.0,0.0,3060 +0.0,0.0,0.0,0.0,3080 +0.0,0.0,0.0,0.0,3100 +0.0,0.0,0.0,0.0,3131 +0.0,0.0,0.0,0.0,3210 +0.0,0.0,0.0,0.0,3223 +0.0,0.0,0.0,0.0,3236 +0.0,0.0,0.0,0.0,3247 +0.0,0.0,0.0,0.0,3258 +0.0,0.0,0.0,0.0,3270 +0.0,0.0,0.0,0.0,3283 +0.0,0.0,0.0,0.0,3293 +0.0,0.0,0.0,0.0,3305 +0.0,0.0,0.0,0.0,3320 +0.0,0.0,0.0,0.0,3340 +0.0,0.0,0.0,0.0,3360 +0.0,0.0,0.0,0.0,3380 +0.0,0.0,0.0,0.0,3422 +0.0,0.0,0.0,0.0,3436 +0.0,0.0,0.0,0.0,3448 +0.0,0.0,0.0,0.0,3463 +0.0,0.0,0.0,0.0,3480 +0.0,0.0,0.0,0.0,3500 +0.0,0.0,0.0,0.0,3520 +0.0,0.0,0.0,0.0,3540 +0.0,0.0,0.0,0.0,3560 +0.0,0.0,0.0,0.0,3580 +0.0,0.0,0.0,0.0,3600 +0.0,0.0,0.0,0.0,3621 +0.0,0.0,0.0,0.0,3664 +0.0,0.0,0.0,0.0,3677 +0.0,0.0,0.0,0.0,3693 +0.0,0.0,0.0,0.0,3706 +0.0,0.0,0.0,0.0,3720 +0.0,0.0,0.0,0.0,3740 +0.0,0.0,0.0,0.0,3769 +0.0,0.0,0.0,0.0,3790 +0.0,0.0,0.0,0.0,3801 +0.0,0.0,0.0,0.0,3820 +0.0,0.0,0.0,0.0,3840 +0.0,0.0,0.0,0.0,3860 +0.0,0.0,0.0,0.0,3902 +0.0,0.0,0.0,0.0,3915 +0.0,0.0,0.0,0.0,3928 +0.0,0.0,0.0,0.0,3943 +0.0,0.0,0.0,0.0,3960 +0.0,0.0,0.0,0.0,3980 +0.0,0.0,0.0,0.0,4000 +0.0,0.0,0.0,0.0,4020 +0.0,0.0,0.0,0.0,4040 +0.0,0.0,0.0,0.0,4060 +0.0,0.0,0.0,0.0,4080 +0.0,0.0,0.0,0.0,4100 +0.0,0.0,0.0,0.0,4138 +0.0,0.0,0.0,0.0,4151 +0.0,0.0,0.0,0.0,4164 +0.0,0.0,0.0,0.0,4180 +0.0,0.0,0.0,0.0,4200 +0.0,0.0,0.0,0.0,4220 +0.0,0.0,0.0,0.0,4240 +0.0,0.0,0.0,0.0,4261 +0.0,0.0,0.0,0.0,4280 +0.0,0.0,0.0,0.0,4300 +0.0,0.0,0.0,0.0,4320 +0.0,0.0,0.0,0.0,4340 +0.0,0.0,0.0,0.0,4438 +0.0,0.0,0.0,0.0,4449 +0.0,0.0,0.0,0.0,4460 +0.0,0.0,0.0,0.0,4471 +0.0,0.0,0.0,0.0,4484 +0.0,0.0,0.0,0.0,4496 +0.0,0.0,0.0,0.0,4508 +0.0,0.0,0.0,0.0,4520 +0.0,0.0,0.0,0.0,4532 +0.0,0.0,0.0,0.0,4543 +0.0,0.0,0.0,0.0,4560 +0.0,0.0,0.0,0.0,4580 +0.0,0.0,0.0,0.0,4624 +0.0,0.0,0.0,0.0,4638 +0.0,0.0,0.0,0.0,4651 +0.0,0.0,0.0,0.0,4665 +0.0,0.0,0.0,0.0,4679 +0.0,0.0,0.0,0.0,4700 +0.0,0.0,0.0,0.0,4720 +0.0,0.0,0.0,0.0,4740 +0.0,0.0,0.0,0.0,4760 +0.0,0.0,0.0,0.0,4780 +0.0,0.0,0.0,0.0,4800 +0.0,0.0,0.0,0.0,4820 +0.0,0.0,0.0,0.0,4859 +0.0,0.0,0.0,0.0,4871 +0.0,0.0,0.0,0.0,4893 +0.0,0.0,0.0,0.0,4914 +0.0,0.0,0.0,0.0,4927 +0.0,0.0,0.0,0.0,4940 +0.0,0.0,0.0,0.0,4960 +0.0,0.0,0.0,0.0,4980 +0.11023622006177902,0.0,0.0,0.0,5000 +0.13385826349258423,0.0,0.0,0.0,5020 +0.17322835326194763,0.0,0.0,0.0,5040 +0.21259842813014984,0.0,0.0,0.0,5060 +0.29921260476112366,0.007874015718698502,0.0,0.0,5104 +0.4015747904777527,0.04724409431219101,0.0,0.0,5123 +0.4015747904777527,0.04724409431219101,0.0,0.0,5133 +0.4251968562602997,0.05511811003088951,0.0,0.0,5143 +0.4251968562602997,0.05511811003088951,0.0,0.0,5160 +0.4251968562602997,0.05511811003088951,0.0,0.0,5180 +0.4330708682537079,0.05511811003088951,0.0,0.0,5200 +0.4330708682537079,0.04724409431219101,0.0,0.0,5220 +0.4330708682537079,0.04724409431219101,0.0,0.0,5240 +0.4330708682537079,0.04724409431219101,0.0,0.0,5260 +0.4330708682537079,0.04724409431219101,0.0,0.0,5280 +0.4330708682537079,0.04724409431219101,0.0,0.0,5300 +0.4330708682537079,0.04724409431219101,0.0,0.0,5343 +0.4330708682537079,0.04724409431219101,0.0,0.0,5353 +0.4330708682537079,0.04724409431219101,0.0,0.0,5366 +0.4330708682537079,0.04724409431219101,0.0,0.0,5380 +0.4645669162273407,0.04724409431219101,0.0,0.0,5400 +0.4724409580230713,0.04724409431219101,0.0,0.0,5420 +0.4724409580230713,0.04724409431219101,0.0,0.0,5440 +0.4724409580230713,0.04724409431219101,0.0,0.0,5460 +0.4724409580230713,0.04724409431219101,0.0,0.0,5480 +0.4724409580230713,0.04724409431219101,0.0,0.0,5500 +0.4724409580230713,0.04724409431219101,0.0,0.0,5520 +0.4724409580230713,0.04724409431219101,0.0,0.0,5540 +0.4724409580230713,0.04724409431219101,0.0,0.0,5628 +0.4724409580230713,0.04724409431219101,0.0,0.0,5641 +0.4724409580230713,0.04724409431219101,0.0,0.0,5652 +0.4724409580230713,0.04724409431219101,0.0,0.0,5663 +0.4724409580230713,0.04724409431219101,0.0,0.0,5676 +0.4724409580230713,0.04724409431219101,0.0,0.0,5688 +0.4724409580230713,0.04724409431219101,0.0,0.0,5701 +0.4724409580230713,0.04724409431219101,0.0,0.0,5710 +0.4724409580230713,0.04724409431219101,0.0,0.0,5724 +0.4724409580230713,0.04724409431219101,0.0,0.0,5740 +0.4724409580230713,0.04724409431219101,0.0,0.0,5760 +0.4724409580230713,0.04724409431219101,0.0,0.0,5780 +0.4724409580230713,0.04724409431219101,0.0,0.0,5821 +0.4724409580230713,0.04724409431219101,0.0,0.0,5832 +0.4724409580230713,0.04724409431219101,0.0,0.0,5845 +0.4724409580230713,0.04724409431219101,0.0,0.0,5860 +0.4724409580230713,0.04724409431219101,0.0,0.0,5880 +0.4724409580230713,0.04724409431219101,0.0,0.0,5900 +0.4724409580230713,0.04724409431219101,0.0,0.0,5920 +0.4724409580230713,0.04724409431219101,0.0,0.0,5940 +0.4724409580230713,0.04724409431219101,0.0,0.0,5960 +0.4724409580230713,0.04724409431219101,0.0,0.0,5980 +0.4724409580230713,0.03937007859349251,0.0,0.0,6000 +0.4724409580230713,0.03937007859349251,0.0,0.0,6020 +0.4724409580230713,0.031496062874794006,0.0,0.0,6062 +0.4724409580230713,0.031496062874794006,0.0,0.0,6073 +0.4724409580230713,0.031496062874794006,0.0,0.0,6086 +0.4724409580230713,0.031496062874794006,0.0,0.0,6100 +0.4724409580230713,0.031496062874794006,0.0,0.0,6120 +0.4724409580230713,0.031496062874794006,0.0,0.0,6140 +0.4724409580230713,0.031496062874794006,0.0,0.0,6160 +0.4724409580230713,0.031496062874794006,0.0,0.0,6180 +0.4724409580230713,0.031496062874794006,0.0,0.0,6200 +0.4724409580230713,0.031496062874794006,0.0,0.0,6220 +0.4724409580230713,0.031496062874794006,0.0,0.0,6240 +0.4724409580230713,0.031496062874794006,0.0,0.0,6260 +0.4724409580230713,0.031496062874794006,0.0,0.0,6305 +0.4724409580230713,0.031496062874794006,0.0,0.0,6318 +0.4724409580230713,0.031496062874794006,0.0,0.0,6330 +0.4724409580230713,0.031496062874794006,0.0,0.0,6349 +0.4724409580230713,0.031496062874794006,0.0,0.0,6369 +0.4724409580230713,0.031496062874794006,0.0,0.0,6382 +0.4724409580230713,0.031496062874794006,0.0,0.0,6404 +0.4724409580230713,0.031496062874794006,0.0,0.0,6420 +0.4724409580230713,0.031496062874794006,0.0,0.0,6440 +0.4724409580230713,0.031496062874794006,0.0,0.0,6460 +0.4724409580230713,0.031496062874794006,0.0,0.0,6480 +0.4724409580230713,0.031496062874794006,0.0,0.0,6500 +0.4724409580230713,0.031496062874794006,0.0,0.0,6543 +0.4724409580230713,0.031496062874794006,0.0,0.0,6555 +0.4724409580230713,0.031496062874794006,0.0,0.0,6568 +0.4724409580230713,0.031496062874794006,0.0,0.0,6583 +0.31496062874794006,0.0,0.0,0.0,6600 +0.0,0.0,0.0,0.0,6620 +0.0,0.0,0.0,0.0,6640 +0.0,0.0,0.0,0.0,6660 +0.0,0.0,0.0,0.0,6680 +0.0,0.0,0.0,0.0,6700 +0.0,0.0,0.0,0.0,6720 +0.0,0.0,0.0,0.0,6740 +0.0,0.0,0.0,0.0,6839 +0.0,0.0,0.0,0.0,6849 +0.0,0.0,0.0,0.0,6862 +0.0,0.0,0.0,0.0,6873 +0.0,0.0,0.0,0.0,6888 +0.0,0.0,0.0,0.0,6901 +0.0,0.0,0.0,0.0,6911 +0.0,0.0,0.0,0.0,6922 +0.0,0.0,0.0,0.0,6934 +0.0,0.0,0.0,0.0,6945 +0.0,0.0,0.0,0.0,6960 +0.0,0.0,0.0,0.0,6980 +0.0,0.0,0.0,0.0,7022 +0.0,0.0,0.0,0.0,7034 +0.0,0.0,0.0,0.0,7043 +0.0,0.0,0.0,0.0,7060 +0.0,0.0,0.0,0.0,7080 \ No newline at end of file From b620098fdbd743f2cbae8aa800b3811ba522970a Mon Sep 17 00:00:00 2001 From: Aarav Date: Sat, 25 Feb 2023 14:11:31 -0700 Subject: [PATCH 28/40] chooser moved to Robot.java --- src/main/java/frc4388/robot/Robot.java | 5 ++++- src/main/java/frc4388/robot/RobotContainer.java | 9 +++------ 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 61a27d3..1652902 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -13,6 +13,7 @@ import java.io.PrintWriter; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc4388.utility.RobotTime; @@ -41,7 +42,7 @@ public class Robot extends TimedRobot { // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); - + SmartDashboard.putData("AutoPlayback Chooser", m_robotContainer.chooser); } /** @@ -70,6 +71,8 @@ public class Robot extends TimedRobot { @Override public void disabledInit() { m_robotTime.endMatchTime(); + + SmartDashboard.putData("AutoPlayback Chooser", m_robotContainer.chooser); } @Override diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 279be06..c36a39a 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -64,7 +64,7 @@ public class RobotContainer { private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); /* Autos */ - private SendableChooser chooser = new SendableChooser<>(); + public SendableChooser chooser = new SendableChooser<>(); private Command noAuto = new InstantCommand(); @@ -102,8 +102,6 @@ public class RobotContainer { chooser.addOption("Red1PathWithBalance", red1PathWithBalance); chooser.addOption("Taxi", taxi); - - SmartDashboard.putData(chooser); } @@ -134,8 +132,8 @@ public class RobotContainer { "Blue1Path.txt")) .onFalse(new InstantCommand()); - new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) - .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt")); + // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) + // .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt")); // * Operator Buttons } @@ -148,7 +146,6 @@ public class RobotContainer { public Command getAutonomousCommand() { return chooser.getSelected(); - // return new InstantCommand(); } public DeadbandedXboxController getDeadbandedDriverController() { From e3c8011e5e9ff09987f1f97a82ade0f8fd5a1227 Mon Sep 17 00:00:00 2001 From: 66945 <54561572+66945@users.noreply.github.com> Date: Sat, 25 Feb 2023 14:16:32 -0700 Subject: [PATCH 29/40] stuff --- .../java/frc4388/robot/PlaybackChooser.java | 78 +++++++++++++++++++ .../java/frc4388/robot/RobotContainer.java | 9 +++ 2 files changed, 87 insertions(+) create mode 100644 src/main/java/frc4388/robot/PlaybackChooser.java diff --git a/src/main/java/frc4388/robot/PlaybackChooser.java b/src/main/java/frc4388/robot/PlaybackChooser.java new file mode 100644 index 0000000..77d20c1 --- /dev/null +++ b/src/main/java/frc4388/robot/PlaybackChooser.java @@ -0,0 +1,78 @@ +package frc4388.robot; + +import java.io.File; +import java.util.ArrayList; +import java.util.HashMap; + +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc4388.robot.commands.JoystickPlayback; +import frc4388.robot.subsystems.SwerveDrive; + +public class PlaybackChooser { + private ArrayList> m_choosers = new ArrayList<>(); + private SendableChooser m_playback = new SendableChooser<>(); + private HashMap m_commandPool = new HashMap<>(); + + private File m_dir = new File("/home/lvuser/autos/"); + + private SwerveDrive m_swerve; + + // commands + private Command m_noAuto = new InstantCommand(); + + public PlaybackChooser(SwerveDrive swerve, Object... pool) { + m_swerve = swerve; + + for (int i = 0; i < pool.length; i += 2) { + if (!(pool[i] instanceof String)) throw new RuntimeException("Need (string, command)"); + if (!(pool[i + 1] instanceof Command)) throw new RuntimeException("Need (string, command)"); + + m_commandPool.put((String) pool[i], (Command) pool[i + 1]); + } + + m_playback.addOption("No Auto", m_noAuto); + for (String auto : m_dir.list()) { + m_playback.addOption(auto, new JoystickPlayback(m_swerve, auto)); + } + + m_choosers.add(m_playback); + SmartDashboard.putData(m_playback); + } + + // This will be bound to a button for the time being + public void appendCommand() { + var chooser = new SendableChooser(); + + for (var cmd_name : m_commandPool.keySet()) { + chooser.addOption(cmd_name, m_commandPool.get(cmd_name)); + } + + m_choosers.add(chooser); + SmartDashboard.putData("Command: " + m_choosers.size(), chooser); + } + + // This will be bound to a button for the time being + public void appendPlayback() { + var chooser = new SendableChooser(); + + for (String auto : m_dir.list()) { + m_playback.addOption(auto, new JoystickPlayback(m_swerve, auto)); + } + + m_choosers.add(chooser); + SmartDashboard.putData("Command: " + m_choosers.size(), chooser); + } + + public Command getCommand() { + Command command = m_playback.getSelected(); + + for (int i = 1; i < m_choosers.size(); i++) { + command.andThen(m_choosers.get(i).getSelected()); + } + + return command; + } +} diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 279be06..fe4420c 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -104,6 +104,15 @@ public class RobotContainer { chooser.addOption("Taxi", taxi); SmartDashboard.putData(chooser); + + PlaybackChooser playbackChooser = new PlaybackChooser(m_robotSwerveDrive, + "Balance", new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); + + new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) + .onTrue(new InstantCommand(() -> playbackChooser.appendCommand())); + + new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) + .onTrue(new InstantCommand(() -> playbackChooser.appendPlayback())); } From f10728410246dc2569dffa099d461553b893a1d5 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 25 Feb 2023 14:39:41 -0700 Subject: [PATCH 30/40] autos directory --- src/main/java/frc4388/robot/RobotContainer.java | 3 +-- src/main/java/frc4388/robot/commands/JoystickPlayback.java | 2 +- src/main/java/frc4388/robot/commands/JoystickRecorder.java | 2 +- .../java/frc4388/robot/{ => commands}/PlaybackChooser.java | 5 ++--- 4 files changed, 5 insertions(+), 7 deletions(-) rename src/main/java/frc4388/robot/{ => commands}/PlaybackChooser.java (95%) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index d0c9329..d4411b3 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -37,6 +37,7 @@ import frc4388.robot.Constants.SwerveDriveConstants.PIDConstants; import frc4388.robot.commands.AutoBalance; import frc4388.robot.commands.JoystickPlayback; import frc4388.robot.commands.JoystickRecorder; +import frc4388.robot.commands.PlaybackChooser; import frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.controller.DeadbandedXboxController; import frc4388.utility.controller.XboxController; @@ -103,8 +104,6 @@ public class RobotContainer { chooser.addOption("Taxi", taxi); - SmartDashboard.putData(chooser); - PlaybackChooser playbackChooser = new PlaybackChooser(m_robotSwerveDrive, "Balance", new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); diff --git a/src/main/java/frc4388/robot/commands/JoystickPlayback.java b/src/main/java/frc4388/robot/commands/JoystickPlayback.java index c844abd..9519497 100644 --- a/src/main/java/frc4388/robot/commands/JoystickPlayback.java +++ b/src/main/java/frc4388/robot/commands/JoystickPlayback.java @@ -46,7 +46,7 @@ public class JoystickPlayback extends CommandBase { playbackTime = 0; lastIndex = 0; try { - input = new Scanner(new File("/home/lvuser/" + filename)); + input = new Scanner(new File("/home/lvuser/autos/" + filename)); String line = ""; while (input.hasNextLine()) { diff --git a/src/main/java/frc4388/robot/commands/JoystickRecorder.java b/src/main/java/frc4388/robot/commands/JoystickRecorder.java index 76e2464..d1bf047 100644 --- a/src/main/java/frc4388/robot/commands/JoystickRecorder.java +++ b/src/main/java/frc4388/robot/commands/JoystickRecorder.java @@ -70,7 +70,7 @@ public class JoystickRecorder extends CommandBase { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - File output = new File("/home/lvuser/" + filename); + File output = new File("/home/lvuser/autos/" + filename); try (PrintWriter writer = new PrintWriter(output)) { for (var input : outputs) { diff --git a/src/main/java/frc4388/robot/PlaybackChooser.java b/src/main/java/frc4388/robot/commands/PlaybackChooser.java similarity index 95% rename from src/main/java/frc4388/robot/PlaybackChooser.java rename to src/main/java/frc4388/robot/commands/PlaybackChooser.java index 77d20c1..2934e40 100644 --- a/src/main/java/frc4388/robot/PlaybackChooser.java +++ b/src/main/java/frc4388/robot/commands/PlaybackChooser.java @@ -1,4 +1,4 @@ -package frc4388.robot; +package frc4388.robot.commands; import java.io.File; import java.util.ArrayList; @@ -8,7 +8,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; -import frc4388.robot.commands.JoystickPlayback; import frc4388.robot.subsystems.SwerveDrive; public class PlaybackChooser { @@ -39,7 +38,7 @@ public class PlaybackChooser { } m_choosers.add(m_playback); - SmartDashboard.putData(m_playback); + SmartDashboard.putData("Command: 0", m_playback); } // This will be bound to a button for the time being From 7712134aeb9020d17022dc3110b859d681744af9 Mon Sep 17 00:00:00 2001 From: Aarav Date: Sat, 25 Feb 2023 15:03:05 -0700 Subject: [PATCH 31/40] change --- src/main/java/frc4388/robot/Robot.java | 2 - .../robot/commands/JoystickPlayback.java | 3 + .../robot/commands/JoystickRecorder.java | 4 + src/main/java/frc4388/robot/commands/Taxi.txt | 575 +++++++----------- 4 files changed, 229 insertions(+), 355 deletions(-) diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 1652902..7995804 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -71,8 +71,6 @@ public class Robot extends TimedRobot { @Override public void disabledInit() { m_robotTime.endMatchTime(); - - SmartDashboard.putData("AutoPlayback Chooser", m_robotContainer.chooser); } @Override diff --git a/src/main/java/frc4388/robot/commands/JoystickPlayback.java b/src/main/java/frc4388/robot/commands/JoystickPlayback.java index 9519497..0f496a9 100644 --- a/src/main/java/frc4388/robot/commands/JoystickPlayback.java +++ b/src/main/java/frc4388/robot/commands/JoystickPlayback.java @@ -42,6 +42,9 @@ public class JoystickPlayback extends CommandBase { // Called when the command is initially scheduled. @Override public void initialize() { + outputs.clear(); + m_finished = false; + startTime = System.currentTimeMillis(); playbackTime = 0; lastIndex = 0; diff --git a/src/main/java/frc4388/robot/commands/JoystickRecorder.java b/src/main/java/frc4388/robot/commands/JoystickRecorder.java index d1bf047..eea72b4 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 { // Called when the command is initially scheduled. @Override public void initialize() { + outputs.clear(); + this.startTime = System.currentTimeMillis(); outputs.add(new TimedOutput()); @@ -65,6 +67,8 @@ public class JoystickRecorder extends CommandBase { swerve.driveWithInput(new Translation2d(inputs.leftX, inputs.leftY), new Translation2d(inputs.rightX, inputs.rightY), true); + + System.out.println("RECORDING"); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/commands/Taxi.txt b/src/main/java/frc4388/robot/commands/Taxi.txt index f49eded..c99ee2c 100644 --- a/src/main/java/frc4388/robot/commands/Taxi.txt +++ b/src/main/java/frc4388/robot/commands/Taxi.txt @@ -1,356 +1,225 @@ 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,20 -0.0,0.0,0.0,0.0,70 -0.0,0.0,0.0,0.0,85 -0.0,0.0,0.0,0.0,97 -0.0,0.0,0.0,0.0,109 -0.0,0.0,0.0,0.0,122 -0.0,0.0,0.0,0.0,140 +0.0,0.0,0.0,0.0,0 +0.0,0.0,0.0,0.0,12 +0.0,0.0,0.0,0.0,26 +0.0,0.0,0.0,0.0,37 +0.0,0.0,0.0,0.0,50 +0.0,0.0,0.0,0.0,62 +0.0,0.0,0.0,0.0,73 +0.0,0.0,0.0,0.0,88 +0.0,0.0,0.0,0.0,103 +0.0,0.0,0.0,0.0,116 0.0,0.0,0.0,0.0,160 -0.0,0.0,0.0,0.0,180 +0.0,0.0,0.0,0.0,173 +0.0,0.0,0.0,0.0,185 0.0,0.0,0.0,0.0,200 -0.0,0.0,0.0,0.0,220 -0.0,0.0,0.0,0.0,240 -0.0,0.0,0.0,0.0,260 -0.0,0.0,0.0,0.0,305 -0.0,0.0,0.0,0.0,314 -0.0,0.0,0.0,0.0,327 -0.0,0.0,0.0,0.0,340 -0.0,-0.1171875,0.0,0.0,360 -0.0,-0.125,0.0,0.0,380 -0.0,-0.15625,0.0,0.0,400 -0.0,-0.1796875,0.0,0.0,420 -0.0,-0.1796875,0.0,0.0,441 -0.0,-0.2109375,0.0,0.0,460 -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,541 -0.0,-0.2421875,0.0,0.0,552 -0.0,-0.2578125,0.0,0.0,565 -0.0,-0.2890625,0.0,0.0,584 -0.0,-0.2890625,0.0,0.0,600 -0.0,-0.328125,0.0,0.0,620 -0.0,-0.3515625,0.0,0.0,640 -0.0,-0.375,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,829 -0.0,-0.375,0.0,0.0,841 -0.0,-0.375,0.0,0.0,852 -0.0,-0.375,0.0,0.0,865 -0.0,-0.375,0.0,0.0,878 -0.0,-0.375,0.0,0.0,890 -0.0,-0.375,0.0,0.0,901 -0.0,-0.375,0.0,0.0,913 -0.0,-0.375,0.0,0.0,925 -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.3828125,0.0,0.0,1026 -0.0,-0.3828125,0.0,0.0,1042 -0.0,-0.3828125,0.0,0.0,1053 -0.0,-0.3828125,0.0,0.0,1066 -0.0,-0.390625,0.0,0.0,1084 -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.4140625,0.0,0.0,1180 -0.0,-0.421875,0.0,0.0,1200 -0.0,-0.453125,0.0,0.0,1222 -0.0,-0.4609375,0.0,0.0,1260 -0.0,-0.4609375,0.0,0.0,1273 -0.0,-0.4609375,0.0,0.0,1288 -0.0,-0.4609375,0.0,0.0,1301 -0.0,-0.4609375,0.0,0.0,1320 -0.0,-0.4609375,0.0,0.0,1340 -0.0,-0.4609375,0.0,0.0,1360 -0.0,-0.4609375,0.0,0.0,1380 -0.0,-0.4609375,0.0,0.0,1400 -0.0,-0.4609375,0.0,0.0,1420 -0.0,-0.4609375,0.0,0.0,1440 -0.0,-0.4609375,0.0,0.0,1460 -0.0,-0.4609375,0.0,0.0,1503 -0.0,-0.4609375,0.0,0.0,1513 -0.0,-0.4609375,0.0,0.0,1524 -0.0,-0.4609375,0.0,0.0,1540 -0.0,-0.4609375,0.0,0.0,1560 -0.0,-0.4609375,0.0,0.0,1580 -0.0,-0.4609375,0.0,0.0,1600 -0.0,-0.4609375,0.0,0.0,1620 -0.0,-0.4609375,0.0,0.0,1640 -0.0,-0.4609375,0.0,0.0,1660 -0.0,-0.4609375,0.0,0.0,1680 -0.0,-0.4609375,0.0,0.0,1700 -0.0,-0.4609375,0.0,0.0,1745 -0.0,-0.4609375,0.0,0.0,1760 -0.0,-0.4609375,0.0,0.0,1772 -0.0,-0.4609375,0.0,0.0,1783 -0.0,-0.4609375,0.0,0.0,1800 -0.0,-0.4609375,0.0,0.0,1820 -0.0,-0.4609375,0.0,0.0,1840 -0.0,-0.4609375,0.0,0.0,1860 -0.0,-0.4609375,0.0,0.0,1880 -0.0,-0.4609375,0.0,0.0,1900 -0.0,-0.4609375,0.0,0.0,1920 -0.0,-0.4609375,0.0,0.0,1941 -0.0,-0.4609375,0.0,0.0,2018 -0.0,-0.4609375,0.0,0.0,2030 -0.0,-0.4609375,0.0,0.0,2040 -0.0,-0.4609375,0.0,0.0,2052 -0.0,-0.4609375,0.0,0.0,2064 -0.0,-0.4609375,0.0,0.0,2083 -0.0,-0.4609375,0.0,0.0,2092 -0.0,-0.4609375,0.0,0.0,2105 -0.0,-0.4609375,0.0,0.0,2120 -0.0,-0.4609375,0.0,0.0,2140 -0.0,-0.4609375,0.0,0.0,2160 -0.0,-0.4609375,0.0,0.0,2180 -0.0,-0.4609375,0.0,0.0,2221 -0.0,-0.4609375,0.0,0.0,2235 -0.0,-0.4609375,0.0,0.0,2253 -0.0,-0.4609375,0.0,0.0,2269 -0.0,-0.4609375,0.0,0.0,2283 -0.0,-0.4609375,0.0,0.0,2301 -0.0,-0.4609375,0.0,0.0,2321 -0.0,-0.4609375,0.0,0.0,2340 -0.0,-0.4609375,0.0,0.0,2360 -0.0,-0.4609375,0.0,0.0,2380 -0.0,-0.4609375,0.0,0.0,2400 -0.0,-0.4609375,0.0,0.0,2423 -0.0,-0.4609375,0.0,0.0,2463 -0.0,-0.4609375,0.0,0.0,2474 -0.0,-0.4609375,0.0,0.0,2487 -0.0,-0.4609375,0.0,0.0,2499 -0.0,-0.4609375,0.0,0.0,2520 -0.0,-0.4609375,0.0,0.0,2540 -0.0,-0.4609375,0.0,0.0,2560 -0.0,-0.4609375,0.0,0.0,2580 -0.0,-0.4609375,0.0,0.0,2600 -0.0,-0.4609375,0.0,0.0,2620 -0.0,-0.4609375,0.0,0.0,2640 -0.0,-0.4609375,0.0,0.0,2660 -0.0,-0.4609375,0.0,0.0,2702 -0.0,-0.4609375,0.0,0.0,2716 -0.0,-0.4609375,0.0,0.0,2728 -0.0,-0.4609375,0.0,0.0,2740 -0.0,-0.4609375,0.0,0.0,2760 -0.0,-0.4609375,0.0,0.0,2780 -0.0,-0.4609375,0.0,0.0,2801 -0.0,-0.453125,0.0,0.0,2820 -0.0,-0.4296875,0.0,0.0,2840 -0.0,-0.3984375,0.0,0.0,2861 -0.0,-0.3828125,0.0,0.0,2880 -0.0,-0.234375,0.0,0.0,2900 -0.0,0.0,0.0,0.0,2937 -0.0,0.0,0.0,0.0,2953 -0.0,0.0,0.0,0.0,2965 -0.0,0.0,0.0,0.0,2980 -0.0,0.0,0.0,0.0,3000 -0.0,0.0,0.0,0.0,3020 -0.0,0.0,0.0,0.0,3040 -0.0,0.0,0.0,0.0,3060 -0.0,0.0,0.0,0.0,3080 -0.0,0.0,0.0,0.0,3100 -0.0,0.0,0.0,0.0,3131 -0.0,0.0,0.0,0.0,3210 -0.0,0.0,0.0,0.0,3223 -0.0,0.0,0.0,0.0,3236 -0.0,0.0,0.0,0.0,3247 -0.0,0.0,0.0,0.0,3258 -0.0,0.0,0.0,0.0,3270 -0.0,0.0,0.0,0.0,3283 -0.0,0.0,0.0,0.0,3293 -0.0,0.0,0.0,0.0,3305 -0.0,0.0,0.0,0.0,3320 -0.0,0.0,0.0,0.0,3340 -0.0,0.0,0.0,0.0,3360 -0.0,0.0,0.0,0.0,3380 -0.0,0.0,0.0,0.0,3422 -0.0,0.0,0.0,0.0,3436 -0.0,0.0,0.0,0.0,3448 -0.0,0.0,0.0,0.0,3463 -0.0,0.0,0.0,0.0,3480 -0.0,0.0,0.0,0.0,3500 -0.0,0.0,0.0,0.0,3520 -0.0,0.0,0.0,0.0,3540 -0.0,0.0,0.0,0.0,3560 -0.0,0.0,0.0,0.0,3580 -0.0,0.0,0.0,0.0,3600 -0.0,0.0,0.0,0.0,3621 -0.0,0.0,0.0,0.0,3664 -0.0,0.0,0.0,0.0,3677 -0.0,0.0,0.0,0.0,3693 -0.0,0.0,0.0,0.0,3706 -0.0,0.0,0.0,0.0,3720 -0.0,0.0,0.0,0.0,3740 -0.0,0.0,0.0,0.0,3769 -0.0,0.0,0.0,0.0,3790 -0.0,0.0,0.0,0.0,3801 -0.0,0.0,0.0,0.0,3820 -0.0,0.0,0.0,0.0,3840 -0.0,0.0,0.0,0.0,3860 -0.0,0.0,0.0,0.0,3902 -0.0,0.0,0.0,0.0,3915 -0.0,0.0,0.0,0.0,3928 -0.0,0.0,0.0,0.0,3943 -0.0,0.0,0.0,0.0,3960 -0.0,0.0,0.0,0.0,3980 -0.0,0.0,0.0,0.0,4000 -0.0,0.0,0.0,0.0,4020 -0.0,0.0,0.0,0.0,4040 -0.0,0.0,0.0,0.0,4060 -0.0,0.0,0.0,0.0,4080 -0.0,0.0,0.0,0.0,4100 -0.0,0.0,0.0,0.0,4138 -0.0,0.0,0.0,0.0,4151 -0.0,0.0,0.0,0.0,4164 -0.0,0.0,0.0,0.0,4180 -0.0,0.0,0.0,0.0,4200 -0.0,0.0,0.0,0.0,4220 -0.0,0.0,0.0,0.0,4240 -0.0,0.0,0.0,0.0,4261 -0.0,0.0,0.0,0.0,4280 -0.0,0.0,0.0,0.0,4300 -0.0,0.0,0.0,0.0,4320 -0.0,0.0,0.0,0.0,4340 -0.0,0.0,0.0,0.0,4438 -0.0,0.0,0.0,0.0,4449 -0.0,0.0,0.0,0.0,4460 -0.0,0.0,0.0,0.0,4471 -0.0,0.0,0.0,0.0,4484 -0.0,0.0,0.0,0.0,4496 -0.0,0.0,0.0,0.0,4508 -0.0,0.0,0.0,0.0,4520 -0.0,0.0,0.0,0.0,4532 -0.0,0.0,0.0,0.0,4543 -0.0,0.0,0.0,0.0,4560 -0.0,0.0,0.0,0.0,4580 -0.0,0.0,0.0,0.0,4624 -0.0,0.0,0.0,0.0,4638 -0.0,0.0,0.0,0.0,4651 -0.0,0.0,0.0,0.0,4665 -0.0,0.0,0.0,0.0,4679 -0.0,0.0,0.0,0.0,4700 -0.0,0.0,0.0,0.0,4720 -0.0,0.0,0.0,0.0,4740 -0.0,0.0,0.0,0.0,4760 -0.0,0.0,0.0,0.0,4780 -0.0,0.0,0.0,0.0,4800 -0.0,0.0,0.0,0.0,4820 -0.0,0.0,0.0,0.0,4859 -0.0,0.0,0.0,0.0,4871 -0.0,0.0,0.0,0.0,4893 -0.0,0.0,0.0,0.0,4914 -0.0,0.0,0.0,0.0,4927 -0.0,0.0,0.0,0.0,4940 -0.0,0.0,0.0,0.0,4960 -0.0,0.0,0.0,0.0,4980 -0.11023622006177902,0.0,0.0,0.0,5000 -0.13385826349258423,0.0,0.0,0.0,5020 -0.17322835326194763,0.0,0.0,0.0,5040 -0.21259842813014984,0.0,0.0,0.0,5060 -0.29921260476112366,0.007874015718698502,0.0,0.0,5104 -0.4015747904777527,0.04724409431219101,0.0,0.0,5123 -0.4015747904777527,0.04724409431219101,0.0,0.0,5133 -0.4251968562602997,0.05511811003088951,0.0,0.0,5143 -0.4251968562602997,0.05511811003088951,0.0,0.0,5160 -0.4251968562602997,0.05511811003088951,0.0,0.0,5180 -0.4330708682537079,0.05511811003088951,0.0,0.0,5200 -0.4330708682537079,0.04724409431219101,0.0,0.0,5220 -0.4330708682537079,0.04724409431219101,0.0,0.0,5240 -0.4330708682537079,0.04724409431219101,0.0,0.0,5260 -0.4330708682537079,0.04724409431219101,0.0,0.0,5280 -0.4330708682537079,0.04724409431219101,0.0,0.0,5300 -0.4330708682537079,0.04724409431219101,0.0,0.0,5343 -0.4330708682537079,0.04724409431219101,0.0,0.0,5353 -0.4330708682537079,0.04724409431219101,0.0,0.0,5366 -0.4330708682537079,0.04724409431219101,0.0,0.0,5380 -0.4645669162273407,0.04724409431219101,0.0,0.0,5400 -0.4724409580230713,0.04724409431219101,0.0,0.0,5420 -0.4724409580230713,0.04724409431219101,0.0,0.0,5440 -0.4724409580230713,0.04724409431219101,0.0,0.0,5460 -0.4724409580230713,0.04724409431219101,0.0,0.0,5480 -0.4724409580230713,0.04724409431219101,0.0,0.0,5500 -0.4724409580230713,0.04724409431219101,0.0,0.0,5520 -0.4724409580230713,0.04724409431219101,0.0,0.0,5540 -0.4724409580230713,0.04724409431219101,0.0,0.0,5628 -0.4724409580230713,0.04724409431219101,0.0,0.0,5641 -0.4724409580230713,0.04724409431219101,0.0,0.0,5652 -0.4724409580230713,0.04724409431219101,0.0,0.0,5663 -0.4724409580230713,0.04724409431219101,0.0,0.0,5676 -0.4724409580230713,0.04724409431219101,0.0,0.0,5688 -0.4724409580230713,0.04724409431219101,0.0,0.0,5701 -0.4724409580230713,0.04724409431219101,0.0,0.0,5710 -0.4724409580230713,0.04724409431219101,0.0,0.0,5724 -0.4724409580230713,0.04724409431219101,0.0,0.0,5740 -0.4724409580230713,0.04724409431219101,0.0,0.0,5760 -0.4724409580230713,0.04724409431219101,0.0,0.0,5780 -0.4724409580230713,0.04724409431219101,0.0,0.0,5821 -0.4724409580230713,0.04724409431219101,0.0,0.0,5832 -0.4724409580230713,0.04724409431219101,0.0,0.0,5845 -0.4724409580230713,0.04724409431219101,0.0,0.0,5860 -0.4724409580230713,0.04724409431219101,0.0,0.0,5880 -0.4724409580230713,0.04724409431219101,0.0,0.0,5900 -0.4724409580230713,0.04724409431219101,0.0,0.0,5920 -0.4724409580230713,0.04724409431219101,0.0,0.0,5940 -0.4724409580230713,0.04724409431219101,0.0,0.0,5960 -0.4724409580230713,0.04724409431219101,0.0,0.0,5980 -0.4724409580230713,0.03937007859349251,0.0,0.0,6000 -0.4724409580230713,0.03937007859349251,0.0,0.0,6020 -0.4724409580230713,0.031496062874794006,0.0,0.0,6062 -0.4724409580230713,0.031496062874794006,0.0,0.0,6073 -0.4724409580230713,0.031496062874794006,0.0,0.0,6086 -0.4724409580230713,0.031496062874794006,0.0,0.0,6100 -0.4724409580230713,0.031496062874794006,0.0,0.0,6120 -0.4724409580230713,0.031496062874794006,0.0,0.0,6140 -0.4724409580230713,0.031496062874794006,0.0,0.0,6160 -0.4724409580230713,0.031496062874794006,0.0,0.0,6180 -0.4724409580230713,0.031496062874794006,0.0,0.0,6200 -0.4724409580230713,0.031496062874794006,0.0,0.0,6220 -0.4724409580230713,0.031496062874794006,0.0,0.0,6240 -0.4724409580230713,0.031496062874794006,0.0,0.0,6260 -0.4724409580230713,0.031496062874794006,0.0,0.0,6305 -0.4724409580230713,0.031496062874794006,0.0,0.0,6318 -0.4724409580230713,0.031496062874794006,0.0,0.0,6330 -0.4724409580230713,0.031496062874794006,0.0,0.0,6349 -0.4724409580230713,0.031496062874794006,0.0,0.0,6369 -0.4724409580230713,0.031496062874794006,0.0,0.0,6382 -0.4724409580230713,0.031496062874794006,0.0,0.0,6404 -0.4724409580230713,0.031496062874794006,0.0,0.0,6420 -0.4724409580230713,0.031496062874794006,0.0,0.0,6440 -0.4724409580230713,0.031496062874794006,0.0,0.0,6460 -0.4724409580230713,0.031496062874794006,0.0,0.0,6480 -0.4724409580230713,0.031496062874794006,0.0,0.0,6500 -0.4724409580230713,0.031496062874794006,0.0,0.0,6543 -0.4724409580230713,0.031496062874794006,0.0,0.0,6555 -0.4724409580230713,0.031496062874794006,0.0,0.0,6568 -0.4724409580230713,0.031496062874794006,0.0,0.0,6583 -0.31496062874794006,0.0,0.0,0.0,6600 -0.0,0.0,0.0,0.0,6620 -0.0,0.0,0.0,0.0,6640 -0.0,0.0,0.0,0.0,6660 -0.0,0.0,0.0,0.0,6680 -0.0,0.0,0.0,0.0,6700 -0.0,0.0,0.0,0.0,6720 -0.0,0.0,0.0,0.0,6740 -0.0,0.0,0.0,0.0,6839 -0.0,0.0,0.0,0.0,6849 -0.0,0.0,0.0,0.0,6862 -0.0,0.0,0.0,0.0,6873 -0.0,0.0,0.0,0.0,6888 -0.0,0.0,0.0,0.0,6901 -0.0,0.0,0.0,0.0,6911 -0.0,0.0,0.0,0.0,6922 -0.0,0.0,0.0,0.0,6934 -0.0,0.0,0.0,0.0,6945 -0.0,0.0,0.0,0.0,6960 -0.0,0.0,0.0,0.0,6980 -0.0,0.0,0.0,0.0,7022 -0.0,0.0,0.0,0.0,7034 -0.0,0.0,0.0,0.0,7043 -0.0,0.0,0.0,0.0,7060 -0.0,0.0,0.0,0.0,7080 \ No newline at end of file +0.0,0.0,0.0,0.0,211 +0.0,0.0,0.0,0.0,223 +0.0,0.0,0.0,0.0,235 +0.0,0.0,0.0,0.0,247 +0.0,0.0,0.0,0.0,263 +0.0,0.0,0.0,0.0,283 +0.0,0.0,0.0,0.0,303 +0.0,-0.109375,0.0,0.0,323 +0.0,-0.1484375,0.0,0.0,343 +0.0,-0.2109375,0.0,0.0,363 +0.0,-0.3671875,0.0,0.0,398 +0.0,-0.4140625,0.0,0.0,411 +0.0,-0.4765625,0.0,0.0,425 +0.0,-0.5078125,0.0,0.0,443 +0.0,-0.5078125,0.0,0.0,463 +0.0,-0.53125,0.0,0.0,483 +0.0,-0.5546875,0.0,0.0,504 +0.0,-0.5625,0.0,0.0,523 +0.0,-0.5625,0.0,0.0,544 +0.0,-0.5703125,0.0,0.0,563 +0.0,-0.5859375,0.0,0.0,584 +0.0,-0.5859375,0.0,0.0,603 +0.0,-0.5859375,0.0,0.0,640 +0.0,-0.59375,0.0,0.0,657 +0.0,-0.6015625,0.0,0.0,672 +0.0,-0.6015625,0.0,0.0,685 +0.0,-0.6015625,0.0,0.0,703 +0.0,-0.6015625,0.0,0.0,723 +0.0,-0.6015625,0.0,0.0,743 +0.0,-0.6015625,0.0,0.0,763 +0.0,-0.6015625,0.0,0.0,783 +0.0,-0.6015625,0.0,0.0,803 +0.0,-0.6015625,0.0,0.0,823 +0.0,-0.6015625,0.0,0.0,844 +0.0,-0.6015625,0.0,0.0,878 +0.0,-0.6015625,0.0,0.0,893 +0.0,-0.6015625,0.0,0.0,907 +0.0,-0.6015625,0.0,0.0,924 +0.0,-0.609375,0.0,0.0,943 +0.0,-0.609375,0.0,0.0,963 +0.0,-0.609375,0.0,0.0,983 +0.0,-0.609375,0.0,0.0,1004 +0.0,-0.609375,0.0,0.0,1023 +0.0,-0.609375,0.0,0.0,1043 +0.0,-0.609375,0.0,0.0,1064 +0.0,-0.609375,0.0,0.0,1083 +0.0,-0.609375,0.0,0.0,1156 +0.0,-0.609375,0.0,0.0,1172 +0.0,-0.609375,0.0,0.0,1185 +0.0,-0.609375,0.0,0.0,1200 +0.0,-0.609375,0.0,0.0,1215 +0.0,-0.609375,0.0,0.0,1225 +0.0,-0.609375,0.0,0.0,1236 +0.0,-0.609375,0.0,0.0,1249 +0.0,-0.609375,0.0,0.0,1263 +0.0,-0.609375,0.0,0.0,1283 +0.0,-0.609375,0.0,0.0,1303 +0.0,-0.609375,0.0,0.0,1323 +0.0,-0.609375,0.0,0.0,1363 +0.0,-0.6015625,0.0,0.0,1376 +0.0,-0.6015625,0.0,0.0,1394 +0.0,-0.6015625,0.0,0.0,1405 +0.0,-0.6015625,0.0,0.0,1423 +0.0,-0.6015625,0.0,0.0,1443 +0.0,-0.6015625,0.0,0.0,1463 +0.0,-0.6015625,0.0,0.0,1483 +0.0,-0.6015625,0.0,0.0,1503 +0.0,-0.6015625,0.0,0.0,1523 +0.0,-0.6015625,0.0,0.0,1543 +0.0,-0.6015625,0.0,0.0,1563 +0.0,-0.6015625,0.0,0.0,1597 +0.0,-0.6015625,0.0,0.0,1608 +0.0,-0.6015625,0.0,0.0,1624 +0.0,-0.6015625,0.0,0.0,1643 +0.0,-0.6015625,0.0,0.0,1664 +0.0,-0.5859375,0.0,0.0,1683 +0.0,-0.5859375,0.0,0.0,1703 +0.0,-0.5625,0.0,0.0,1723 +0.0,-0.5625,0.0,0.0,1743 +0.0,-0.5625,0.0,0.0,1763 +0.0,-0.5625,0.0,0.0,1783 +0.0,-0.5625,0.0,0.0,1803 +0.0,-0.5625,0.0,0.0,1843 +0.0,-0.5625,0.0,0.0,1855 +0.0,-0.5625,0.0,0.0,1868 +0.0,-0.5625,0.0,0.0,1883 +0.0,-0.5625,0.0,0.0,1903 +0.0,-0.5625,0.0,0.0,1923 +0.0,-0.5625,0.0,0.0,1943 +0.0,-0.5625,0.0,0.0,1963 +0.0,-0.5625,0.0,0.0,1983 +0.0,-0.5625,0.0,0.0,2003 +0.0,-0.5625,0.0,0.0,2024 +0.0,-0.5625,0.0,0.0,2043 +0.0,-0.5625,0.0,0.0,2081 +0.0,-0.5625,0.0,0.0,2093 +0.0,-0.5625,0.0,0.0,2105 +0.0,-0.5625,0.0,0.0,2123 +0.0,-0.5625,0.0,0.0,2143 +0.0,-0.5625,0.0,0.0,2163 +0.0,-0.5625,0.0,0.0,2183 +0.0,-0.5625,0.0,0.0,2203 +0.0,-0.5625,0.0,0.0,2223 +0.0,-0.5625,0.0,0.0,2243 +0.0,-0.5625,0.0,0.0,2263 +0.0,-0.5625,0.0,0.0,2283 +0.0,-0.5625,0.0,0.0,2366 +0.0,-0.5625,0.0,0.0,2377 +0.0,-0.5625,0.0,0.0,2394 +0.0,-0.5703125,0.0,0.0,2405 +0.0,-0.5703125,0.0,0.0,2418 +0.0,-0.5703125,0.0,0.0,2431 +0.0,-0.5703125,0.0,0.0,2444 +0.0,-0.5703125,0.0,0.0,2458 +0.0,-0.5703125,0.0,0.0,2470 +0.0,-0.5703125,0.0,0.0,2485 +0.0,-0.5703125,0.0,0.0,2503 +0.0,-0.5703125,0.0,0.0,2523 +0.0,-0.5703125,0.0,0.0,2563 +0.0,-0.5703125,0.0,0.0,2577 +0.0,-0.5703125,0.0,0.0,2591 +0.0,-0.5703125,0.0,0.0,2608 +0.0,-0.5703125,0.0,0.0,2624 +0.0,-0.5703125,0.0,0.0,2643 +0.0,-0.5703125,0.0,0.0,2677 +0.0,-0.5703125,0.0,0.0,2698 +0.0,-0.5703125,0.0,0.0,2711 +0.0,-0.5703125,0.0,0.0,2725 +0.0,-0.5703125,0.0,0.0,2743 +0.0,-0.5703125,0.0,0.0,2764 +0.0,-0.5703125,0.0,0.0,2810 +0.0,-0.5703125,0.0,0.0,2820 +0.0,-0.5703125,0.0,0.0,2833 +0.0,-0.5703125,0.0,0.0,2845 +0.0,-0.5703125,0.0,0.0,2863 +0.0,-0.5703125,0.0,0.0,2883 +0.0,-0.5703125,0.0,0.0,2904 +0.0,-0.5703125,0.0,0.0,2924 +0.0,-0.5703125,0.0,0.0,2943 +0.0,-0.5703125,0.0,0.0,2963 +0.0,-0.5703125,0.0,0.0,2983 +0.0,-0.5703125,0.0,0.0,3003 +0.0,-0.5703125,0.0,0.0,3033 +0.0,-0.5703125,0.0,0.0,3050 +0.0,-0.5703125,0.0,0.0,3065 +0.0,-0.5703125,0.0,0.0,3083 +0.0,-0.5703125,0.0,0.0,3103 +0.0,-0.5703125,0.0,0.0,3123 +0.0,-0.5703125,0.0,0.0,3144 +0.0,-0.5703125,0.0,0.0,3164 +0.0,-0.5703125,0.0,0.0,3184 +0.0,-0.5703125,0.0,0.0,3203 +0.0,-0.5703125,0.0,0.0,3223 +0.0,-0.5703125,0.0,0.0,3243 +0.0,-0.5703125,0.0,0.0,3272 +0.0,-0.5703125,0.0,0.0,3289 +0.0,-0.5703125,0.0,0.0,3303 +0.0,-0.5703125,0.0,0.0,3323 +0.0,-0.5703125,0.0,0.0,3343 +0.0,-0.5703125,0.0,0.0,3363 +0.0,-0.5703125,0.0,0.0,3383 +0.0,-0.5703125,0.0,0.0,3403 +0.0,-0.5703125,0.0,0.0,3423 +0.0,-0.5703125,0.0,0.0,3443 +0.0,-0.5703125,0.0,0.0,3463 +0.0,-0.5703125,0.0,0.0,3483 +0.0,-0.5703125,0.0,0.0,3566 +0.0,-0.5703125,0.0,0.0,3578 +0.0,-0.5703125,0.0,0.0,3596 +0.0,-0.5703125,0.0,0.0,3610 +0.0,-0.5703125,0.0,0.0,3623 +0.0,-0.5703125,0.0,0.0,3640 +0.0,-0.5703125,0.0,0.0,3651 +0.0,-0.5703125,0.0,0.0,3663 +0.0,-0.5703125,0.0,0.0,3678 +0.0,-0.5703125,0.0,0.0,3691 +0.0,-0.5703125,0.0,0.0,3706 +0.0,-0.5703125,0.0,0.0,3723 +0.0,-0.5703125,0.0,0.0,3766 +0.0,-0.5703125,0.0,0.0,3778 +0.0,-0.5703125,0.0,0.0,3792 +0.0,-0.5703125,0.0,0.0,3807 +0.0,-0.5703125,0.0,0.0,3823 +0.0,-0.5703125,0.0,0.0,3843 +0.0,-0.53125,0.0,0.0,3863 +0.0,-0.53125,0.0,0.0,3884 +0.0,-0.421875,0.0,0.0,3904 +0.0,0.0,0.0,0.0,3924 +0.0,0.0,0.0,0.0,3944 +0.0,0.0,0.0,0.0,3963 +0.0,0.0,0.0,0.0,3999 +0.0,0.0,0.0,0.0,4010 +0.0,0.0,0.0,0.0,4025 +0.0,0.0,0.0,0.0,4043 +0.0,0.0,0.0,0.0,4063 +0.0,0.0,0.0,0.0,4083 +0.0,0.0,0.0,0.0,4103 +0.0,0.0,0.0,0.0,4123 +0.0,0.0,0.0,0.0,4143 +0.0,0.0,0.0,0.0,4163 +0.0,0.0,0.0,0.0,4183 +0.0,0.0,0.0,0.0,4203 +0.0,0.0,0.0,0.0,4236 +0.0,0.0,0.0,0.0,4247 +0.0,0.0,0.0,0.0,4264 +0.0,0.0,0.0,0.0,4284 +0.0,0.0,0.0,0.0,4304 +0.0,0.0,0.0,0.0,4324 +0.0,0.0,0.0,0.0,4343 +0.0,0.0,0.0,0.0,4363 \ No newline at end of file From d09c7e464bba131c6797261f5129462e549ce8ae Mon Sep 17 00:00:00 2001 From: Aarav Date: Sat, 25 Feb 2023 16:16:54 -0700 Subject: [PATCH 32/40] fixy fixy --- src/main/java/frc4388/robot/RobotContainer.java | 10 ++++++---- .../java/frc4388/robot/commands/PlaybackChooser.java | 9 +++++---- 2 files changed, 11 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index d4411b3..e274800 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -79,6 +79,7 @@ public class RobotContainer { private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"); + PlaybackChooser playbackChooser; /** * The container for the robot. Contains subsystems, OI devices, and commands. */ @@ -104,8 +105,9 @@ public class RobotContainer { chooser.addOption("Taxi", taxi); - PlaybackChooser playbackChooser = new PlaybackChooser(m_robotSwerveDrive, - "Balance", new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); + playbackChooser = new PlaybackChooser(m_robotSwerveDrive, + "Balance", new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive), + "Balance2", new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) .onTrue(new InstantCommand(() -> playbackChooser.appendCommand())); @@ -154,8 +156,8 @@ public class RobotContainer { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - - return chooser.getSelected(); + // return chooser.getSelected(); + return playbackChooser.getCommand(); } public DeadbandedXboxController getDeadbandedDriverController() { diff --git a/src/main/java/frc4388/robot/commands/PlaybackChooser.java b/src/main/java/frc4388/robot/commands/PlaybackChooser.java index 2934e40..59fb2b1 100644 --- a/src/main/java/frc4388/robot/commands/PlaybackChooser.java +++ b/src/main/java/frc4388/robot/commands/PlaybackChooser.java @@ -66,12 +66,13 @@ public class PlaybackChooser { } public Command getCommand() { - Command command = m_playback.getSelected(); + Command command = m_playback.getSelected().asProxy(); - for (int i = 1; i < m_choosers.size(); i++) { - command.andThen(m_choosers.get(i).getSelected()); + Command[] commands = new Command[m_choosers.size() - 1]; + for (int i = 0; i < m_choosers.size()-1; i++) { + commands[i] = m_choosers.get(i + 1).getSelected().asProxy(); } - return command; + return command.andThen(commands); } } From 2f35e591e88b37affe55ddc0007adc04563a246d Mon Sep 17 00:00:00 2001 From: 66945 <54561572+66945@users.noreply.github.com> Date: Sat, 25 Feb 2023 20:33:24 -0700 Subject: [PATCH 33/40] fix auto chooser --- .../java/frc4388/robot/RobotContainer.java | 3 --- .../robot/commands/PlaybackChooser.java | 18 +++++++++++------- 2 files changed, 11 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index e274800..7ae2f55 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -111,9 +111,6 @@ public class RobotContainer { new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) .onTrue(new InstantCommand(() -> playbackChooser.appendCommand())); - - new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) - .onTrue(new InstantCommand(() -> playbackChooser.appendPlayback())); } diff --git a/src/main/java/frc4388/robot/commands/PlaybackChooser.java b/src/main/java/frc4388/robot/commands/PlaybackChooser.java index 59fb2b1..8a9e197 100644 --- a/src/main/java/frc4388/robot/commands/PlaybackChooser.java +++ b/src/main/java/frc4388/robot/commands/PlaybackChooser.java @@ -15,8 +15,7 @@ public class PlaybackChooser { private SendableChooser m_playback = new SendableChooser<>(); private HashMap m_commandPool = new HashMap<>(); - private File m_dir = new File("/home/lvuser/autos/"); - + private File m_dir = new File("/home/lvuser/autos/"); private SwerveDrive m_swerve; // commands @@ -26,16 +25,17 @@ public class PlaybackChooser { m_swerve = swerve; for (int i = 0; i < pool.length; i += 2) { - if (!(pool[i] instanceof String)) throw new RuntimeException("Need (string, command)"); - if (!(pool[i + 1] instanceof Command)) throw new RuntimeException("Need (string, command)"); + if (!(pool[i] instanceof String) || !(pool[i + 1] instanceof Command)) { + throw new RuntimeException("Need (string, command)"); + } m_commandPool.put((String) pool[i], (Command) pool[i + 1]); } - m_playback.addOption("No Auto", m_noAuto); for (String auto : m_dir.list()) { m_playback.addOption(auto, new JoystickPlayback(m_swerve, auto)); } + m_playback.addOption("No Auto", m_noAuto); m_choosers.add(m_playback); SmartDashboard.putData("Command: 0", m_playback); @@ -66,11 +66,15 @@ public class PlaybackChooser { } public Command getCommand() { - Command command = m_playback.getSelected().asProxy(); + Command command = m_playback.getSelected(); + command = command == null ? m_noAuto : command.asProxy(); Command[] commands = new Command[m_choosers.size() - 1]; for (int i = 0; i < m_choosers.size()-1; i++) { - commands[i] = m_choosers.get(i + 1).getSelected().asProxy(); + Command command2 = m_choosers.get(i + 1).getSelected(); + command2 = command2 == null ? m_noAuto : command2.asProxy(); + + commands[i] = command2.asProxy(); } return command.andThen(commands); From 220d511958b214e569807edccb629ef85e821fc4 Mon Sep 17 00:00:00 2001 From: 66945 <54561572+66945@users.noreply.github.com> Date: Sat, 25 Feb 2023 20:39:33 -0700 Subject: [PATCH 34/40] unified choosers --- .../robot/commands/PlaybackChooser.java | 36 +++++++------------ 1 file changed, 12 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/PlaybackChooser.java b/src/main/java/frc4388/robot/commands/PlaybackChooser.java index 8a9e197..87b2c75 100644 --- a/src/main/java/frc4388/robot/commands/PlaybackChooser.java +++ b/src/main/java/frc4388/robot/commands/PlaybackChooser.java @@ -11,12 +11,12 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import frc4388.robot.subsystems.SwerveDrive; public class PlaybackChooser { - private ArrayList> m_choosers = new ArrayList<>(); - private SendableChooser m_playback = new SendableChooser<>(); - private HashMap m_commandPool = new HashMap<>(); + private final ArrayList> m_choosers = new ArrayList<>(); + private final SendableChooser m_playback; + private final HashMap m_commandPool = new HashMap<>(); - private File m_dir = new File("/home/lvuser/autos/"); - private SwerveDrive m_swerve; + private final File m_dir = new File("/home/lvuser/autos/"); + private final SwerveDrive m_swerve; // commands private Command m_noAuto = new InstantCommand(); @@ -32,12 +32,8 @@ public class PlaybackChooser { m_commandPool.put((String) pool[i], (Command) pool[i + 1]); } - for (String auto : m_dir.list()) { - m_playback.addOption(auto, new JoystickPlayback(m_swerve, auto)); - } - m_playback.addOption("No Auto", m_noAuto); - - m_choosers.add(m_playback); + appendCommand(); + m_playback = m_choosers.get(0); SmartDashboard.putData("Command: 0", m_playback); } @@ -45,21 +41,13 @@ public class PlaybackChooser { public void appendCommand() { var chooser = new SendableChooser(); - for (var cmd_name : m_commandPool.keySet()) { - chooser.addOption(cmd_name, m_commandPool.get(cmd_name)); - } - - m_choosers.add(chooser); - SmartDashboard.putData("Command: " + m_choosers.size(), chooser); - } - - // This will be bound to a button for the time being - public void appendPlayback() { - var chooser = new SendableChooser(); - for (String auto : m_dir.list()) { m_playback.addOption(auto, new JoystickPlayback(m_swerve, auto)); } + for (var cmd_name : m_commandPool.keySet()) { + chooser.addOption(cmd_name, m_commandPool.get(cmd_name)); + } + chooser.addOption("No Auto", m_noAuto); m_choosers.add(chooser); SmartDashboard.putData("Command: " + m_choosers.size(), chooser); @@ -73,7 +61,7 @@ public class PlaybackChooser { for (int i = 0; i < m_choosers.size()-1; i++) { Command command2 = m_choosers.get(i + 1).getSelected(); command2 = command2 == null ? m_noAuto : command2.asProxy(); - + commands[i] = command2.asProxy(); } From 7a69a3dd00ccaca5d92f85ee68c9e0d4b3cbe717 Mon Sep 17 00:00:00 2001 From: 66945 <54561572+66945@users.noreply.github.com> Date: Sat, 25 Feb 2023 20:50:51 -0700 Subject: [PATCH 35/40] added buttons and stuff --- src/main/java/frc4388/robot/RobotContainer.java | 3 +-- .../frc4388/robot/commands/PlaybackChooser.java | 14 ++++++++++++-- 2 files changed, 13 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 7ae2f55..e619546 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -106,8 +106,7 @@ public class RobotContainer { chooser.addOption("Taxi", taxi); playbackChooser = new PlaybackChooser(m_robotSwerveDrive, - "Balance", new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive), - "Balance2", new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); + "Balance", new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) .onTrue(new InstantCommand(() -> playbackChooser.appendCommand())); diff --git a/src/main/java/frc4388/robot/commands/PlaybackChooser.java b/src/main/java/frc4388/robot/commands/PlaybackChooser.java index 87b2c75..b039067 100644 --- a/src/main/java/frc4388/robot/commands/PlaybackChooser.java +++ b/src/main/java/frc4388/robot/commands/PlaybackChooser.java @@ -4,6 +4,9 @@ import java.io.File; import java.util.ArrayList; import java.util.HashMap; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.WidgetType; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -34,7 +37,10 @@ public class PlaybackChooser { appendCommand(); m_playback = m_choosers.get(0); - SmartDashboard.putData("Command: 0", m_playback); + + Shuffleboard.getTab("Auto Chooser") + .add("Add Sequence", new InstantCommand(() -> appendCommand())) + .withPosition(4, 0); } // This will be bound to a button for the time being @@ -50,7 +56,11 @@ public class PlaybackChooser { chooser.addOption("No Auto", m_noAuto); m_choosers.add(chooser); - SmartDashboard.putData("Command: " + m_choosers.size(), chooser); + Shuffleboard.getTab("Auto Chooser") + .add("Command: " + m_choosers.size(), chooser) + .withSize(4, 1) + .withPosition(0, 0) + .withWidget(BuiltInWidgets.kSplitButtonChooser); } public Command getCommand() { From 0081419c19370e4b4e9f253eba6a311e3d1c2ff0 Mon Sep 17 00:00:00 2001 From: 66945 <54561572+66945@users.noreply.github.com> Date: Sat, 25 Feb 2023 20:59:16 -0700 Subject: [PATCH 36/40] Removed giant embedded file from robot --- src/main/java/frc4388/robot/Robot.java | 771 ------------------------- 1 file changed, 771 deletions(-) diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 7995804..04dbb0b 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -103,777 +103,6 @@ public class Robot extends TimedRobot { public void teleopInit() { m_robotContainer.m_robotSwerveDrive.resetGyro(); -// File auto = new File("/home/lvuser/BlueNearDriveToChargeStation.txt"); - -// try (PrintWriter writer = new PrintWriter(auto)) { -// writer.println( -// "0.0,0.0,0.0,0.0,0\n" + -// "0.0,0.0,0.0,0.0,0\n" + -// "0.0,0.0,0.0,0.0,20\n" + -// "0.0,0.0,0.0,0.0,40\n" + -// "0.0,0.0,0.0,0.0,60\n" + -// "0.0,0.0,0.0,0.0,79\n" + -// "0.0,0.0,0.0,0.0,99\n" + -// "0.0,0.0,0.0,0.0,119\n" + -// "0.0,0.0,0.0,0.0,139\n" + -// "0.0,0.0,0.0,0.0,160\n" + -// "0.0,0.0,0.0,0.0,180\n" + -// "0.0,0.0,0.0,0.0,200\n" + -// "0.0,0.0,0.0,0.0,220\n" + -// "0.0,0.0,0.0,0.0,239\n" + -// "0.0,0.0,0.0,0.0,260\n" + -// "0.0,0.0,0.0,0.0,280\n" + -// "0.0,0.0,0.0,0.0,300\n" + -// "0.0,0.0,0.0,0.0,319\n" + -// "0.0,0.0,0.0,0.0,340\n" + -// "0.0,0.0,0.0,0.0,360\n" + -// "0.0,0.0,0.0,0.0,379\n" + -// "0.0,0.0,0.0,0.0,399\n" + -// "0.0,0.0,0.0,0.0,419\n" + -// "0.0,0.0,0.0,0.0,440\n" + -// "0.0,0.0,0.0,0.0,459\n" + -// "0.0,-0.109375,0.0,0.0,479\n" + -// "0.0,-0.109375,0.0,0.0,499\n" + -// "0.0,-0.109375,0.0,0.0,520\n" + -// "0.0,-0.109375,0.0,0.0,540\n" + -// "0.0,-0.1171875,0.0,0.0,559\n" + -// "0.0,-0.1171875,0.0,0.0,580\n" + -// "0.0,-0.1171875,0.0,0.0,599\n" + -// "0.0,-0.1171875,0.0,0.0,621\n" + -// "0.0,-0.1171875,0.0,0.0,639\n" + -// "0.0,-0.1171875,0.0,0.0,661\n" + -// "0.0,-0.1171875,0.0,0.0,680\n" + -// "0.0,-0.1171875,0.0,0.0,699\n" + -// "0.0,-0.1171875,0.0,0.0,721\n" + -// "0.0,-0.1171875,0.0,0.0,769\n" + -// "0.0,-0.125,0.0,0.0,778\n" + -// "0.0,-0.1328125,0.0,0.0,790\n" + -// "0.0,-0.1328125,0.0,0.0,802\n" + -// "0.0,-0.1328125,0.0,0.0,820\n" + -// "0.0,-0.1328125,0.0,0.0,840\n" + -// "0.0,-0.1328125,0.0,0.0,860\n" + -// "0.0,-0.1328125,0.0,0.0,879\n" + -// "0.0,-0.140625,0.0,0.0,899\n" + -// "0.0,-0.171875,0.0,0.0,933\n" + -// "0.0,-0.1875,0.0,0.0,944\n" + -// "0.0,-0.1875,0.0,0.0,959\n" + -// "0.0,-0.203125,0.0,0.0,979\n" + -// "0.0,-0.203125,0.0,0.0,999\n" + -// "0.0,-0.203125,0.0,0.0,1019\n" + -// "0.0,-0.203125,0.0,0.0,1039\n" + -// "0.0,-0.203125,0.0,0.0,1059\n" + -// "0.0,-0.203125,0.0,0.0,1079\n" + -// "0.0,-0.2109375,0.0,0.0,1099\n" + -// "0.0,-0.2109375,0.0,0.0,1120\n" + -// "0.0,-0.2109375,0.0,0.0,1140\n" + -// "0.0,-0.21875,0.0,0.0,1159\n" + -// "0.0,-0.2109375,0.0,0.0,1180\n" + -// "0.0,-0.2265625,0.0,0.0,1199\n" + -// "0.0,-0.234375,0.0,0.0,1219\n" + -// "0.0,-0.234375,0.0,0.0,1239\n" + -// "0.0,-0.234375,0.0,0.0,1259\n" + -// "0.0,-0.2421875,0.0,0.0,1280\n" + -// "0.0,-0.2578125,0.0,0.0,1300\n" + -// "0.0,-0.296875,0.0,0.0,1320\n" + -// "0.0,-0.296875,0.0,0.0,1340\n" + -// "0.0,-0.3046875,0.0,0.0,1359\n" + -// "0.0,-0.3125,0.0,0.0,1379\n" + -// "0.0,-0.3125,0.0,0.0,1399\n" + -// "0.0,-0.3125,0.0,0.0,1419\n" + -// "0.0,-0.3203125,0.0,0.0,1442\n" + -// "0.0,-0.3203125,0.0,0.0,1460\n" + -// "0.0,-0.3203125,0.0,0.0,1479\n" + -// "0.0,-0.3203125,0.0,0.0,1499\n" + -// "0.0,-0.3203125,0.0,0.0,1520\n" + -// "0.0,-0.3203125,0.0,0.0,1540\n" + -// "0.0,-0.3203125,0.0,0.0,1560\n" + -// "0.0,-0.3203125,0.0,0.0,1579\n" + -// "0.0,-0.3203125,0.0,0.0,1599\n" + -// "0.0,-0.328125,0.0,0.0,1619\n" + -// "0.0,-0.34375,0.0,0.0,1640\n" + -// "0.0,-0.3515625,0.0,0.0,1659\n" + -// "0.0,-0.359375,0.0,0.0,1680\n" + -// "0.0,-0.359375,0.0,0.0,1699\n" + -// "0.0,-0.359375,0.0,0.0,1720\n" + -// "0.0,-0.359375,0.0,0.0,1739\n" + -// "0.0,-0.3671875,0.0,0.0,1759\n" + -// "0.0,-0.375,0.0,0.0,1780\n" + -// "0.0,-0.390625,0.0,0.0,1799\n" + -// "0.0,-0.4140625,0.0,0.0,1820\n" + -// "0.0,-0.421875,0.0,0.0,1839\n" + -// "0.0,-0.4296875,0.0,0.0,1859\n" + -// "0.0,-0.4296875,0.0,0.0,1879\n" + -// "0.0,-0.4296875,0.0,0.0,1900\n" + -// "0.0,-0.4296875,0.0,0.0,1919\n" + -// "0.0,-0.4296875,0.0,0.0,1939\n" + -// "0.0,-0.4296875,0.0,0.0,1959\n" + -// "0.0,-0.4296875,0.0,0.0,1979\n" + -// "0.0,-0.4296875,0.0,0.0,2000\n" + -// "0.0,-0.4296875,0.0,0.0,2019\n" + -// "0.0,-0.4296875,0.0,0.0,2039\n" + -// "0.0,-0.4296875,0.0,0.0,2060\n" + -// "0.0,-0.4296875,0.0,0.0,2080\n" + -// "0.0,-0.4296875,0.0,0.0,2099\n" + -// "0.0,-0.4296875,0.0,0.0,2119\n" + -// "0.0,-0.4296875,0.0,0.0,2139\n" + -// "0.0,-0.4296875,0.0,0.0,2160\n" + -// "0.0,-0.4375,0.0,0.0,2179\n" + -// "0.0,-0.4453125,0.0,0.0,2199\n" + -// "0.0,-0.453125,0.0,0.0,2220\n" + -// "0.0,-0.453125,0.0,0.0,2240\n" + -// "0.0,-0.453125,0.0,0.0,2260\n" + -// "0.0,-0.453125,0.0,0.0,2280\n" + -// "0.0,-0.453125,0.0,0.0,2300\n" + -// "0.0,-0.453125,0.0,0.0,2320\n" + -// "0.0,-0.453125,0.0,0.0,2340\n" + -// "0.0,-0.453125,0.0,0.0,2359\n" + -// "0.0,-0.453125,0.0,0.0,2379\n" + -// "0.0,-0.453125,0.0,0.0,2399\n" + -// "0.0,-0.453125,0.0,0.0,2419\n" + -// "0.0,-0.453125,0.0,0.0,2440\n" + -// "0.0,-0.453125,0.0,0.0,2460\n" + -// "0.0,-0.453125,0.0,0.0,2479\n" + -// "0.0,-0.453125,0.0,0.0,2500\n" + -// "0.0,-0.453125,0.0,0.0,2520\n" + -// "0.0,-0.453125,0.0,0.0,2540\n" + -// "0.0,-0.453125,0.0,0.0,2559\n" + -// "0.0,-0.453125,0.0,0.0,2579\n" + -// "0.0,-0.4609375,0.0,0.0,2600\n" + -// "0.0,-0.4609375,0.0,0.0,2620\n" + -// "0.0,-0.4609375,0.0,0.0,2641\n" + -// "0.0,-0.4609375,0.0,0.0,2660\n" + -// "0.0,-0.4609375,0.0,0.0,2679\n" + -// "0.0,-0.4609375,0.0,0.0,2700\n" + -// "0.0,-0.4609375,0.0,0.0,2720\n" + -// "0.0,-0.46875,0.0,0.0,2740\n" + -// "0.0,-0.46875,0.0,0.0,2759\n" + -// "0.0,-0.46875,0.0,0.0,2779\n" + -// "0.0,-0.46875,0.0,0.0,2799\n" + -// "0.0,-0.46875,0.0,0.0,2819\n" + -// "0.0,-0.46875,0.0,0.0,2839\n" + -// "0.0,-0.46875,0.0,0.0,2859\n" + -// "0.0,-0.4765625,0.0,0.0,2880\n" + -// "0.0,-0.5078125,0.0,0.0,2915\n" + -// "0.0,-0.515625,0.0,0.0,2933\n" + -// "0.0,-0.515625,0.0,0.0,2945\n" + -// "0.0,-0.5234375,0.0,0.0,2960\n" + -// "0.0,-0.5234375,0.0,0.0,2979\n" + -// "0.0,-0.5234375,0.0,0.0,2999\n" + -// "0.0,-0.5234375,0.0,0.0,3020\n" + -// "0.0,-0.5234375,0.0,0.0,3040\n" + -// "0.0,-0.5234375,0.0,0.0,3059\n" + -// "0.0,-0.5234375,0.0,0.0,3079\n" + -// "0.0,-0.5234375,0.0,0.0,3099\n" + -// "0.0,-0.5234375,0.0,0.0,3119\n" + -// "0.0,-0.5234375,0.0,0.0,3140\n" + -// "0.0,-0.5234375,0.0,0.0,3159\n" + -// "0.0,-0.5234375,0.0,0.0,3180\n" + -// "0.0,-0.5234375,0.0,0.0,3199\n" + -// "0.0,-0.5234375,0.0,0.0,3219\n" + -// "0.0,-0.5234375,0.0,0.0,3239\n" + -// "0.0,-0.5234375,0.0,0.0,3260\n" + -// "0.0,-0.5234375,0.0,0.0,3279\n" + -// "0.0,-0.5234375,0.0,0.0,3299\n" + -// "0.0,-0.5234375,0.0,0.0,3320\n" + -// "0.0,-0.5234375,0.0,0.0,3339\n" + -// "0.0,-0.5234375,0.0,0.0,3359\n" + -// "0.0,-0.5234375,0.0,0.0,3380\n" + -// "0.0,-0.5234375,0.0,0.0,3400\n" + -// "0.0,-0.5234375,0.0,0.0,3420\n" + -// "0.0,-0.5234375,0.0,0.0,3439\n" + -// "0.0,-0.5234375,0.0,0.0,3459\n" + -// "0.0,-0.5234375,0.0,0.0,3479\n" + -// "0.0,-0.5234375,0.0,0.0,3499\n" + -// "0.0,-0.5234375,0.0,0.0,3520\n" + -// "0.0,-0.5234375,0.0,0.0,3539\n" + -// "0.0,-0.5234375,0.0,0.0,3559\n" + -// "0.0,-0.5234375,0.0,0.0,3579\n" + -// "0.0,-0.5234375,0.0,0.0,3600\n" + -// "0.0,-0.5234375,0.0,0.0,3619\n" + -// "0.0,-0.5234375,0.0,0.0,3640\n" + -// "0.0,-0.5234375,0.0,0.0,3659\n" + -// "0.0,-0.5234375,0.0,0.0,3679\n" + -// "0.0,-0.5234375,0.0,0.0,3699\n" + -// "0.0,-0.5234375,0.0,0.0,3720\n" + -// "0.0,-0.5234375,0.0,0.0,3740\n" + -// "0.0,-0.5234375,0.0,0.0,3759\n" + -// "0.0,-0.5234375,0.0,0.0,3779\n" + -// "0.0,-0.5234375,0.0,0.0,3800\n" + -// "0.0,-0.5234375,0.0,0.0,3820\n" + -// "0.0,-0.5234375,0.0,0.0,3840\n" + -// "0.0,-0.5234375,0.0,0.0,3859\n" + -// "0.0,-0.5234375,0.0,0.0,3879\n" + -// "0.0,-0.5234375,0.0,0.0,3900\n" + -// "0.0,-0.5390625,0.0,0.0,3920\n" + -// "0.0,-0.546875,0.0,0.0,3939\n" + -// "0.0,-0.546875,0.0,0.0,3959\n" + -// "0.0,-0.546875,0.0,0.0,3979\n" + -// "0.0,-0.546875,0.0,0.0,3999\n" + -// "0.0,-0.546875,0.0,0.0,4019\n" + -// "0.0,-0.5546875,0.0,0.0,4039\n" + -// "0.0,-0.5546875,0.0,0.0,4059\n" + -// "0.0,-0.5546875,0.0,0.0,4079\n" + -// "0.0,-0.5546875,0.0,0.0,4134\n" + -// "0.0,-0.5546875,0.0,0.0,4147\n" + -// "0.0,-0.5546875,0.0,0.0,4157\n" + -// "0.0,-0.5546875,0.0,0.0,4170\n" + -// "0.0,-0.5546875,0.0,0.0,4185\n" + -// "0.0,-0.5546875,0.0,0.0,4199\n" + -// "0.0,-0.5546875,0.0,0.0,4219\n" + -// "0.0,-0.5546875,0.0,0.0,4240\n" + -// "-0.0234375,-0.5625,0.0,0.0,4259\n" + -// "-0.03125,-0.5625,0.0,0.0,4279\n" + -// "-0.03125,-0.5625,0.0,0.0,4300\n" + -// "-0.03125,-0.5625,0.0,0.0,4319\n" + -// "-0.03125,-0.5625,0.0,0.0,4339\n" + -// "-0.0234375,-0.5625,0.0,0.0,4360\n" + -// "-0.03125,-0.5625,0.0,0.0,4379\n" + -// "-0.03125,-0.5625,0.0,0.0,4399\n" + -// "-0.03125,-0.5625,0.0,0.0,4419\n" + -// "-0.03125,-0.5625,0.0,0.0,4440\n" + -// "-0.03125,-0.5625,0.0,0.0,4461\n" + -// "-0.0390625,-0.5625,0.0,0.0,4479\n" + -// "-0.046875,-0.5625,0.0,0.0,4499\n" + -// "-0.0546875,-0.5625,0.0,0.0,4520\n" + -// "-0.0625,-0.5625,0.0,0.0,4540\n" + -// "-0.0625,-0.5625,0.0,0.0,4561\n" + -// "-0.0546875,-0.5625,0.0,0.0,4581\n" + -// "-0.0546875,-0.5625,0.0,0.0,4600\n" + -// "-0.0546875,-0.5625,0.0,0.0,4620\n" + -// "-0.0546875,-0.546875,0.0,0.0,4639\n" + -// "-0.0546875,-0.546875,0.0,0.0,4659\n" + -// "-0.0546875,-0.484375,0.0,0.0,4680\n" + -// "-0.0546875,-0.4375,0.0,0.0,4700\n" + -// "-0.0546875,-0.390625,0.0,0.0,4720\n" + -// "-0.0546875,-0.3671875,0.0,0.0,4739\n" + -// "-0.0546875,-0.3359375,0.0,0.0,4759\n" + -// "-0.0546875,-0.3203125,0.0,0.0,4779\n" + -// "-0.0546875,-0.2734375,0.0,0.0,4800\n" + -// "0.0,0.0,0.0,0.0,4820\n" + -// "0.0,0.0,0.0,0.0,4840\n" + -// "0.0,0.0,0.0,0.0,4859\n" + -// "0.0,0.0,0.0,0.0,4879\n" + -// "0.0,0.0,0.0,0.0,4899\n" + -// "0.0,0.0,0.0,0.0,4919\n" + -// "0.0,0.0,0.0,0.0,4939\n" + -// "0.0,0.0,0.0,0.0,4960\n" + -// "0.0,0.0,0.0,0.0,4980\n" + -// "0.0,0.0,0.0,0.0,4999\n" + -// "0.0,0.0,0.0,0.0,5019\n" + -// "0.0,0.0,0.0,0.0,5040\n" + -// "0.0,0.0,0.0,0.0,5061\n" + -// "0.0,0.0,0.0,0.0,5079\n" + -// "0.0,0.0,0.0,0.0,5100\n" + -// "0.0,0.0,0.0,0.0,5119\n" + -// "0.0,0.0,0.0,0.0,5140\n" + -// "0.0,0.0,0.0,0.0,5159\n" + -// "0.0,0.0,0.0,0.0,5180\n" + -// "0.0,0.0,0.0,0.0,5199\n" + -// "0.0,0.0,0.0,0.0,5219\n" + -// "0.0,0.0,0.0,0.0,5239\n" + -// "0.0,0.0,0.0,0.0,5259\n" + -// "0.0,0.0,0.0,0.0,5280\n" + -// "0.0,0.0,0.0,0.0,5299\n" + -// "0.0,0.0,0.0,0.0,5319\n" + -// "0.0,0.0,0.0,0.0,5339\n" + -// "0.0,0.0,0.0,0.0,5360\n" + -// "0.0,0.0,0.0,0.0,5379\n" + -// "0.0,0.0,0.0,0.0,5399\n" + -// "0.0,0.0,0.0,0.0,5419\n" + -// "0.0,0.0,0.0,0.0,5439\n" + -// "0.0,0.0,0.0,0.0,5459\n" + -// "0.0,0.0,0.0,0.0,5480\n" + -// "0.0,0.0,0.0,0.0,5500\n" + -// "0.0,0.0,0.0,0.0,5519\n" + -// "0.0,0.0,0.0,0.0,5540\n" + -// "0.0,0.0,0.0,0.0,5559\n" + -// "0.0,0.0,0.0,0.0,5579\n" + -// "0.0,0.0,0.0,0.0,5599\n" + -// "0.0,0.0,0.0,0.0,5619\n" + -// "0.0,0.0,0.0,0.0,5639\n" + -// "0.0,0.0,0.0,0.0,5659\n" + -// "0.0,0.0,0.0,0.0,5680\n" + -// "0.0,0.0,0.0,0.0,5699\n" + -// "0.0,0.0,0.0,0.0,5721\n" + -// "0.0,0.0,0.0,0.0,5739\n" + -// "0.0,0.0,0.0,0.0,5760\n" + -// "0.0,0.0,0.0,0.0,5779\n" + -// "0.0,0.0,0.0,0.0,5800\n" + -// "0.0,0.0,0.0,0.0,5819\n" + -// "0.0,0.0,0.0,0.0,5839\n" + -// "0.0,0.0,0.0,0.0,5859\n" + -// "0.0,0.0,0.0,0.0,5879\n" + -// "0.0,0.0,0.0,0.0,5900\n" + -// "0.0,0.0,0.0,0.0,5920\n" + -// "0.0,0.0,0.0,0.0,5939\n" + -// "0.0,0.0,0.0,0.0,5959\n" + -// "0.0,0.0,0.0,0.0,5979\n" + -// "-0.03125,0.11811023950576782,0.0,0.0,5999\n" + -// "-0.03125,0.14173229038715363,0.0,0.0,6020\n" + -// "-0.03125,0.14173229038715363,0.0,0.0,6039\n" + -// "-0.03125,0.14173229038715363,0.0,0.0,6059\n" + -// "-0.03125,0.14173229038715363,0.0,0.0,6079\n" + -// "-0.03125,0.14173229038715363,0.0,0.0,6100\n" + -// "-0.03125,0.14173229038715363,0.0,0.0,6119\n" + -// "-0.03125,0.14173229038715363,0.0,0.0,6139\n" + -// "-0.03125,0.14173229038715363,0.0,0.0,6159\n" + -// "-0.03125,0.14173229038715363,0.0,0.0,6179\n" + -// "-0.03125,0.14960630238056183,0.0,0.0,6199\n" + -// "-0.03125,0.14960630238056183,0.0,0.0,6219\n" + -// "-0.03125,0.14960630238056183,0.0,0.0,6239\n" + -// "-0.03125,0.14960630238056183,0.0,0.0,6259\n" + -// "-0.03125,0.14960630238056183,0.0,0.0,6280\n" + -// "-0.03125,0.14960630238056183,0.0,0.0,6299\n" + -// "-0.03125,0.16535432636737823,0.0,0.0,6319\n" + -// "-0.03125,0.19685038924217224,0.0,0.0,6340\n" + -// "-0.03125,0.20472441613674164,0.0,0.0,6360\n" + -// "-0.03125,0.21259842813014984,0.0,0.0,6381\n" + -// "-0.03125,0.21259842813014984,0.0,0.0,6399\n" + -// "-0.03125,0.21259842813014984,0.0,0.0,6419\n" + -// "-0.03125,0.21259842813014984,0.0,0.0,6440\n" + -// "-0.03125,0.21259842813014984,0.0,0.0,6459\n" + -// "-0.03125,0.21259842813014984,0.0,0.0,6479\n" + -// "-0.03125,0.21259842813014984,0.0,0.0,6499\n" + -// "-0.03125,0.21259842813014984,0.0,0.0,6519\n" + -// "-0.03125,0.21259842813014984,0.0,0.0,6540\n" + -// "-0.03125,0.21259842813014984,0.0,0.0,6559\n" + -// "-0.03125,0.21259842813014984,0.0,0.0,6579\n" + -// "-0.03125,0.21259842813014984,0.0,0.0,6599\n" + -// "-0.03125,0.21259842813014984,0.0,0.0,6619\n" + -// "-0.03125,0.21259842813014984,0.0,0.0,6640\n" + -// "-0.03125,0.21259842813014984,0.0,0.0,6660\n" + -// "-0.03125,0.21259842813014984,0.0,0.0,6679\n" + -// "-0.03125,0.33070865273475647,0.0,0.0,6700\n" + -// "-0.03125,0.33070865273475647,0.0,0.0,6720\n" + -// "-0.03125,0.3700787425041199,0.0,0.0,6740\n" + -// "-0.03125,0.3779527544975281,0.0,0.0,6760\n" + -// "-0.03125,0.3779527544975281,0.0,0.0,6780\n" + -// "-0.03125,0.3779527544975281,0.0,0.0,6800\n" + -// "-0.03125,0.3779527544975281,0.0,0.0,6819\n" + -// "-0.03125,0.3779527544975281,0.0,0.0,6839\n" + -// "-0.03125,0.3700787425041199,0.0,0.0,6859\n" + -// "-0.03125,0.3700787425041199,0.0,0.0,6879\n" + -// "-0.03125,0.3700787425041199,0.0,0.0,6899\n" + -// "-0.03125,0.3700787425041199,0.0,0.0,6920\n" + -// "-0.03125,0.3700787425041199,0.0,0.0,6939\n" + -// "-0.03125,0.3700787425041199,0.0,0.0,6959\n" + -// "-0.03125,0.3700787425041199,0.0,0.0,6979\n" + -// "-0.03125,0.3700787425041199,0.0,0.0,6999\n" + -// "-0.03125,0.3700787425041199,0.0,0.0,7019\n" + -// "-0.03125,0.3700787425041199,0.0,0.0,7039\n" + -// "-0.03125,0.3700787425041199,0.0,0.0,7060\n" + -// "-0.03125,0.3700787425041199,0.0,0.0,7080\n" + -// "-0.03125,0.3700787425041199,0.0,0.0,7100\n" + -// "-0.03125,0.3700787425041199,0.0,0.0,7119\n" + -// "-0.03125,0.3700787425041199,0.0,0.0,7139\n" + -// "-0.03125,0.3700787425041199,0.0,0.0,7159\n" + -// "-0.03125,0.3700787425041199,0.0,0.0,7180\n" + -// "-0.03125,0.3700787425041199,0.0,0.0,7199\n" + -// "-0.03125,0.3700787425041199,0.0,0.0,7220\n" + -// "-0.03125,0.3700787425041199,0.0,0.0,7239\n" + -// "-0.03125,0.3700787425041199,0.0,0.0,7260\n" + -// "-0.03125,0.36220473051071167,0.0,0.0,7279\n" + -// "-0.03125,0.34645670652389526,0.0,0.0,7299\n" + -// "0.0,0.33070865273475647,0.0,0.0,7319\n" + -// "0.0,0.33070865273475647,0.0,0.0,7339\n" + -// "0.0,0.3385826647281647,0.0,0.0,7359\n" + -// "0.0,0.3385826647281647,0.0,0.0,7379\n" + -// "0.0,0.3385826647281647,0.0,0.0,7399\n" + -// "0.0,0.3385826647281647,0.0,0.0,7419\n" + -// "0.0,0.3385826647281647,0.0,0.0,7440\n" + -// "0.0,0.33070865273475647,0.0,0.0,7460\n" + -// "0.0,0.31496062874794006,0.0,0.0,7479\n" + -// "0.0,0.31496062874794006,0.0,0.0,7499\n" + -// "0.0,0.29133859276771545,0.0,0.0,7519\n" + -// "0.0,0.24409449100494385,0.0,0.0,7540\n" + -// "0.0,0.24409449100494385,0.0,0.0,7559\n" + -// "0.0,0.24409449100494385,0.0,0.0,7580\n" + -// "0.0,0.24409449100494385,0.0,0.0,7600\n" + -// "0.0,0.24409449100494385,0.0,0.0,7619\n" + -// "0.0,0.24409449100494385,0.0,0.0,7639\n" + -// "0.0,0.24409449100494385,0.0,0.0,7660\n" + -// "0.0,0.24409449100494385,0.0,0.0,7679\n" + -// "0.0,0.24409449100494385,0.0,0.0,7700\n" + -// "0.0,0.25196850299835205,0.0,0.0,7720\n" + -// "0.0,0.33070865273475647,0.0,0.0,7740\n" + -// "0.0,0.3700787425041199,0.0,0.0,7760\n" + -// "0.0,0.4251968562602997,0.0,0.0,7780\n" + -// "0.0,0.4566929042339325,0.0,0.0,7800\n" + -// "0.0,0.4645669162273407,0.0,0.0,7820\n" + -// "0.0,0.4645669162273407,0.0,0.0,7839\n" + -// "0.0,0.4645669162273407,0.0,0.0,7859\n" + -// "0.0,0.4645669162273407,0.0,0.0,7880\n" + -// "0.0,0.4645669162273407,0.0,0.0,7899\n" + -// "0.0,0.4645669162273407,0.0,0.0,7920\n" + -// "0.0,0.4645669162273407,0.0,0.0,7939\n" + -// "0.0,0.4803149700164795,0.0,0.0,7960\n" + -// "0.0,0.5039370059967041,0.0,0.0,7980\n" + -// "-0.0078125,0.5433070659637451,0.0,0.0,7999\n" + -// "-0.0078125,0.5748031735420227,0.0,0.0,8019\n" + -// "-0.015625,0.5748031735420227,0.0,0.0,8039\n" + -// "-0.015625,0.5748031735420227,0.0,0.0,8059\n" + -// "-0.015625,0.5748031735420227,0.0,0.0,8080\n" + -// "-0.015625,0.5511810779571533,0.0,0.0,8099\n" + -// "-0.015625,0.5354330539703369,0.0,0.0,8119\n" + -// "-0.015625,0.5354330539703369,0.0,0.0,8139\n" + -// "-0.015625,0.5354330539703369,0.0,0.0,8159\n" + -// "-0.0078125,0.5354330539703369,0.0,0.0,8180\n" + -// "-0.0078125,0.5354330539703369,0.0,0.0,8199\n" + -// "-0.0078125,0.5354330539703369,0.0,0.0,8220\n" + -// "-0.0078125,0.5354330539703369,0.0,0.0,8239\n" + -// "-0.0078125,0.5354330539703369,0.0,0.0,8259\n" + -// "-0.0078125,0.5354330539703369,0.0,0.0,8279\n" + -// "-0.0078125,0.5354330539703369,0.0,0.0,8299\n" + -// "-0.0078125,0.5354330539703369,0.0,0.0,8319\n" + -// "-0.0078125,0.5354330539703369,0.0,0.0,8340\n" + -// "-0.0078125,0.5275590419769287,0.0,0.0,8359\n" + -// "-0.0078125,0.4803149700164795,0.0,0.0,8379\n" + -// "-0.0078125,0.4803149700164795,0.0,0.0,8399\n" + -// "-0.0078125,0.4803149700164795,0.0,0.0,8421\n" + -// "-0.0078125,0.4803149700164795,0.0,0.0,8440\n" + -// "-0.0078125,0.4803149700164795,0.0,0.0,8460\n" + -// "-0.0078125,0.4803149700164795,0.0,0.0,8479\n" + -// "0.0,0.4803149700164795,0.0,0.0,8499\n" + -// "0.0,0.4803149700164795,0.0,0.0,8520\n" + -// "0.0,0.4803149700164795,0.0,0.0,8540\n" + -// "0.0,0.4803149700164795,0.0,0.0,8559\n" + -// "0.0,0.4803149700164795,0.0,0.0,8579\n" + -// "0.0,0.4803149700164795,0.0,0.0,8600\n" + -// "0.0,0.4803149700164795,0.0,0.0,8619\n" + -// "0.0,0.4803149700164795,0.0,0.0,8639\n" + -// "0.0,0.4803149700164795,0.0,0.0,8659\n" + -// "0.0,0.4724409580230713,0.0,0.0,8679\n" + -// "0.0,0.4409448802471161,0.0,0.0,8699\n" + -// "0.0,0.4173228442668915,0.0,0.0,8719\n" + -// "0.0,0.4094488322734833,0.0,0.0,8739\n" + -// "0.0,0.4094488322734833,0.0,0.0,8759\n" + -// "0.0,0.4094488322734833,0.0,0.0,8779\n" + -// "0.015748031437397003,0.4094488322734833,0.0,0.0,8799\n" + -// "0.04724409431219101,0.4094488322734833,0.0,0.0,8820\n" + -// "0.07874015718698502,0.4094488322734833,0.0,0.0,8840\n" + -// "0.07874015718698502,0.4094488322734833,0.0,0.0,8859\n" + -// "0.07874015718698502,0.4094488322734833,0.0,0.0,8879\n" + -// "0.07874015718698502,0.4251968562602997,0.0,0.0,8899\n" + -// "0.07874015718698502,0.4488188922405243,0.0,0.0,8920\n" + -// "0.07874015718698502,0.4488188922405243,0.0,0.0,8939\n" + -// "0.07874015718698502,0.4488188922405243,0.0,0.0,8959\n" + -// "0.07874015718698502,0.4488188922405243,0.0,0.0,8980\n" + -// "0.05511811003088951,0.4488188922405243,0.0,0.0,8999\n" + -// "0.031496062874794006,0.4488188922405243,0.0,0.0,9019\n" + -// "0.031496062874794006,0.4488188922405243,0.0,0.0,9039\n" + -// "0.031496062874794006,0.4488188922405243,0.0,0.0,9059\n" + -// "0.031496062874794006,0.4488188922405243,0.0,0.0,9079\n" + -// "0.031496062874794006,0.4488188922405243,0.0,0.0,9099\n" + -// "0.031496062874794006,0.4488188922405243,0.0,0.0,9120\n" + -// "0.031496062874794006,0.4488188922405243,0.0,0.0,9139\n" + -// "0.031496062874794006,0.4488188922405243,0.0,0.0,9159\n" + -// "0.031496062874794006,0.4488188922405243,0.0,0.0,9179\n" + -// "0.031496062874794006,0.4488188922405243,0.0,0.0,9199\n" + -// "0.031496062874794006,0.4488188922405243,0.0,0.0,9219\n" + -// "0.031496062874794006,0.4488188922405243,0.0,0.0,9239\n" + -// "0.04724409431219101,0.4488188922405243,0.0,0.0,9260\n" + -// "0.06299212574958801,0.4488188922405243,0.0,0.0,9279\n" + -// "0.06299212574958801,0.4488188922405243,0.0,0.0,9299\n" + -// "0.06299212574958801,0.4488188922405243,0.0,0.0,9319\n" + -// "0.06299212574958801,0.4488188922405243,0.0,0.0,9339\n" + -// "0.07874015718698502,0.4488188922405243,0.0,0.0,9359\n" + -// "0.07874015718698502,0.4488188922405243,0.0,0.0,9380\n" + -// "0.07874015718698502,0.4488188922405243,0.0,0.0,9400\n" + -// "0.07874015718698502,0.4488188922405243,0.0,0.0,9419\n" + -// "0.07874015718698502,0.4488188922405243,0.0,0.0,9439\n" + -// "0.07874015718698502,0.4488188922405243,0.0,0.0,9460\n" + -// "0.07874015718698502,0.4488188922405243,0.0,0.0,9480\n" + -// "0.07874015718698502,0.4488188922405243,0.0,0.0,9499\n" + -// "0.07874015718698502,0.4488188922405243,0.0,0.0,9519\n" + -// "0.07874015718698502,0.4488188922405243,0.0,0.0,9539\n" + -// "0.07874015718698502,0.4488188922405243,0.0,0.0,9559\n" + -// "0.07874015718698502,0.4488188922405243,0.0,0.0,9579\n" + -// "0.07874015718698502,0.4488188922405243,0.0,0.0,9600\n" + -// "0.07874015718698502,0.4488188922405243,0.0,0.0,9619\n" + -// "0.07874015718698502,0.4488188922405243,0.0,0.0,9639\n" + -// "0.07874015718698502,0.4488188922405243,0.0,0.0,9659\n" + -// "0.07874015718698502,0.4488188922405243,0.0,0.0,9679\n" + -// "0.07874015718698502,0.4488188922405243,0.0,0.0,9699\n" + -// "0.07874015718698502,0.4488188922405243,0.0,0.0,9722\n" + -// "0.07874015718698502,0.4488188922405243,0.0,0.0,9739\n" + -// "0.07874015718698502,0.4488188922405243,0.0,0.0,9759\n" + -// "0.08661417663097382,0.4488188922405243,0.0,0.0,9780\n" + -// "0.08661417663097382,0.4488188922405243,0.0,0.0,9800\n" + -// "0.11811023950576782,0.4488188922405243,0.0,0.0,9820\n" + -// "0.14173229038715363,0.4488188922405243,0.0,0.0,9839\n" + -// "0.16535432636737823,0.4488188922405243,0.0,0.0,9859\n" + -// "0.22834645211696625,0.4488188922405243,0.0,0.0,9879\n" + -// "0.23622047901153564,0.4488188922405243,0.0,0.0,9899\n" + -// "0.23622047901153564,0.4409448802471161,0.0,0.0,9919\n" + -// "0.25984251499176025,0.4094488322734833,0.0,0.0,9940\n" + -// "0.29921260476112366,0.3858267664909363,0.0,0.0,9959\n" + -// "0.31496062874794006,0.3779527544975281,0.0,0.0,9979\n" + -// "0.32283464074134827,0.3700787425041199,0.0,0.0,9999\n" + -// "0.32283464074134827,0.3700787425041199,0.0,0.0,10019\n" + -// "0.36220473051071167,0.33070865273475647,0.0,0.0,10039\n" + -// "0.3779527544975281,0.25984251499176025,0.0,0.0,10059\n" + -// "0.3937007784843445,0.21259842813014984,0.0,0.0,10081\n" + -// "0.3937007784843445,0.20472441613674164,0.0,0.0,10100\n" + -// "0.3937007784843445,0.20472441613674164,0.0,0.0,10124\n" + -// "0.3937007784843445,0.20472441613674164,0.0,0.0,10140\n" + -// "0.3937007784843445,0.18897637724876404,0.0,0.0,10159\n" + -// "0.3937007784843445,0.18897637724876404,0.0,0.0,10180\n" + -// "0.3937007784843445,0.16535432636737823,0.0,0.0,10199\n" + -// "0.3937007784843445,0.16535432636737823,0.0,0.0,10221\n" + -// "0.3937007784843445,0.17322835326194763,0.0,0.0,10239\n" + -// "0.3937007784843445,0.16535432636737823,0.0,0.0,10260\n" + -// "0.3937007784843445,0.14960630238056183,0.0,0.0,10279\n" + -// "0.3937007784843445,0.14960630238056183,0.0,0.0,10300\n" + -// "0.3937007784843445,0.14960630238056183,0.0,0.0,10319\n" + -// "0.3937007784843445,0.14960630238056183,0.0,0.0,10340\n" + -// "0.3937007784843445,0.14173229038715363,0.0,0.0,10360\n" + -// "0.3937007784843445,0.14173229038715363,0.0,0.0,10380\n" + -// "0.3937007784843445,0.14173229038715363,0.0,0.0,10399\n" + -// "0.3937007784843445,0.14173229038715363,0.0,0.0,10419\n" + -// "0.3937007784843445,0.14173229038715363,0.0,0.0,10440\n" + -// "0.3937007784843445,0.14173229038715363,0.0,0.0,10459\n" + -// "0.3937007784843445,0.14173229038715363,0.0,0.0,10479\n" + -// "0.3937007784843445,0.14173229038715363,0.0,0.0,10500\n" + -// "0.3937007784843445,0.14173229038715363,0.0,0.0,10519\n" + -// "0.4015747904777527,0.14173229038715363,0.0,0.0,10540\n" + -// "0.4173228442668915,0.14173229038715363,0.0,0.0,10559\n" + -// "0.4251968562602997,0.13385826349258423,0.0,0.0,10580\n" + -// "0.4251968562602997,0.12598425149917603,0.0,0.0,10600\n" + -// "0.4251968562602997,0.10236220806837082,0.0,0.0,10620\n" + -// "0.4251968562602997,0.10236220806837082,0.0,0.0,10639\n" + -// "0.4330708682537079,0.10236220806837082,0.0,0.0,10659\n" + -// "0.4409448802471161,0.10236220806837082,0.0,0.0,10680\n" + -// "0.4488188922405243,0.07086614519357681,0.0,0.0,10699\n" + -// "0.4566929042339325,0.023622047156095505,0.0,0.0,10719\n" + -// "0.4566929042339325,0.0,0.0,0.0,10739\n" + -// "0.4645669162273407,0.0,0.0,0.0,10760\n" + -// "0.4645669162273407,0.0,0.0,0.0,10779\n" + -// "0.4724409580230713,0.0,0.0,0.0,10800\n" + -// "0.4724409580230713,0.0,0.0,0.0,10819\n" + -// "0.4724409580230713,0.0,0.0,0.0,10839\n" + -// "0.4803149700164795,0.0,0.0,0.0,10860\n" + -// "0.4881889820098877,0.0,0.0,0.0,10879\n" + -// "0.5039370059967041,0.0,0.0,0.0,10899\n" + -// "0.5275590419769287,0.0,0.0,0.0,10919\n" + -// "0.5354330539703369,0.0,0.0,0.0,10940\n" + -// "0.5354330539703369,0.0,0.0,0.0,10960\n" + -// "0.5354330539703369,0.0,0.0,0.0,10979\n" + -// "0.5354330539703369,0.0,0.0,0.0,10999\n" + -// "0.5354330539703369,0.0,0.0,0.0,11020\n" + -// "0.5354330539703369,0.0,0.0,0.0,11039\n" + -// "0.5354330539703369,0.0,0.0,0.0,11059\n" + -// "0.5354330539703369,0.0,0.0,0.0,11079\n" + -// "0.5354330539703369,0.0,0.0,0.0,11099\n" + -// "0.5354330539703369,0.0,0.0,0.0,11119\n" + -// "0.5354330539703369,0.0,0.0,0.0,11140\n" + -// "0.5354330539703369,0.0,0.0,0.0,11159\n" + -// "0.5354330539703369,0.0,0.0,0.0,11179\n" + -// "0.5354330539703369,0.0,0.0,0.0,11200\n" + -// "0.5354330539703369,0.0,0.0,0.0,11219\n" + -// "0.5354330539703369,0.0,0.0,0.0,11239\n" + -// "0.5354330539703369,0.0,0.0,0.0,11260\n" + -// "0.5354330539703369,0.0,0.0,0.0,11279\n" + -// "0.5354330539703369,0.0,0.0,0.0,11299\n" + -// "0.5275590419769287,0.0,0.0,0.0,11319\n" + -// "0.5275590419769287,0.0,0.0,0.0,11339\n" + -// "0.5275590419769287,0.0,0.0,0.0,11360\n" + -// "0.5275590419769287,0.0,0.0,0.0,11379\n" + -// "0.5275590419769287,0.0,0.0,0.0,11399\n" + -// "0.5275590419769287,0.0,0.0,0.0,11419\n" + -// "0.5275590419769287,0.0,0.0,0.0,11440\n" + -// "0.5275590419769287,0.0,0.0,0.0,11459\n" + -// "0.5275590419769287,0.0,0.0,0.0,11479\n" + -// "0.5275590419769287,0.0,0.0,0.0,11500\n" + -// "0.5275590419769287,0.0,0.0,0.0,11519\n" + -// "0.5275590419769287,0.0,0.0,0.0,11539\n" + -// "0.5275590419769287,0.0,0.0,0.0,11559\n" + -// "0.5275590419769287,0.0,0.0,0.0,11579\n" + -// "0.5118110179901123,0.0,0.0,0.0,11599\n" + -// "0.5118110179901123,0.0,0.0,0.0,11620\n" + -// "0.5118110179901123,0.0,0.0,0.0,11640\n" + -// "0.5039370059967041,0.0,0.0,0.0,11660\n" + -// "0.4803149700164795,0.0,0.0,0.0,11679\n" + -// "0.4566929042339325,0.0,0.0,0.0,11699\n" + -// "0.4330708682537079,0.0,0.0,0.0,11720\n" + -// "0.4173228442668915,0.0,0.0,0.0,11739\n" + -// "0.4094488322734833,0.0,0.0,0.0,11760\n" + -// "0.4094488322734833,0.0,0.0,0.0,11780\n" + -// "0.4094488322734833,0.0,0.0,0.0,11800\n" + -// "0.4094488322734833,0.0,0.0,0.0,11819\n" + -// "0.4094488322734833,0.0,0.0,0.0,11840\n" + -// "0.3937007784843445,0.0,0.0,0.0,11859\n" + -// "0.25196850299835205,-0.0390625,0.0,0.0,11879\n" + -// "0.11811023950576782,-0.078125,0.0,0.0,11900\n" + -// "0.06299212574958801,-0.078125,0.0,0.0,11920\n" + -// "0.0,0.0,0.0,0.0,11939\n" + -// "0.0,0.0,0.0,0.0,11959\n" + -// "0.0,0.0,0.0,0.0,11979\n" + -// "0.0,0.0,0.0,0.0,12000\n" + -// "0.0,0.0,0.0,0.0,12019\n" + -// "0.0,0.0,0.0,0.0,12040\n" + -// "0.0,0.0,0.0,0.0,12060\n" + -// "0.0,0.0,0.0,0.0,12080\n" + -// "0.0,0.0,0.0,0.0,12099\n" + -// "0.0,0.0,0.0,0.0,12119\n" + -// "0.0,-0.1015625,0.0,0.0,12139\n" + -// "0.0,-0.1015625,0.0,0.0,12159\n" + -// "0.0,-0.109375,0.0,0.0,12179\n" + -// "0.0,-0.1171875,0.0,0.0,12199\n" + -// "0.0,-0.1171875,0.0,0.0,12219\n" + -// "0.0,-0.1171875,0.0,0.0,12240\n" + -// "0.0,-0.1328125,0.0,0.0,12259\n" + -// "0.0,-0.15625,0.0,0.0,12279\n" + -// "0.0,-0.1953125,0.0,0.0,12300\n" + -// "0.0,-0.21875,0.0,0.0,12319\n" + -// "0.0,-0.28125,0.0,0.0,12339\n" + -// "0.0,-0.296875,0.0,0.0,12360\n" + -// "0.0,-0.3046875,0.0,0.0,12379\n" + -// "0.0,-0.3125,0.0,0.0,12399\n" + -// "0.0,-0.3359375,0.0,0.0,12419\n" + -// "0.0,-0.3359375,0.0,0.0,12440\n" + -// "0.0,-0.328125,0.0,0.0,12459\n" + -// "0.0,-0.328125,0.0,0.0,12479\n" + -// "0.0,-0.328125,0.0,0.0,12499\n" + -// "0.0,-0.328125,0.0,0.0,12520\n" + -// "0.0,-0.328125,0.0,0.0,12539\n" + -// "0.0,-0.328125,0.0,0.0,12560\n" + -// "0.0,-0.328125,0.0,0.0,12580\n" + -// "0.0,-0.328125,0.0,0.0,12599\n" + -// "0.0,-0.328125,0.0,0.0,12620\n" + -// "0.0,-0.328125,0.0,0.0,12639\n" + -// "0.0,-0.328125,0.0,0.0,12659\n" + -// "0.0,-0.328125,0.0,0.0,12679\n" + -// "0.0,-0.328125,0.0,0.0,12699\n" + -// "0.0,-0.328125,0.0,0.0,12719\n" + -// "0.0,-0.328125,0.0,0.0,12739\n" + -// "0.0,-0.328125,0.0,0.0,12760\n" + -// "0.0,-0.34375,0.0,0.0,12780\n" + -// "0.0,-0.3515625,0.0,0.0,12799\n" + -// "0.0,-0.359375,0.0,0.0,12819\n" + -// "0.0,-0.359375,0.0,0.0,12840\n" + -// "0.0,-0.359375,0.0,0.0,12860\n" + -// "0.0,-0.359375,0.0,0.0,12880\n" + -// "0.0,-0.359375,0.0,0.0,12900\n" + -// "0.0,-0.3671875,0.0,0.0,12921\n" + -// "0.0,-0.3671875,0.0,0.0,12939\n" + -// "0.0,-0.375,0.0,0.0,12959\n" + -// "0.0,-0.375,0.0,0.0,12979\n" + -// "0.0,-0.390625,0.0,0.0,12999\n" + -// "0.015748031437397003,-0.40625,0.0,0.0,13019\n" + -// "0.023622047156095505,-0.421875,0.0,0.0,13039\n" + -// "0.03937007859349251,-0.4296875,0.0,0.0,13060\n" + -// "0.03937007859349251,-0.4296875,0.0,0.0,13079\n" + -// "0.04724409431219101,-0.453125,0.0,0.0,13099\n" + -// "0.05511811003088951,-0.46875,0.0,0.0,13119\n" + -// "0.05511811003088951,-0.484375,0.0,0.0,13139\n" + -// "0.05511811003088951,-0.5,0.0,0.0,13160\n" + -// "0.05511811003088951,-0.515625,0.0,0.0,13179\n" + -// "0.05511811003088951,-0.515625,0.0,0.0,13199\n" + -// "0.05511811003088951,-0.515625,0.0,0.0,13219\n" + -// "0.05511811003088951,-0.5234375,0.0,0.0,13239\n" + -// "0.05511811003088951,-0.5234375,0.0,0.0,13260\n" + -// "0.05511811003088951,-0.5234375,0.0,0.0,13279\n" + -// "0.05511811003088951,-0.5234375,0.0,0.0,13299\n" + -// "0.05511811003088951,-0.5234375,0.0,0.0,13319\n" + -// "0.04724409431219101,-0.5234375,0.0,0.0,13339\n" + -// "0.04724409431219101,-0.5234375,0.0,0.0,13359\n" + -// "0.04724409431219101,-0.5234375,0.0,0.0,13379\n" + -// "0.04724409431219101,-0.5234375,0.0,0.0,13400\n" + -// "0.04724409431219101,-0.5234375,0.0,0.0,13419\n" + -// "0.04724409431219101,-0.5234375,0.0,0.0,13439\n" + -// "0.04724409431219101,-0.5234375,0.0,0.0,13459\n" + -// "0.04724409431219101,-0.5234375,0.0,0.0,13479\n" + -// "0.04724409431219101,-0.5234375,0.0,0.0,13499\n" + -// "0.04724409431219101,-0.5234375,0.0,0.0,13519\n" + -// "0.04724409431219101,-0.5234375,0.0,0.0,13539\n" + -// "0.04724409431219101,-0.5234375,0.0,0.0,13559\n" + -// "0.04724409431219101,-0.5234375,0.0,0.0,13579\n" + -// "0.04724409431219101,-0.5234375,0.0,0.0,13599\n" + -// "0.04724409431219101,-0.5234375,0.0,0.0,13620\n" + -// "0.04724409431219101,-0.5234375,0.0,0.0,13642\n" + -// "0.04724409431219101,-0.5234375,0.0,0.0,13659\n" + -// "0.04724409431219101,-0.5234375,0.0,0.0,13679\n" + -// "0.04724409431219101,-0.5234375,0.0,0.0,13699\n" + -// "0.04724409431219101,-0.5234375,0.0,0.0,13720\n" + -// "0.04724409431219101,-0.5234375,0.0,0.0,13740\n" + -// "0.04724409431219101,-0.5234375,0.0,0.0,13760\n" + -// "0.04724409431219101,-0.5234375,0.0,0.0,13779\n" + -// "0.04724409431219101,-0.53125,0.0,0.0,13800\n" + -// "0.04724409431219101,-0.546875,0.0,0.0,13819\n" + -// "0.04724409431219101,-0.5625,0.0,0.0,13840\n" + -// "0.04724409431219101,-0.578125,0.0,0.0,13859\n" + -// "0.04724409431219101,-0.59375,0.0,0.0,13880\n" + -// "0.07874015718698502,-0.6875,0.0,0.0,13900\n" + -// "0.07874015718698502,-0.703125,0.0,0.0,13919\n" + -// "0.07874015718698502,-0.7265625,0.0,0.0,13944\n" + -// "0.07874015718698502,-0.7421875,0.0,0.0,13959\n" + -// "0.07874015718698502,-0.765625,0.0,0.0,13980\n" + -// "0.07874015718698502,-0.78125,0.0,0.0,14000\n" + -// "0.07874015718698502,-0.7890625,0.0,0.0,14020\n" + -// "0.07874015718698502,-0.8125,0.0,0.0,14039\n" + -// "0.07874015718698502,-0.8203125,0.0,0.0,14060\n" + -// "0.07874015718698502,-0.828125,0.0,0.0,14079\n" + -// "0.07874015718698502,-0.8359375,0.0,0.0,14099\n" + -// "0.07874015718698502,-0.8359375,0.0,0.0,14119\n" + -// "0.07874015718698502,-0.8359375,0.0,0.0,14139\n" + -// "0.07874015718698502,-0.8359375,0.0,0.0,14159\n" + -// "0.07874015718698502,-0.8359375,0.0,0.0,14179\n" + -// "0.07874015718698502,-0.8203125,0.0,0.0,14199\n" + -// "0.07874015718698502,-0.78125,0.0,0.0,14219\n" + -// "0.007874015718698502,-0.265625,0.0,0.0,14239\n" + -// "0.0,0.0,0.0,0.0,14259\n" + -// "0.0,0.0,0.0,0.0,14279\n" + -// "0.0,-0.1171875,0.0,0.0,14299\n" + -// "0.0,-0.1328125,0.0,0.0,14319\n" + -// "0.0,-0.171875,0.0,0.0,14339\n" + -// "0.0,-0.171875,0.0,0.0,14359\n" + -// "0.0,0.0,0.0,0.0,14379\n" + -// "0.0,0.0,0.0,0.0,14399\n" + -// "0.0,0.0,0.0,0.0,14419\n" + -// "0.0,0.0,0.0,0.0,14439\n" + -// "0.0,0.0,0.0,0.0,14459\n" + -// "0.0,0.0,0.0,0.0,14479\n" + -// "0.0,0.0,0.0,0.0,14499\n" + -// "0.0,0.0,0.0,0.0,14519\n" + -// "0.0,-0.203125,0.0,0.0,14539\n" + -// "0.0,-0.4609375,0.0,0.0,14559\n" + -// "0.031496062874794006,-0.71875,0.0,0.0,14579\n" + -// "0.031496062874794006,-0.9453125,0.0,0.0,14599\n" + -// "0.03148045240972986,-0.999504367732367,0.0,0.0,14619\n" + -// "0.03148045240972986,-0.999504367732367,0.0,0.0,14639\n" + -// "0.03148045240972986,-0.999504367732367,0.0,0.0,14660\n" + -// "0.03148045240972986,-0.999504367732367,0.0,0.0,14679\n" + -// "0.03148045240972986,-0.999504367732367,0.0,0.0,14699\n" + -// "0.03148045240972986,-0.999504367732367,0.0,0.0,14719\n" + -// "0.031496062874794006,-0.9609375,0.0,0.0,14739\n" + -// "0.031496062874794006,-0.9609375,0.0,0.0,14759\n" + -// "0.031496062874794006,-0.9609375,0.0,0.0,14780\n" + -// "0.031496062874794006,-0.9609375,0.0,0.0,14799\n" + -// "0.031496062874794006,-0.953125,0.0,0.0,14820\n" + -// "0.031496062874794006,-0.9609375,0.0,0.0,14839\n" + -// "0.03148045240972986,-0.999504367732367,0.0,0.0,14860\n" + -// "0.03148045240972986,-0.999504367732367,0.0,0.0,14880\n" + -// "0.03148045240972986,-0.999504367732367,0.0,0.0,14900\n" + -// "0.03148045240972986,-0.999504367732367,0.0,0.0,14919\n" + -// "0.031496062874794006,-0.984375,0.0,0.0,14939\n" + -// "0.0,-0.3515625,0.0,0.0,14959\n" + -// "0.0,0.0,0.0,0.0,14980\n" + -// "0.0,0.0,0.0,0.0,14999\n" + -// "0.0,0.0,0.0,0.0,15019\n" + -// "0.0,0.0,0.0,0.0,15039\n" + -// "0.0,0.0,0.0,0.0,15059\n" + -// "0.0,0.0,0.0,0.0,15082\n" + -// "0.0,0.0,0.0,0.0,15099\n" + -// "0.0,0.0,0.0,0.0,15120\n" -// ); - -// writer.close(); - -// System.out.println("Writing Done."); -// } catch (IOException e) { -// e.printStackTrace(); -// } - // 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 From cdefdda36456b7b56f818f8c466655b41fcc0c90 Mon Sep 17 00:00:00 2001 From: 66945 <54561572+66945@users.noreply.github.com> Date: Sat, 25 Feb 2023 21:00:55 -0700 Subject: [PATCH 37/40] Removed button bind We don't need this now that we have the shuffleboard button --- src/main/java/frc4388/robot/RobotContainer.java | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index e619546..06ba4be 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -107,9 +107,6 @@ public class RobotContainer { playbackChooser = new PlaybackChooser(m_robotSwerveDrive, "Balance", new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); - - new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) - .onTrue(new InstantCommand(() -> playbackChooser.appendCommand())); } From 94f38856d11c62b3d8c8dceee655c2b4b98a6748 Mon Sep 17 00:00:00 2001 From: 66945 <54561572+66945@users.noreply.github.com> Date: Sat, 25 Feb 2023 21:04:40 -0700 Subject: [PATCH 38/40] added defaults --- .../java/frc4388/robot/RobotContainer.java | 26 +++++++++---------- .../robot/commands/PlaybackChooser.java | 2 +- 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 06ba4be..1644231 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -69,17 +69,17 @@ public class RobotContainer { private Command noAuto = new InstantCommand(); - private Command balance = new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive); + // private Command balance = new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive); - private Command blue1Path = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt"); - private Command blue1PathWithBalance = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt").andThen(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); + // private Command blue1Path = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt"); + // private Command blue1PathWithBalance = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt").andThen(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); - private Command red1Path = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt", -1); - private Command red1PathWithBalance = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt", -1).andThen(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); + // private Command red1Path = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt", -1); + // private Command red1PathWithBalance = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt", -1).andThen(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); - private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"); + // private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"); - PlaybackChooser playbackChooser; + private PlaybackChooser playbackChooser; /** * The container for the robot. Contains subsystems, OI devices, and commands. */ @@ -95,15 +95,15 @@ public class RobotContainer { .withName("SwerveDrive DefaultCommand")); // * Auto Commands - chooser.setDefaultOption("NoAuto", noAuto); + // chooser.setDefaultOption("NoAuto", noAuto); - chooser.addOption("Blue1Path", blue1Path); - chooser.addOption("Blue1PathWithBalance", blue1PathWithBalance); + // chooser.addOption("Blue1Path", blue1Path); + // chooser.addOption("Blue1PathWithBalance", blue1PathWithBalance); - chooser.addOption("Red1Path", red1Path); - chooser.addOption("Red1PathWithBalance", red1PathWithBalance); + // chooser.addOption("Red1Path", red1Path); + // chooser.addOption("Red1PathWithBalance", red1PathWithBalance); - chooser.addOption("Taxi", taxi); + // chooser.addOption("Taxi", taxi); playbackChooser = new PlaybackChooser(m_robotSwerveDrive, "Balance", new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); diff --git a/src/main/java/frc4388/robot/commands/PlaybackChooser.java b/src/main/java/frc4388/robot/commands/PlaybackChooser.java index b039067..1a94efc 100644 --- a/src/main/java/frc4388/robot/commands/PlaybackChooser.java +++ b/src/main/java/frc4388/robot/commands/PlaybackChooser.java @@ -47,13 +47,13 @@ public class PlaybackChooser { public void appendCommand() { var chooser = new SendableChooser(); + chooser.setDefaultOption("No Auto", m_noAuto); for (String auto : m_dir.list()) { m_playback.addOption(auto, new JoystickPlayback(m_swerve, auto)); } for (var cmd_name : m_commandPool.keySet()) { chooser.addOption(cmd_name, m_commandPool.get(cmd_name)); } - chooser.addOption("No Auto", m_noAuto); m_choosers.add(chooser); Shuffleboard.getTab("Auto Chooser") From e43c44a28c46f65944b14bc40bd137b954ef1f0b Mon Sep 17 00:00:00 2001 From: 66945 <54561572+66945@users.noreply.github.com> Date: Sat, 25 Feb 2023 21:14:25 -0700 Subject: [PATCH 39/40] added builder pattern instead of object pool --- src/main/java/frc4388/robot/RobotContainer.java | 8 +++----- .../frc4388/robot/commands/PlaybackChooser.java | 16 ++++++++-------- 2 files changed, 11 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 1644231..0267c8b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -105,8 +105,9 @@ public class RobotContainer { // chooser.addOption("Taxi", taxi); - playbackChooser = new PlaybackChooser(m_robotSwerveDrive, - "Balance", new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); + playbackChooser = new PlaybackChooser(m_robotSwerveDrive) + .addOption("Balance", new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)) + .buildDisplay(); } @@ -137,9 +138,6 @@ public class RobotContainer { "Blue1Path.txt")) .onFalse(new InstantCommand()); - // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) - // .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt")); - // * Operator Buttons } diff --git a/src/main/java/frc4388/robot/commands/PlaybackChooser.java b/src/main/java/frc4388/robot/commands/PlaybackChooser.java index 1a94efc..a0ae141 100644 --- a/src/main/java/frc4388/robot/commands/PlaybackChooser.java +++ b/src/main/java/frc4388/robot/commands/PlaybackChooser.java @@ -15,7 +15,7 @@ import frc4388.robot.subsystems.SwerveDrive; public class PlaybackChooser { private final ArrayList> m_choosers = new ArrayList<>(); - private final SendableChooser m_playback; + private SendableChooser m_playback = null; private final HashMap m_commandPool = new HashMap<>(); private final File m_dir = new File("/home/lvuser/autos/"); @@ -26,21 +26,21 @@ public class PlaybackChooser { public PlaybackChooser(SwerveDrive swerve, Object... pool) { m_swerve = swerve; + } - for (int i = 0; i < pool.length; i += 2) { - if (!(pool[i] instanceof String) || !(pool[i + 1] instanceof Command)) { - throw new RuntimeException("Need (string, command)"); - } - - m_commandPool.put((String) pool[i], (Command) pool[i + 1]); - } + public PlaybackChooser addOption(String name, Command option) { + m_commandPool.put(name, option); + return this; + } + public PlaybackChooser buildDisplay() { appendCommand(); m_playback = m_choosers.get(0); Shuffleboard.getTab("Auto Chooser") .add("Add Sequence", new InstantCommand(() -> appendCommand())) .withPosition(4, 0); + return this; } // This will be bound to a button for the time being From 10dca06a765965e0931804e806dc5d0b4d8dd527 Mon Sep 17 00:00:00 2001 From: 66945 <54561572+66945@users.noreply.github.com> Date: Mon, 27 Feb 2023 09:28:44 -0700 Subject: [PATCH 40/40] made UI work right --- src/main/java/frc4388/robot/commands/PlaybackChooser.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/commands/PlaybackChooser.java b/src/main/java/frc4388/robot/commands/PlaybackChooser.java index a0ae141..a86c38e 100644 --- a/src/main/java/frc4388/robot/commands/PlaybackChooser.java +++ b/src/main/java/frc4388/robot/commands/PlaybackChooser.java @@ -59,7 +59,7 @@ public class PlaybackChooser { Shuffleboard.getTab("Auto Chooser") .add("Command: " + m_choosers.size(), chooser) .withSize(4, 1) - .withPosition(0, 0) + .withPosition(0, m_choosers.size() - 1) .withWidget(BuiltInWidgets.kSplitButtonChooser); }