From f9da4cdb54be0d90bb93858b1caf13814650d5d3 Mon Sep 17 00:00:00 2001 From: stebb1976 Date: Mon, 12 Apr 2021 19:02:49 -0400 Subject: [PATCH] Simplified the process of characterizing drivetrai --- src/main/java/frc/robot/Robot.java | 4 +- src/main/java/frc/robot/RobotContainer.java | 7 ++- src/main/java/frc/robot/Utility/Gyro.java | 11 +++++ .../java/frc/robot/Utility/Regression.java | 4 +- .../commands/Drivetrain/ArcadeDrive.java | 2 +- .../FindFeedForwardGainsForVelocity.java | 8 ++- .../subsystems/DifferentialDrivetrain.java | 49 ++++++++++++++++++- 7 files changed, 77 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d94711d..83d125d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/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()); } /** diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index eb5a6d9..b02d702 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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(); } } diff --git a/src/main/java/frc/robot/Utility/Gyro.java b/src/main/java/frc/robot/Utility/Gyro.java index 47d7d27..eaa40a3 100644 --- a/src/main/java/frc/robot/Utility/Gyro.java +++ b/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; + } } diff --git a/src/main/java/frc/robot/Utility/Regression.java b/src/main/java/frc/robot/Utility/Regression.java index 94afaf2..46b9b4f 100644 --- a/src/main/java/frc/robot/Utility/Regression.java +++ b/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; diff --git a/src/main/java/frc/robot/commands/Drivetrain/ArcadeDrive.java b/src/main/java/frc/robot/commands/Drivetrain/ArcadeDrive.java index fdf6f59..5c9c41d 100644 --- a/src/main/java/frc/robot/commands/Drivetrain/ArcadeDrive.java +++ b/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. diff --git a/src/main/java/frc/robot/commands/FindFeedForwardGainsForVelocity.java b/src/main/java/frc/robot/commands/FindFeedForwardGainsForVelocity.java index a601f5f..9b6661c 100644 --- a/src/main/java/frc/robot/commands/FindFeedForwardGainsForVelocity.java +++ b/src/main/java/frc/robot/commands/FindFeedForwardGainsForVelocity.java @@ -37,7 +37,12 @@ public FindFeedForwardGainsForVelocity(Subsystem subsystem, Consumer set // Called when the command is initially scheduled. @Override - public void initialize() {} + public void initialize() { + m_value = 0; + m_values = new Vector(); + m_velocities = new Vector(); + m_accelerations = new Vector(); + } // 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("#"); diff --git a/src/main/java/frc/robot/subsystems/DifferentialDrivetrain.java b/src/main/java/frc/robot/subsystems/DifferentialDrivetrain.java index 11525d0..e3e316e 100644 --- a/src/main/java/frc/robot/subsystems/DifferentialDrivetrain.java +++ b/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); } + + }