Merge branch 'master' into autonomous

This commit is contained in:
Aarav Shah
2023-02-17 19:53:58 -07:00
committed by GitHub
8 changed files with 1165 additions and 61 deletions
+5 -5
View File
@@ -66,11 +66,11 @@ public final class Constants {
public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11.0; // TODO: find the actual value public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11.0; // TODO: find the actual value
public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 1.0; // TODO: find the actual value public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 1.0; // TODO: find the actual value
public static final double MOTOR_REV_PER_WHEEL_REV = 6.12; // TODO: find the actual value public static final double MOTOR_REV_PER_WHEEL_REV = 5.12;
public static final double MOTOR_REV_PER_STEER_REV = 12.8; // TODO: find the actual value public static final double MOTOR_REV_PER_STEER_REV = 12.8;
public static final double TICKS_PER_MOTOR_REV = 2048; public static final double TICKS_PER_MOTOR_REV = 2048;
public static final double WHEEL_DIAMETER_INCHES = 4.0; // TODO: the actual value public static final double WHEEL_DIAMETER_INCHES = 3.9;
public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI; public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI;
public static final double WHEEL_REV_PER_MOTOR_REV = 1 / MOTOR_REV_PER_WHEEL_REV; public static final double WHEEL_REV_PER_MOTOR_REV = 1 / MOTOR_REV_PER_WHEEL_REV;
@@ -108,8 +108,8 @@ public final class Constants {
public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; // 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 // dimensions
public static final double WIDTH = 18.5; // TODO: find the actual value public static final double WIDTH = 18.5;
public static final double HEIGHT = 18.5; // TODO: find the actual value public static final double HEIGHT = 18.5;
public static final double HALF_WIDTH = WIDTH / 2.d; public static final double HALF_WIDTH = WIDTH / 2.d;
public static final double HALF_HEIGHT = HEIGHT / 2.d; public static final double HALF_HEIGHT = HEIGHT / 2.d;
+3
View File
@@ -74,6 +74,7 @@ public class Robot extends TimedRobot {
*/ */
@Override @Override
public void autonomousInit() { public void autonomousInit() {
m_robotContainer.m_robotSwerveDrive.resetGyro();
m_autonomousCommand = m_robotContainer.getAutonomousCommand(); m_autonomousCommand = m_robotContainer.getAutonomousCommand();
// schedule the autonomous command (example) // schedule the autonomous command (example)
@@ -92,6 +93,7 @@ public class Robot extends TimedRobot {
@Override @Override
public void teleopInit() { public void teleopInit() {
m_robotContainer.m_robotSwerveDrive.resetGyro();
// This makes sure that the autonomous stops running when // This makes sure that the autonomous stops running when
// teleop starts running. If you want the autonomous to // teleop starts running. If you want the autonomous to
// continue until interrupted by another command, remove // continue until interrupted by another command, remove
@@ -99,6 +101,7 @@ public class Robot extends TimedRobot {
if (m_autonomousCommand != null) { if (m_autonomousCommand != null) {
m_autonomousCommand.cancel(); m_autonomousCommand.cancel();
} }
m_robotContainer.m_robotSwerveDrive.resetGyro();
m_robotTime.startMatchTime(); m_robotTime.startMatchTime();
} }
+28 -51
View File
@@ -34,9 +34,10 @@ import frc4388.robot.Constants.*;
import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants; import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants;
import frc4388.robot.Constants.SwerveDriveConstants.PIDConstants; import frc4388.robot.Constants.SwerveDriveConstants.PIDConstants;
import frc4388.robot.commands.AutoBalance; import frc4388.robot.commands.AutoBalance;
import frc4388.robot.commands.JoystickPlayback;
import frc4388.robot.commands.JoystickRecorder;
import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.SwerveDrive;
import frc4388.utility.controller.DeadbandedXboxController; import frc4388.utility.controller.DeadbandedXboxController;
import frc4388.utility.controller.IHandController;
import frc4388.utility.controller.XboxController; import frc4388.utility.controller.XboxController;
/** /**
@@ -51,14 +52,13 @@ public class RobotContainer {
public final RobotMap m_robotMap = new RobotMap(); public final RobotMap m_robotMap = new RobotMap();
/* Subsystems */ /* Subsystems */
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.rightFront, m_robotMap.leftBack, m_robotMap.rightBack, m_robotMap.gyro); public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront,
// private final LED m_robotLED = new LED(m_robotMap.LEDController); m_robotMap.rightFront,
m_robotMap.leftBack,
m_robotMap.rightBack,
m_robotMap.gyro);
/* Controllers */ /* 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_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
@@ -68,15 +68,13 @@ public class RobotContainer {
public RobotContainer() { public RobotContainer() {
configureButtonBindings(); configureButtonBindings();
/* Default Commands */ // * Default Commands
m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(),
m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(), getDeadbandedDriverController().getRight(), true) getDeadbandedDriverController().getRight(),
, m_robotSwerveDrive).withName("SwerveDrive DefaultCommand")); true);
}, m_robotSwerveDrive)
// m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); .withName("SwerveDrive DefaultCommand"));
} }
@@ -87,8 +85,7 @@ public class RobotContainer {
* passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
*/ */
private void configureButtonBindings() { private void configureButtonBindings() {
/* Driver Buttons */ // * Driver Buttons
new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive));
@@ -98,11 +95,19 @@ public class RobotContainer {
new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
.onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); .onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive));
// /* Operator Buttons */ new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
// // interrupt button .whileTrue(new JoystickRecorder(m_robotSwerveDrive,
// new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) () -> getDeadbandedDriverController().getLeftX(),
// .onTrue(new InstantCommand()); () -> getDeadbandedDriverController().getLeftY(),
() -> getDeadbandedDriverController().getRightX(),
() -> getDeadbandedDriverController().getRightY()))
.onFalse(new InstantCommand());
new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
.onTrue(new JoystickPlayback(m_robotSwerveDrive));
// * Operator Buttons
} }
/** /**
@@ -177,13 +182,6 @@ public class RobotContainer {
new InstantCommand(() -> m_robotSwerveDrive.stopModules())); new InstantCommand(() -> m_robotSwerveDrive.stopModules()));
} }
/**
* Add your docs here.
*/
// public IHandController getDriverController() {
// return m_driverXbox;
// }
public DeadbandedXboxController getDeadbandedDriverController() { public DeadbandedXboxController getDeadbandedDriverController() {
return this.m_driverXbox; return this.m_driverXbox;
} }
@@ -191,25 +189,4 @@ public class RobotContainer {
public DeadbandedXboxController getDeadbandedOperatorController() { public DeadbandedXboxController getDeadbandedOperatorController() {
return this.m_operatorXbox; 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();
// }
} }
@@ -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
@@ -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<TimedOutput> 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;
}
}
@@ -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<Double> leftX;
public final Supplier<Double> leftY;
public final Supplier<Double> rightX;
public final Supplier<Double> rightY;
public final ArrayList<TimedOutput> outputs = new ArrayList<>();
private long startTime = -1;
/** Creates a new JoystickRecorder. */
public JoystickRecorder(SwerveDrive swerve, Supplier<Double> leftX, Supplier<Double> leftY,
Supplier<Double> rightX, Supplier<Double> 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;
}
}
@@ -205,6 +205,12 @@ public class SwerveDrive extends SubsystemBase {
public void resetPoseEstimator() { public void resetPoseEstimator() {
setPoseEstimator(new Pose2d()); setPoseEstimator(new Pose2d());
} }
public void stopModules() {
for (SwerveModule module : this.modules) {
module.stop();
}
}
/** /**
* Resets the odometry of the SwerveDrive to 0. * Resets the odometry of the SwerveDrive to 0.
@@ -221,14 +227,17 @@ public class SwerveDrive extends SubsystemBase {
@Override @Override
public void periodic() { public void periodic() {
// This method will be called once per scheduler run // This method will be called once per scheduler run
// updateOdometry(); // updateOdometry();
updatePoseEstimator(); updatePoseEstimator();
// SmartDashboard.putNumber("Odo X", getOdometry().getX()); SmartDashboard.putNumber("Odo X (ft)", Units.metersToFeet(this.getOdometry().getX()));
// SmartDashboard.putNumber("Odo Y", getOdometry().getY()); SmartDashboard.putNumber("Odo Y (ft)", Units.metersToFeet(this.getOdometry().getY()));
// SmartDashboard.putNumber("Odo Theta", getOdometry().getRotation().getDegrees()); SmartDashboard.putNumber("Odo Theta", this.getOdometry().getRotation().getDegrees());
// SmartDashboard.putNumber("Gyro Angle", getGyroAngle()); SmartDashboard.putNumber("Gyro Angle", getGyroAngle());
SmartDashboard.putNumber("rotTarget", this.rotTarget.getDegrees());
} }
/** /**
@@ -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;
}
}