From 9e59ef3e591cca9c45279de4f2d289e396a351ce Mon Sep 17 00:00:00 2001 From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com> Date: Sat, 14 Jan 2023 10:37:09 -0700 Subject: [PATCH] Added PelvicInflamatoryDisease --- src/main/java/frc4388/robot/Robot.java | 3 +- .../robot/commands/AutoBalanceTF2.java | 61 ++++++--------- .../commands/PelvicInflamitoryDisease.java | 59 ++++++++++++++ src/main/java/frc4388/utility/Gains.java | 77 +++++++++++++++++++ 4 files changed, 162 insertions(+), 38 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/PelvicInflamitoryDisease.java create mode 100644 src/main/java/frc4388/utility/Gains.java diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index c6f72b1..7e548a1 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -72,8 +72,7 @@ public class Robot extends TimedRobot { // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); - bot.setDefaultCommand(new AutoBalanceTF2(bot.frontLeft, - bot.frontRight, bot.backLeft, bot.backRight, bot.gyro, bot)); + bot.setDefaultCommand(new AutoBalanceTF2(bot)); } /** diff --git a/src/main/java/frc4388/robot/commands/AutoBalanceTF2.java b/src/main/java/frc4388/robot/commands/AutoBalanceTF2.java index 4299737..1c6f63c 100644 --- a/src/main/java/frc4388/robot/commands/AutoBalanceTF2.java +++ b/src/main/java/frc4388/robot/commands/AutoBalanceTF2.java @@ -16,46 +16,35 @@ import edu.wpi.first.wpilibj2.command.PIDCommand; import frc4388.robot.Robot; import frc4388.utility.RobotGyro; -// NOTE: Consider using this command inline, rather than writing a subclass. For more +// NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html -public class AutoBalanceTF2 extends PIDCommand { - // private TalonSRX frontLeft; - // private TalonSRX frontRight; - // private TalonSRX backLeft; - // private TalonSRX backRight; +public class AutoBalanceTF2 extends PelvicInflamitoryDisease { + Robot.MicroBot bot; - /** Creates a new AutoBalanceTF2. */ - public AutoBalanceTF2(TalonSRX frontLeft, TalonSRX frontRight, TalonSRX backLeft, TalonSRX backRight, RobotGyro gyro, Robot.MicroBot bot) { - super( - // The controller that the command will use - new PIDController(.7, .02, .1), - // This should return the measurement - () -> Math.abs(gyro.getPitch()) < 3 ? 0 : gyro.getPitch(), - // This should return the setpoint (can also be a constant) - () -> 0, - // This uses the output - output -> { - // Use the output here - double out2 = -MathUtil.clamp(output / 20, -1, 1); - if (Math.abs(gyro.getPitch()) < 3) out2 = 0; + /** Creates a new AutoBalanceTF2. */ + // ! finish integrating PelvicInflamatoryDisease + public AutoBalanceTF2(Robot.MicroBot bot) { + super(.7, .02, .1, 0); + addRequirements(bot); + this.bot = bot; + } - SmartDashboard.putNumber("Output", out2); - frontLeft.set(ControlMode.PercentOutput, out2); - frontRight.set(ControlMode.PercentOutput, out2); - backRight.set(ControlMode.PercentOutput, out2); - backLeft.set(ControlMode.PercentOutput, out2); - }); + @Override + public double getError() { + return bot.gyro.getPitch(); + } - gyro.reset(); - addRequirements(bot); - // Use addRequirements() here to declare subsystem dependencies. - // Configure additional PID options by calling `getController` here. - } + @Override + public void runWithOutput(double output) { + double out2 = -MathUtil.clamp(output / 20, -1, 1); + if (Math.abs(bot.gyro.getPitch()) < 3) out2 = 0; + bot.setOutput(out2); + } - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } } diff --git a/src/main/java/frc4388/robot/commands/PelvicInflamitoryDisease.java b/src/main/java/frc4388/robot/commands/PelvicInflamitoryDisease.java new file mode 100644 index 0000000..c596201 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/PelvicInflamitoryDisease.java @@ -0,0 +1,59 @@ +// 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 frc4388.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.utility.Gains; + +public abstract class PelvicInflamitoryDisease extends CommandBase { + protected Gains gains; + private double output = 0; + + /** Creates a new PelvicInflamitoryDisease. */ + public PelvicInflamitoryDisease(double kp, double ki, double kd, double kf) { + gains = new Gains(kp, ki, kd, kf, 0); + } + + public PelvicInflamitoryDisease(Gains gains) { + this.gains = gains; + } + + /** produces the error from the setpoint */ + public abstract double getError(); + /** figure it out bitch */ + public abstract void runWithOutput(double output); + + // Called when the command is initially scheduled. + @Override + public void initialize() { + output = 0; + } + + private double prevError, cumError = 0; + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + double error = getError(); + cumError += error; + double delta = error - prevError; + + output = error * gains.kP; + output += cumError * gains.kI; + output += delta * gains.kD; + output += gains.kF; + + runWithOutput(output); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc4388/utility/Gains.java b/src/main/java/frc4388/utility/Gains.java new file mode 100644 index 0000000..7cda1e0 --- /dev/null +++ b/src/main/java/frc4388/utility/Gains.java @@ -0,0 +1,77 @@ +// 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 frc4388.utility; + +/** Add your docs here. */ +public class Gains { + public double kP; + public double kI; + public double kD; + public double kF; + public int kIZone; + public double kPeakOutput; + public double kMaxOutput; + public double kMinOutput; + + /** + * Creates Gains object for PIDs + * @param kP The P value. + * @param kI The I value. + * @param kD The D value. + * @param kF The F value. + * @param kIZone The zone of the I value. + * @param kPeakOutput The peak output setting the motors to run the gains at, in both forward and reverse directions. By default 1.0. + */ + public Gains(double kP, double kI, double kD, double kF, int kIZone, double kPeakOutput) { + this.kP = kP; + this.kI = kI; + this.kD = kD; + this.kF = kF; + this.kIZone = kIZone; + this.kPeakOutput = kPeakOutput; + this.kMaxOutput = kPeakOutput; + this.kMinOutput = -kPeakOutput; + } + + /** + * Creates Gains object for PIDs + * @param kP The P value. + * @param kI The I value. + * @param kD The D value. + * @param kF The F value. + * @param kIZone The zone of the I value. + */ + public Gains(double kP, double kI, double kD, double kF, int kIZone) { + this.kP = kP; + this.kI = kI; + this.kD = kD; + this.kF = kF; + this.kIZone = kIZone; + this.kPeakOutput = 1.0; + this.kMaxOutput = 1.0; + this.kMinOutput = -1.0; + } + + /** + * Creates Gains object for PIDs + * @param kP The P value. + * @param kI The I value. + * @param kD The D value. + * @param kF The F value. + * @param kIZone The zone of the I value. + * @param kMinOutput The lowest output setting to run the gains at, usually in the reverse direction. By default -1.0. + * @param kMaxOutput The highest output setting to run the gains at, usually in the forward direction. By default 1.0. + */ + public Gains(double kP, double kI, double kD, double kF, int kIZone, double kMaxOutput, double kMinOutput) { + this.kP = kP; + this.kI = kI; + this.kD = kD; + this.kF = kF; + this.kIZone = kIZone; + this.kMaxOutput = kMaxOutput; + this.kMinOutput = kMinOutput; + this.kPeakOutput = (Math.abs(kMinOutput) > Math.abs(kMaxOutput)) ? Math.abs(kMinOutput) : Math.abs(kMaxOutput); + } +} \ No newline at end of file