fixes

forgot to commit

--Gabe

src/main/java/frc/robot/Constants.java

@@ -5,19 +5,13 @@
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
-import frc.robot.positioning.Pose;
-import frc.robot.positioning.Team937TrajectoryUtils;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.trajectory.Trajectory;
 import edu.wpi.first.math.trajectory.TrajectoryConfig;
 import edu.wpi.first.math.trajectory.TrajectoryGenerator;
 import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.Filesystem;
-import edu.wpi.first.math.trajectory.Trajectory;
-import frc.robot.positioning.Team937TrajectoryUtils;
-
-
 import frc.robot.positioning.Pose;
+import frc.robot.positioning.Team937TrajectoryUtils;
 import java.util.ArrayList;
 import java.util.List;
 
@@ -73,7 +67,7 @@ public static class Drive {
         new Translation2d(Units.feetToMeters(10 + (7 / 8)), Units.feetToMeters(8 + (5 / 8)));
      */
 
-    public static final double OPEN_LOOP_RAMP_RATE = 1.0;
+    public static final double OPEN_LOOP_RAMP_RATE = 0.5;
 
     public static final double TRACK_WIDTH = Units.feetToMeters(20.5);
 
@@ -210,9 +204,8 @@ public static class Arm {
     public static class Intake {
       public static final int ID_TALON_ARM_CLAW = 0; /* This is actually a Spark */
       public static final double SPEED_ARM_CLAW = 1;
-      // TODO: TODO: TODO: this is prob wrong so this needs to be fixed
-      public static final double CUBE_IN_CONE_OUT_SPEED = 0.5;
-      public static final double CUBE_OUT_CONE_IN_SPEED = -0.5;
+      public static final double CUBE_IN_CONE_OUT_SPEED = 0.85;
+      public static final double CUBE_OUT_CONE_IN_SPEED = -0.85;
     }
 
     public static class ShoulderPID {
@@ -265,12 +258,13 @@ public static class Limits {
   /** Holds constants for things not related to the robot */
   public static class Game {
     public static class Field {
-      
+
       /**
        * Half the length of the field relative to the origin. Positive for Max, Negative for Min.
        * Measured in cm.
        */
       public static final int FIELD_X = 1654 / 2;
+
       /**
        * Half the width of the field relative to the origin. Positive for Max, Negative for Min.
        * Measured in cm.
@@ -358,67 +352,32 @@ public static class Cube {
       }
     }
   }
-  /** IDs for the controller buttons. */
-  public static class ContollerButtons {
-    public static final int A_NUMBER = 1;
-    public static final int B_NUMBER = 2;
-    public static final int X_NUMBER = 3;
-    public static final int Y_NUMBER = 4;
-    public static final int LEFT_BUMPER_NUMBER = 5;
-    public static final int RIGHT_BUMPER_NUMBER = 6;
-    public static final int BACK_NUMBER = 7;
-    public static final int START_NUMBER = 8;
-    public static final int LEFT_STICK_NUMBER = 9;
-    public static final int RIGHT_STICK_NUMBER = 10;
-  }
-
+
   public static class Trajectories {
-    public static final Trajectory LOWER_COMMUNITY_TO_LOADING_ZONE = Team937TrajectoryUtils.generateTrajectory("/pathplanner/JSON/LC2LZ.wpilib.json");
-    public static final Trajectory LOADING_ZONE_TO_LOWER_COMMUNITY = Team937TrajectoryUtils.generateTrajectory("/pathplanner/JSON/LZ2LC.wpilib.json");
-    public static final Trajectory LOADING_ZONE_TO_UPPER_COMMUNITY = Team937TrajectoryUtils.generateTrajectory("/pathplanner/JSON/LZ2UC.wpilib.json");
-    public static final Trajectory UPPER_COMMUNITY_TO_LOADING_ZONE = Team937TrajectoryUtils.generateTrajectory("/pathplanner/JSON/UC2LZ.wpilib.json");
+    public static final Trajectory LOWER_COMMUNITY_TO_LOADING_ZONE =
+        Team937TrajectoryUtils.generateTrajectory("/pathplanner/JSON/LC2LZ.wpilib.json");
+    public static final Trajectory LOADING_ZONE_TO_LOWER_COMMUNITY =
+        Team937TrajectoryUtils.generateTrajectory("/pathplanner/JSON/LZ2LC.wpilib.json");
+    public static final Trajectory LOADING_ZONE_TO_UPPER_COMMUNITY =
+        Team937TrajectoryUtils.generateTrajectory("/pathplanner/JSON/LZ2UC.wpilib.json");
+    public static final Trajectory UPPER_COMMUNITY_TO_LOADING_ZONE =
+        Team937TrajectoryUtils.generateTrajectory("/pathplanner/JSON/UC2LZ.wpilib.json");
   }
 
   /**
    * General constants for the AStar Subsystem. Primarily used by {@link
    * frc.robot.positioning.AStar}.
    */
   public static class AStar {
-    /* 
+    /*
      * Values are *2 as they are accessing half the length of the field
      * Values are /10 as they are in decimeters (/100 for centi and so on)
      */
-    /**
-     * The full length of the field.
-     * Measured in decimeters.
-     */
+    /** The full length of the field. Measured in decimeters. */
     public static final int FIELD_X = Game.Field.FIELD_X * 2 / 10;
-    /**
-     * The full length of the field.
-     * Measured in decimeters.
-     */
-    public static final int FIELD_Y = Game.Field.FIELD_Y * 2 / 10;
-  }
 
-  /** General constants for the drivetrain. Primarily used by {@link frc.robot.subsystems.Drive}. */
-  public static class DriveConstants {
-    /* CAN IDs for the drivetrain motor controllers */
-    public static final int ID_TALON_FRONT_LEFT = 0;
-    public static final int ID_TALON_FRONT_RIGHT = 1;
-    public static final int ID_TALON_REAR_LEFT = 2;
-    public static final int ID_TALON_REAR_RIGHT = 3;
-  }
-
-  /**
-   * General constants for the AStar Subsystem. Primarily used by {@link
-   * frc.robot.positioning.AStar}.
-   */
-  public static class AStar {
-    /** Half the length of the field. Positive for Max, Negative for Min. Measured in cm. */
-    public static final int FIELD_X = 1654 / 2;
-
-    /** Half the width of the field. Positive for Max, Negative for Min. Measured in cm. */
-    public static final int FIELD_Y = 802 / 2;
+    /** The full length of the field. Measured in decimeters. */
+    public static final int FIELD_Y = Game.Field.FIELD_Y * 2 / 10;
   }
 
   /**

src/main/java/frc/robot/Robot.java

@@ -8,11 +8,6 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc.robot.positioning.ArmKinematics;
-import frc.robot.positioning.Pose;
-import frc.robot.positioning.AStarTrajectoryGenerator;
-import frc.robot.positioning.Path;
-import frc.robot.positioning.AStar;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 /**
  * The VM is configured to automatically run this class, and to call the functions corresponding to
@@ -29,7 +24,7 @@ public class Robot extends TimedRobot {
 
   private RobotContainer m_robotContainer;
 
-  //private Path path = new Path();
+  // private Path path = new Path();
 
   /**
    * This function is run when the robot is first started up and should be used for any
@@ -40,8 +35,8 @@ public void robotInit() {
     // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
     // autonomous chooser on the dashboard.
 
-    //AStar aStar = new AStar(1,1,10,10);
-    //this.path = aStar.generateAStarPath().get();
+    // AStar aStar = new AStar(1,1,10,10);
+    // this.path = aStar.generateAStarPath().get();
 
     m_robotContainer = new RobotContainer();
 
@@ -65,10 +60,11 @@ public void robotPeriodic() {
     // block in order for anything in the Command-based framework to work.
     CommandScheduler.getInstance().run();
 
-    //if (this.path!=null) {
-    //System.out.println("datPath " + AStarTrajectoryGenerator.generateTrajectory(path));
-    //SmartDashboard.putString("Traj: ", AStarTrajectoryGenerator.generateTrajectory(path).toString());
-    //}
+    // if (this.path!=null) {
+    // System.out.println("datPath " + AStarTrajectoryGenerator.generateTrajectory(path));
+    // SmartDashboard.putString("Traj: ",
+    // AStarTrajectoryGenerator.generateTrajectory(path).toString());
+    // }
   }
 
   /** This function is called once each time the robot enters Disabled mode. */

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

@@ -31,12 +31,6 @@
 import frc.robot.commands.TrackTrajectory;
 import frc.robot.commands.moveToPose.MoveToPose;
 import frc.robot.positioning.Pose;
-import frc.robot.commands.ExampleCommand;
-import frc.robot.commands.autotasks.ExampleAutoTask;
-import frc.robot.subsystems.Drive;
-import frc.robot.subsystems.ExampleSubsystem;
-import frc.robot.subsystems.Limelight;
-import frc.robot.subsystems.TaskScheduler;
 import frc.robot.subsystems.Camera;
 import frc.robot.subsystems.Drive;
 import frc.robot.subsystems.I2CManager;

src/main/java/frc/robot/positioning/Team937TrajectoryUtils.java

@@ -1,32 +1,22 @@
 // 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 frc.robot.positioning;
 
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.trajectory.*;
 import edu.wpi.first.math.trajectory.Trajectory;
 import edu.wpi.first.math.trajectory.TrajectoryConfig;
 import edu.wpi.first.math.trajectory.TrajectoryGenerator;
-import edu.wpi.first.math.trajectory.*;
 import edu.wpi.first.math.trajectory.TrajectoryUtil;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Filesystem;
 import frc.robot.Constants;
-import frc.robot.Constants.Drive;
-import frc.robot.Constants.Drive.HolonomicController;
-import frc.robot.Constants.Drive.HolonomicController.ThetaController;
-import frc.robot.Constants.Drive.HolonomicController.ThetaController.Constraints;
-
-
-
 import java.util.ArrayList;
 
-/**
- * Static utility class for generating WPILib {@link Trajectory Trajectories} and more
- */
+/** Static utility class for generating WPILib {@link Trajectory Trajectories} and more */
 public class Team937TrajectoryUtils {
 
   /**
@@ -65,18 +55,19 @@ public static Trajectory generateTrajectory(
 
   /**
    * Generates {@link Trajectory} out of Json File intended for pathweaver.
+   *
    * @param jsonFile - A filepath to a JSON file
    * @return A {@link Trajectory}
    */
   public static Trajectory generateTrajectory(String jsonFile) {
 
     try {
-      return TrajectoryUtil.fromPathweaverJson(Filesystem.getDeployDirectory().toPath().resolve(jsonFile));
-    }
-    catch (java.io.IOException e) {
-      DriverStation.reportError("Unable to open trajectory JSON file: " + jsonFile, e.getStackTrace());
+      return TrajectoryUtil.fromPathweaverJson(
+          Filesystem.getDeployDirectory().toPath().resolve(jsonFile));
+    } catch (java.io.IOException e) {
+      DriverStation.reportError(
+          "Unable to open trajectory JSON file: " + jsonFile, e.getStackTrace());
       return null;
     }
   }
-
 }

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

@@ -8,12 +8,6 @@
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
 import com.kauailabs.navx.frc.AHRS;
-import edu.wpi.first.math.controller.HolonomicDriveController;
-import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.controller.ProfiledPIDController;
-import edu.wpi.first.math.estimator.MecanumDrivePoseEstimator;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.controller.RamseteController;
 import edu.wpi.first.math.estimator.DifferentialDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
@@ -23,6 +17,7 @@
 import edu.wpi.first.math.trajectory.Trajectory;
 import edu.wpi.first.wpilibj.SPI;
 import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;