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/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index a3690d9..731275b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -7,16 +7,16 @@ package frc4388.robot; -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.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; import frc4388.utility.controller.XboxController; /** @@ -31,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); @@ -48,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")); } @@ -65,22 +64,28 @@ 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)); - - // /* Operator Buttons */ - // // interrupt button - // new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) - // .onTrue(new InstantCommand()); + + new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) + .whileTrue(new JoystickRecorder(m_robotSwerveDrive, + () -> getDeadbandedDriverController().getLeftX(), + () -> getDeadbandedDriverController().getLeftY(), + () -> getDeadbandedDriverController().getRightX(), + () -> getDeadbandedDriverController().getRightY())) + .onFalse(new InstantCommand()); + + new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) + .onTrue(new JoystickPlayback(m_robotSwerveDrive)); + + // * Operator Buttons } /** @@ -89,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; } @@ -107,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/JoystickInputs.txt b/src/main/java/frc4388/robot/commands/JoystickInputs.txt new file mode 100644 index 0000000..b503e48 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/JoystickInputs.txt @@ -0,0 +1,887 @@ +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 new file mode 100644 index 0000000..6961d09 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/JoystickPlayback.java @@ -0,0 +1,126 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands; + +import java.io.File; +import java.io.FileNotFoundException; +import java.util.ArrayList; +import java.util.Scanner; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.utility.UtilityStructs.TimedOutput; + +public class JoystickPlayback extends CommandBase { + private final SwerveDrive swerve; + private Scanner input; + private final ArrayList outputs = new ArrayList<>(); + private int counter = 0; + private long startTime = 0; + private long playbackTime = 0; + private int lastIndex; + private boolean m_finished = false; // ! find a better way + + /** Creates a new JoystickPlayback. */ + public JoystickPlayback(SwerveDrive swerve) { + this.swerve = swerve; + addRequirements(this.swerve); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + startTime = System.currentTimeMillis(); + playbackTime = 0; + lastIndex = 0; + try { + input = new Scanner(new File("/home/lvuser/JoystickInputs.txt")); + + String line = ""; + while (input.hasNextLine()) { + line = 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.timedOffset = Long.parseLong(values[4]); + + outputs.add(out); + } + + input.close(); + } catch (FileNotFoundException e) { + e.printStackTrace(); + } + + System.out.println("STARTING PLAYBACK"); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if (counter == 0) { + startTime = System.currentTimeMillis(); + playbackTime = 0; + } else { + playbackTime = System.currentTimeMillis() - startTime; + } + + // skip to reasonable time frame + // too tired to write comment: ask daniel thomas; it goes to the thing until it's bigger than the other thing + { + int i = lastIndex == 0 ? 1 : lastIndex; + while (i < outputs.size() && outputs.get(i).timedOffset < playbackTime) { + i++; + } + + if (i >= outputs.size()) { + m_finished = true; // ! kind of a hack + return; + } + lastIndex = i; + } + + TimedOutput lastOut = outputs.get(lastIndex - 1); + TimedOutput out = outputs.get(lastIndex); + + double deltaTime = out.timedOffset - lastOut.timedOffset; + double playbackDelta = playbackTime - lastOut.timedOffset; + + double lerpLX = lastOut.leftX + (out.leftX - lastOut.leftX) * (playbackDelta / deltaTime); + double lerpLY = lastOut.leftY + (out.leftY - lastOut.leftY) * (playbackDelta / deltaTime); + double lerpRX = lastOut.rightX + (out.rightX - lastOut.rightX) * (playbackDelta / deltaTime); + double lerpRY = lastOut.rightY + (out.rightY - lastOut.rightY) * (playbackDelta / deltaTime); + + // this.swerve.driveWithInput(new Translation2d(out.leftX, out.leftY), + // new Translation2d(out.rightX, out.rightY), + // true); + + this.swerve.driveWithInput( new Translation2d(lerpLX, lerpLY), + new Translation2d(lerpRX, lerpRY), + true); + + System.out.println("PLAYING"); + counter++; + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + input.close(); + swerve.stopModules(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return m_finished; + } +} diff --git a/src/main/java/frc4388/robot/commands/JoystickRecorder.java b/src/main/java/frc4388/robot/commands/JoystickRecorder.java new file mode 100644 index 0000000..2b3fd24 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/JoystickRecorder.java @@ -0,0 +1,90 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands; + +import java.io.File; +import java.io.IOException; +import java.io.PrintWriter; +import java.util.ArrayList; +import java.util.function.Supplier; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.utility.UtilityStructs.TimedOutput; + +public class JoystickRecorder extends CommandBase { + public final SwerveDrive swerve; + + public final Supplier leftX; + public final Supplier leftY; + public final Supplier rightX; + public final Supplier rightY; + 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) + { + this.swerve = swerve; + this.leftX = leftX; + this.leftY = leftY; + this.rightX = rightX; + this.rightY = rightY; + + addRequirements(this.swerve); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + this.startTime = System.currentTimeMillis(); + + outputs.add(new TimedOutput()); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + var inputs = new TimedOutput(); + inputs.leftX = leftX.get(); + inputs.leftY = leftY.get(); + inputs.rightX = rightX.get(); + inputs.rightY = rightY.get(); + inputs.timedOffset = System.currentTimeMillis() - startTime; + + outputs.add(inputs); + + swerve.driveWithInput(new Translation2d(inputs.leftX, inputs.leftY), + new Translation2d(inputs.rightX, inputs.rightY), + true); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + File output = new File("/home/lvuser/JoystickInputs.txt"); + + try (PrintWriter writer = new PrintWriter(output)) { + for (var input : outputs) { + writer.println( input.leftX + "," + input.leftY + "," + + input.rightX + "," + input.rightY + "," + + input.timedOffset); + } + + writer.close(); + } catch (IOException e) { + e.printStackTrace(); + } + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc4388/robot/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. 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; + } +}