Path planner -> dev

.pathplanner/settings.json

@@ -0,0 +1,7 @@
+{
+  "robotWidth": 0.75,
+  "robotLength": 0.79,
+  "holonomicMode": true,
+  "generateJSON": true,
+  "generateCSV": false
+}

src/main/deploy/pathplanner/1 meter.path

@@ -0,0 +1,58 @@
+{
+  "waypoints": [
+    {
+      "anchorPoint": {
+        "x": 5.0,
+        "y": 5.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.638097093498402,
+        "y": 5.0
+      },
+      "holonomicAngle": 0.0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 8.0,
+        "y": 5.0
+      },
+      "prevControl": {
+        "x": 6.096443920933169,
+        "y": 5.064827693394764
+      },
+      "nextControl": null,
+      "holonomicAngle": 0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [
+          "StopShooter"
+        ],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    }
+  ],
+  "markers": [
+    {
+      "position": 0.0,
+      "names": [
+        "ShooterByRPM2000"
+      ]
+    }
+  ]
+}

src/main/deploy/pathplanner/3 ball auto.path

@@ -1 +1,90 @@
-{"waypoints":[{"anchorPoint":{"x":6.504985161869057,"y":2.5580101793114975},"prevControl":null,"nextControl":{"x":5.7642039334711574,"y":2.199194271806266},"holonomicAngle":45.9391909457357,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.104445651929278,"y":1.898251897769618},"prevControl":{"x":5.7988654057153415,"y":2.211122336288613},"nextControl":{"x":4.051147342801014,"y":1.4236889233272139},"holonomicAngle":19.311856564352684,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":7.373088163897846,"y":2.0255736714005077},"prevControl":{"x":6.493410455175339,"y":1.5857348170392553},"nextControl":null,"holonomicAngle":73.03385189146377,"isReversal":false,"velOverride":null,"isLocked":false}]}
+{
+  "waypoints": [
+    {
+      "anchorPoint": {
+        "x": 6.249290884748916,
+        "y": 2.5495209491859923
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.869827301614257,
+        "y": 2.288639735780914
+      },
+      "holonomicAngle": -156.57130719125453,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 1.5
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 5.620804325182138,
+        "y": 2.241206787889081
+      },
+      "prevControl": {
+        "x": 5.751244931884676,
+        "y": 2.3123562097268304
+      },
+      "nextControl": {
+        "x": 4.754995772136629,
+        "y": 1.7689475771369774
+      },
+      "holonomicAngle": -132.35745470593506,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 7.731570506368677,
+        "y": 2.8815515844288186
+      },
+      "prevControl": {
+        "x": 6.178141462911166,
+        "y": 1.909176152646255
+      },
+      "nextControl": null,
+      "holonomicAngle": -109.8,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [
+          "FullShoot2000"
+        ],
+        "executionBehavior": "parallel",
+        "waitBehavior": "deadline",
+        "waitTime": 4.0
+      }
+    }
+  ],
+  "markers": [
+    {
+      "position": 1.4109090909090933,
+      "names": [
+        "StopRoller",
+        "ShooterByRPM2000"
+      ]
+    },
+    {
+      "position": 0.0,
+      "names": [
+        "ExtendAndCollect"
+      ]
+    }
+  ]
+}

src/main/deploy/pathplanner/new.path

@@ -0,0 +1,74 @@
+{
+  "waypoints": [
+    {
+      "anchorPoint": {
+        "x": 4.541704760642951,
+        "y": 2.3004979727538712
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.541704760642951,
+        "y": 3.4280286313658017
+      },
+      "holonomicAngle": -90.0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 4.541704760642951,
+        "y": 4.434980627886328
+      },
+      "prevControl": {
+        "x": 4.541704760642951,
+        "y": 3.3203063524282665
+      },
+      "nextControl": {
+        "x": 4.541704760642951,
+        "y": 5.54965490334439
+      },
+      "holonomicAngle": -90.0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 5.881685538587215,
+        "y": 5.680095510046928
+      },
+      "prevControl": {
+        "x": 5.087183661399023,
+        "y": 5.680095510046928
+      },
+      "nextControl": null,
+      "holonomicAngle": 179.40934278535332,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    }
+  ],
+  "markers": []
+}

src/main/java/edu/greenblitz/pegasus/OI.java

@@ -1,5 +1,6 @@
 package edu.greenblitz.pegasus;
 
+import edu.greenblitz.pegasus.commands.auto.PathFollowerBuilder;
 import edu.greenblitz.pegasus.commands.funnel.RunFunnel;
 import edu.greenblitz.pegasus.commands.intake.extender.ToggleRoller;
 import edu.greenblitz.pegasus.commands.intake.roller.RunRoller;
@@ -9,6 +10,7 @@
 import edu.greenblitz.pegasus.commands.shooter.ShooterEvacuate;
 import edu.greenblitz.pegasus.commands.shooter.StopShooter;
 import edu.greenblitz.pegasus.commands.swerve.CombineJoystickMovement;
+import edu.greenblitz.pegasus.commands.swerve.calibration.CalibrateLampreyByNeo;
 import edu.greenblitz.pegasus.subsystems.Intake;
 import edu.greenblitz.pegasus.subsystems.swerve.SwerveChassis;
 import edu.greenblitz.pegasus.utils.DigitalInputMap;
@@ -25,10 +27,12 @@ public class OI {
     private final SmartJoystick mainJoystick;
 
     private final SmartJoystick secondJoystick;
+    private final SmartJoystick debugJoystick;
 
     private OI() {
         mainJoystick = new SmartJoystick(RobotMap.Pegasus.Joystick.MAIN, 0.1);
         secondJoystick = new SmartJoystick(RobotMap.Pegasus.Joystick.SECOND, 0.2);
+        debugJoystick = new SmartJoystick(3, 0.2);
         initButtons();
 
     }
@@ -50,9 +54,11 @@ public static void disableHandling() {
 
     private void initButtons() {
         SwerveChassis.getInstance().setDefaultCommand(new CombineJoystickMovement(mainJoystick, false));
-
+debugJoystick.A.whenPressed(new CalibrateLampreyByNeo(SwerveChassis.Module.BACK_LEFT));
         mainJoystick.Y.whenPressed(new InstantCommand(() -> SwerveChassis.getInstance().resetChassisAngle()));
 
+        mainJoystick.X.whenPressed(PathFollowerBuilder.getInstance().followPath("new"));
+
         mainJoystick.POV_UP.whenPressed(new InstantCommand(() -> SwerveChassis.getInstance().resetAllEncoders()));
 
         mainJoystick.R1.whenHeld(new StartEndCommand(() -> Intake.getInstance().getExtender().retract(),

src/main/java/edu/greenblitz/pegasus/Robot.java

@@ -1,9 +1,14 @@
 package edu.greenblitz.pegasus;
 
+import edu.greenblitz.pegasus.commands.auto.PathFollowerBuilder;
+import edu.greenblitz.pegasus.commands.auto.PathPlannerCommand;
 import edu.greenblitz.pegasus.commands.auto.Taxi;
 import edu.greenblitz.pegasus.subsystems.*;
 import edu.greenblitz.pegasus.subsystems.swerve.SwerveChassis;
+import edu.greenblitz.pegasus.utils.AutoSelector;
 import edu.greenblitz.pegasus.utils.DigitalInputMap;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
@@ -20,14 +25,14 @@ public void robotInit() {
 		Intake.getInstance();
 		Indexing.getInstance();
 		Shooter.create(RobotMap.Pegasus.Shooter.ShooterMotor.PORT_LEADER);
+		AutoSelector.getInstance();
 
 		//todo add voltage compensation
 		//swerve
 
 		SwerveChassis.getInstance().resetChassisAngle();
 		SwerveChassis.getInstance().resetAllEncoders();
 		SmartDashboard.putNumber("auto number", 1);
-
 		OI.getInstance();
 	}
 
@@ -63,8 +68,8 @@ public void autonomousInit() {
 		//resets encoders
 		SwerveChassis.getInstance().resetAllEncoders();
 		SwerveChassis.getInstance().resetLocalizer();
-				(new Taxi(2.5, 2)).schedule(); //THIS   //todo sendablechooser for choosing auto
-
+		PathFollowerBuilder.getInstance().followPath("1 meter").schedule();
+//		SmartDashboard.putString("dood", AutoSelector.getInstance().getSelectedAuto());
 
 	}
 

src/main/java/edu/greenblitz/pegasus/RobotMap.java

@@ -105,7 +105,7 @@ public static class DigitalInputMap {
 
 		public static class Swerve {
 
-			public static final Pose2d initialRobotPosition = new Pose2d(0, 0, new Rotation2d(0));
+			public static final Pose2d initialRobotPosition = new Pose2d(8, 8, new Rotation2d(0));
 			public static final double WHEEL_CIRC = 0.0517 * 2 * Math.PI; //very accurate right now
 
 			public static final double ANG_GEAR_RATIO = 1 / 6.0; //todo maybe 6.0 /1?   input/output
@@ -118,6 +118,7 @@ public static class Swerve {
 			public static final double angleTicksToWheelToRPM = Swerve.ANG_GEAR_RATIO * motors.SPARKMAX_VELOCITY_UNITS_TO_RPM;
 			public static final double linTicksToWheelToRPM = Swerve.LIN_GEAR_RATIO * motors.SPARKMAX_VELOCITY_UNITS_TO_RPM;
 
+			public static final double linRPMToMetersPerSecond = WHEEL_CIRC / 60;
 			public static final double angleTicksToRadians = Swerve.ANG_GEAR_RATIO * motors.SPARKMAX_TICKS_PER_RADIAN;
 
 			public static final double NEO_PHYSICAL_TICKS_TO_RADIANS = angleTicksToRadians / 42; //do not use unless you understand the meaning
@@ -139,7 +140,7 @@ public static class Swerve {
 
 			};
 			//TODO: calibrate GOOD PID
-			public static final PIDObject angPID = new PIDObject().withKp(0.5).withKd(10).withMaxPower(0.8);
+			public static final PIDObject angPID = new PIDObject().withKp(0.5).withMaxPower(0.5);
 			public static final PIDObject linPID = new PIDObject().withKp(0.0003).withMaxPower(0.5);
 			public static final PIDObject rotationPID = new PIDObject().withKp(0.5).withKi(0).withKd(0).withFF(0.1);