Add roller substym

.pathplanner/settings.json

@@ -0,0 +1,12 @@
+{
+  "robotWidth": 0.711,
+  "robotLength": 0.711,
+  "holonomicMode": false,
+  "pathFolders": [],
+  "autoFolders": [],
+  "defaultMaxVel": 3.0,
+  "defaultMaxAccel": 3.0,
+  "defaultMaxAngVel": 540.0,
+  "defaultMaxAngAccel": 720.0,
+  "maxModuleSpeed": 4.5
+}

src/main/deploy/pathplanner/autos/one cube.auto

@@ -0,0 +1,37 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 0.59,
+      "y": 3.73
+    },
+    "rotation": -90.0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Start_To_Perimiter (1)"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "perimiter_To_Back_of_Cube(2)"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Back of Cube into home(3)"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/navgrid.json

@@ -0,0 +1 @@
+{"field_size":{"x":16.54,"y":8.21},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true],[true,true,true,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}

src/main/deploy/pathplanner/paths/Back of Cube into home(3).path

@@ -0,0 +1,49 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.081007635872117,
+        "y": 2.146511524671921
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.859055001204621,
+        "y": 2.146511524671921
+      },
+      "isLocked": false,
+      "linkedName": "behind cube#1"
+    },
+    {
+      "anchor": {
+        "x": 0.8429091763679902,
+        "y": 2.0946366066713966
+      },
+      "prevControl": {
+        "x": 2.0875571473823786,
+        "y": 2.0946366066713966
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "home1"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 3.0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": true,
+  "folder": null,
+  "previewStartingState": null,
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/Cube 1 to perimiter (4).path

@@ -0,0 +1,65 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.8429091763679902,
+        "y": 2.0946366066713966
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.766251005253288,
+        "y": 3.648322422343152
+      },
+      "isLocked": false,
+      "linkedName": "home1"
+    },
+    {
+      "anchor": {
+        "x": 5.074084880795034,
+        "y": 3.616239874583321
+      },
+      "prevControl": {
+        "x": 4.572779876293277,
+        "y": 3.445484258286135
+      },
+      "nextControl": {
+        "x": 5.215686197132219,
+        "y": 3.6644724272658546
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.196224237992037,
+        "y": 3.826728224988743
+      },
+      "prevControl": {
+        "x": 4.928550983072929,
+        "y": 3.826728224988743
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 3.0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": null,
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/Start_To_Perimiter (1).path

@@ -0,0 +1,49 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.59,
+        "y": 3.729931640783178
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 0.5900000000000002,
+        "y": 2.4670756793249056
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.098419859252312,
+        "y": 2.623962929928345
+      },
+      "prevControl": {
+        "x": 0.7683766317023171,
+        "y": 2.657898350931721
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "The actual end of perimiter"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 3.0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": null,
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/perimiter_To_Back_of_Cube(2).path

@@ -0,0 +1,49 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.098419859252312,
+        "y": 2.623962929928345
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.0985432489528355,
+        "y": 2.386063247713733
+      },
+      "isLocked": false,
+      "linkedName": "The actual end of perimiter"
+    },
+    {
+      "anchor": {
+        "x": 5.081007635872117,
+        "y": 2.146511524671921
+      },
+      "prevControl": {
+        "x": 4.6996176702990855,
+        "y": 2.146511524671921
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "behind cube#1"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": null,
+  "useDefaultConstraints": false
+}

src/main/java/frc/robot/RobotContainer.java

@@ -22,6 +22,8 @@
 import frc.robot.subsystems.drive.DriveIOReal;
 import frc.robot.subsystems.drive.GyroIO;
 import frc.robot.subsystems.drive.GyroIOPigeon2;
+import frc.robot.subsystems.drive.rollers.RollerIO;
+import frc.robot.subsystems.drive.rollers.Rollers;
 import frc.robot.util.Alert;
 import frc.robot.util.Alert.AlertType;
 import frc.robot.util.PoseManager;
@@ -36,10 +38,10 @@
 public class RobotContainer {
   // Subsystems
   private final Drive drive;
-
+  public CommandXboxController controller;
   // Pose Manager
   private final PoseManager poseManager = new PoseManager();
-
+private Rollers rollerIO;
   // Controller
   private final CommandXboxController driver = new CommandXboxController(0);
 
@@ -94,6 +96,10 @@ private void configureButtonBindings() {
 
     // Driver controls
 
+    controller.x().whileTrue(rollerIO.run_Flywheel_Full_Speed());
+      controller.y().whileTrue(rollerIO.run_Flywheel_Half_Speed());
+    controller.a().whileTrue(rollerIO.run_Flywheel_x_Speed(5, 4));
+    controller.b().onTrue(rollerIO.Stop_Fly_Wheels());
   }
 
   /** Updates the alerts for disconnected controllers. */

src/main/java/frc/robot/subsystems/drive/Drive.java

@@ -8,6 +8,8 @@
 import frc.robot.util.PoseManager;
 import java.util.function.Supplier;
 import org.littletonrobotics.junction.Logger;
+import frc.robot.util.loggedShuffleboardClasses.*;
+import frc.robot.util.loggedShuffleboardClasses.DriveIOInputsAutoLogged;
 
 public class Drive extends SubsystemBase {
   private final DriveIO io;

src/main/java/frc/robot/subsystems/drive/DriveIOSim.java

@@ -33,7 +33,6 @@ public class DriveIOSim implements DriveIO {
           // l and r position: 0.005 m
           VecBuilder.fill(0.001, 0.001, 0.001, 0.1, 0.1, 0.005, 0.005));
 
-
   private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 3);
 
   private final PIDController m_leftPIDController = new PIDController(8.5, 0, 0);
@@ -56,7 +55,8 @@ public DriveIOSim() {
     m_leftLeader.addFollower(m_leftFollower);
     m_rightLeader.addFollower(m_rightFollower);
   }
-   @Override
+
+  @Override
   public void updateInputs(DriveIOInputs inputs) {
     // TODO: we will have encoders
 
@@ -67,15 +67,14 @@ public void stopDriveTrain() {
     driveTrain.setInputs(0, 0);
   }
 
-
   @Override
   public void arcadeDrive(double xSpeed, double omegaRotation) {
 
     setSpeeds(m_kinematics.toWheelSpeeds(new ChassisSpeeds(xSpeed, 0, omegaRotation)));
   }
 
   /** Sets speeds to the drivetrain motors. */
-  //TODO there is a unit's conflict
+  // TODO there is a unit's conflict
   private void setSpeeds(DifferentialDriveWheelSpeeds speeds) {
     final double leftFeedforward =
         m_feedforward.calculate(MetersPerSecond.of(speeds.leftMetersPerSecond)).in(Volts);