Simplified the process of characterizing drivetrai

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

@@ -14,6 +14,7 @@
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.Utility.Gyro;
 import frc.robot.Utility.Motor;
 import frc.robot.commands.Flywheel.FlywheelToSpeed;
 
@@ -53,8 +54,9 @@ public void robotPeriodic() {
     // block in order for anything in the Command-based framework to work.
     CommandScheduler.getInstance().run();
 
-
+    Gyro.updateAcceleration();
     Motor.updateAcceleration();
+    System.out.println(Gyro.getAngle());
   }
 
   /**

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

@@ -77,7 +77,9 @@ public class RobotContainer {
 
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
-    m_drivetrain.configFeedforward(0.45682532146418686, 2.8207938356301114, 0.03512928023526242); 
+    m_drivetrain.configFeedforwardSided(
+      0.5319812402757178, 2.83098038352017, 0.06669021995516602,
+      0.5148996120213958, 2.8125908259961605, 0.07363291020991329);
     m_drivetrain.enableBrakeMode(true);
     m_drivetrain.setDefaultCommand(new ArcadeDrive(m_drivetrain, m_controller, 1, Math.PI / 2));
     //m_intake.setDefaultCommand(new IntakeDeploy(m_intake));//so it deploys on start
@@ -117,6 +119,7 @@ private void configureButtonBindings() {
   public Command getAutonomousCommand() {
     //return new CharacterizeFlywheel(m_flywheel);
     //return new CharacterizeDrivetrain(m_drivetrain);
+    /*
     return new FindFeedForwardGainsForVelocity(m_drivetrain,
       (Double voltage)->{
         m_drivetrain.setControlMode(frc.robot.subsystems.DifferentialDrivetrain.ControlMode.Voltage);
@@ -131,6 +134,8 @@ public Command getAutonomousCommand() {
       },
       0.01
     );
+    */
+    return m_drivetrain.findFeedForwardGainsLeft;
     //return new ExampleCommand();
   }
 }

src/main/java/frc/robot/Utility/Gyro.java

@@ -17,6 +17,8 @@
  */
 public class Gyro {
     private static AHRS gyro = new AHRS(SPI.Port.kMXP);
+    private static double pastRate = 0;
+    private static double acceleration = 0;
 
     /**
      * Returns the z-axis angle (heading) reported by the gyroscope with alterations from Constants settings.
@@ -71,4 +73,13 @@ public static void reset(){
     public static double getRate(){
         return Math.toDegrees(gyro.getRate()) * (Constants.IS_GYRO_REVERSED ? -1.0 : 1.0);
     }
+
+    public static void updateAcceleration(){
+        Gyro.acceleration = (Gyro.getRate() - Gyro.pastRate) / (20. / 1000.);
+        Gyro.pastRate = Gyro.getRate();
+    }
+
+    public static double getAcceleration(){
+        return Gyro.acceleration;
+    }
 }

src/main/java/frc/robot/Utility/Regression.java

@@ -53,7 +53,7 @@ static public Double[] performRegression(int numberOfIndependentVariables, int n
         int possibleCombinations = (int)Math.pow(3, numberOfExplainingVariables);
 
         double intensity = 1;
-        for(int j = 0; j < 1000; ++j){
+        for(int j = 0; j < 100; ++j){
 
             int lowestCombination = 0;
             double lowestError = Double.POSITIVE_INFINITY;
@@ -70,7 +70,7 @@ static public Double[] performRegression(int numberOfIndependentVariables, int n
             }
 
             explainingVariables = Regression.addDirection(explainingVariables, Regression.getDirection(lowestCombination, intensity, numberOfExplainingVariables));
-            intensity *= 0.99;
+            intensity *= 0.9;
         }
 
         return explainingVariables;

src/main/java/frc/robot/commands/Drivetrain/ArcadeDrive.java

@@ -44,7 +44,7 @@ public void execute() {
     speed *= m_maxSpeed;
     rotation *= m_maxRotation;
 
-    m_drivetrain.setArcadeDrive(-speed, rotation);
+    m_drivetrain.setArcadeDrive(-speed, -rotation);
   }
 
   // Called once the command ends or is interrupted.

src/main/java/frc/robot/commands/FindFeedForwardGainsForVelocity.java

@@ -37,7 +37,12 @@ public FindFeedForwardGainsForVelocity(Subsystem subsystem, Consumer<Double> set
 
   // Called when the command is initially scheduled.
   @Override
-  public void initialize() {}
+  public void initialize() {
+    m_value = 0;
+    m_values = new Vector<Double>();
+    m_velocities = new Vector<Double>();
+    m_accelerations = new Vector<Double>();
+  }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
@@ -53,6 +58,7 @@ public void execute() {
   @Override
   public void end(boolean interrupted) {
     m_set.accept(0.0);
+    m_value = 0;
 
     Double[] values = Regression.findFeedForwardGainsForVelocity(m_values, m_velocities, m_accelerations);
     System.out.println("#");

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

@@ -19,6 +19,7 @@
 import frc.robot.Constants;
 import frc.robot.Utility.Gyro;
 import frc.robot.Utility.Motor;
+import frc.robot.commands.FindFeedForwardGainsForVelocity;
 
 public class DifferentialDrivetrain extends SubsystemBase {
   public enum ControlMode{
@@ -49,6 +50,50 @@ public enum ControlMode{
   protected double m_gearRatio;//1 rotation of wheel / spins of encoder
   protected boolean m_areEncodersInverted;
 
+  public FindFeedForwardGainsForVelocity findFeedForwardGainsLeft = new FindFeedForwardGainsForVelocity(this, 
+  (Double value)->{
+    setControlMode(ControlMode.Voltage);
+    setLeftTarget(value);
+    setRightTarget(value);
+  },
+  ()->{
+    return getLeftVelocity();
+  },
+  ()->{
+    return getLeftAcceleration();
+  },
+  0.01);
+
+  public FindFeedForwardGainsForVelocity findFeedForwardGainsRight = new FindFeedForwardGainsForVelocity(this, 
+  (Double value)->{
+    setControlMode(ControlMode.Voltage);
+    setLeftTarget(value);
+    setRightTarget(value);
+  },
+  ()->{
+    return getLeftVelocity();
+  },
+  ()->{
+    return getLeftAcceleration();
+  },
+  0.01);
+
+  public FindFeedForwardGainsForVelocity findFeedForwardGainsRotation = new FindFeedForwardGainsForVelocity(this, 
+  (Double value)->{
+    if(m_isFeedforwardConfigured == false)
+      System.out.println("You cannot configure rotation before configuring velocity.");
+    setControlMode(ControlMode.Velocity);
+    setLeftTarget(value);
+    setRightTarget(-value);
+  }, 
+  ()->{
+    return Gyro.getRate();
+  }, 
+  ()->{
+    return Gyro.getAcceleration();
+  },
+  0.001);
+
   /**
    * Creates a new Differential Drivetrain
    * @param wheelDiameter
@@ -93,8 +138,6 @@ public DifferentialDrivetrain(double wheelDiameter, double gearRatio, boolean is
   public void periodic() {
     m_odometry.update(Gyro.getRotation2d(), getLeftPosition(), getRightPosition());
 
-    //System.out.println(m_leftMaster.getPosition());
-
     if(Constants.IS_SPY_ENABLED){
       reportSpy();
     }
@@ -348,4 +391,6 @@ protected void reportSpy(){
     m_spyTab.get("Left Target").setDouble(m_leftTarget);
     m_spyTab.get("Right Target").setDouble(m_rightTarget);
   }
+
+
 }