From f895352bb922b0ba66b92f48bde1cc7b3050fa16 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 5 Mar 2022 11:07:54 -0700 Subject: [PATCH] DummySensor class, shoot is tested --- src/main/java/frc4388/robot/Constants.java | 3 +- .../java/frc4388/robot/RobotContainer.java | 24 +++--- src/main/java/frc4388/robot/RobotMap.java | 8 +- .../java/frc4388/robot/commands/Shoot.java | 77 ++++++++----------- .../frc4388/robot/subsystems/BoomBoom.java | 8 +- .../java/frc4388/robot/subsystems/Hood.java | 44 +++++------ .../robot/subsystems/SwerveModule.java | 6 +- .../java/frc4388/robot/subsystems/Turret.java | 12 +-- .../java/frc4388/utility/DummySensor.java | 74 ++++++++++++++++++ src/main/java/frc4388/utility/Gains.java | 48 ++++++------ 10 files changed, 185 insertions(+), 119 deletions(-) create mode 100644 src/main/java/frc4388/utility/DummySensor.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 9600169..bd8cef4 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -142,7 +142,8 @@ public final class Constants { public static final int TURRET_MOTOR_ROTS_PER_ROT = 0; public static final double ENCODER_TICKS_PER_REV = 2048; - + // Shoot Command Constants + public static final Gains SHOOT_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0); /* Turret Constants */ //ID diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index dd05ec5..5c5b4ca 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -58,6 +58,7 @@ import frc4388.robot.Constants.LEDConstants; import frc4388.robot.Constants.OIConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.commands.AimToCenter; +import frc4388.robot.commands.Shoot; import frc4388.robot.subsystems.BoomBoom; import frc4388.robot.subsystems.Hood; import frc4388.robot.subsystems.LED; @@ -88,12 +89,12 @@ public class RobotContainer { private final LED m_robotLED = new LED(m_robotMap.LEDController); private final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight); private final Hood m_robotHood = new Hood(); - // private final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); - // private final Vision m_robotVison = new Vision(m_robotTurret, m_robotBoomBoom); + private final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); + private final Vision m_robotVision = new Vision(m_robotTurret, m_robotBoomBoom); /* Controllers */ private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); private final XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); - + /* Autonomous */ private PathPlannerTrajectory loadedPathTrajectory = null; private final ListeningSendableChooser autoChooser = new ListeningSendableChooser<>(this::loadPath); @@ -119,7 +120,7 @@ public class RobotContainer { //Turret default command - // m_robotTurret.setDefaultCommand(new AimToCenter(m_robotTurret, m_robotSwerveDrive)); + m_robotTurret.setDefaultCommand(new AimToCenter(m_robotTurret, m_robotSwerveDrive)); m_robotSwerveDrive.setDefaultCommand( new RunCommand(() -> m_robotSwerveDrive.driveWithInput( @@ -161,17 +162,16 @@ public class RobotContainer { /* Operator Buttons */ // activates "Lit Mode" - new JoystickButton(getOperatorController(), XboxController.Button.kA.value) + new JoystickButton(getOperatorController(), XboxController.Button.kB.value) .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); + + new JoystickButton(getOperatorController(), XboxController.Button.kY.value) + .whenPressed(new InstantCommand()); + // activates "BoomBoom" - new JoystickButton(getOperatorController(), XboxController.Button.kB.value) - .whenPressed(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0.1)) - .whenReleased(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0)); - // activates hood - new JoystickButton(getOperatorController(), XboxController.Button.kB.value) - .whenPressed(() -> m_robotHood.runHood(0.5d)) - .whenReleased(() -> m_robotHood.runHood(0.d)); + new JoystickButton(getOperatorController(), XboxController.Button.kA.value) + .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood)); } /** diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 649fcb2..3c85082 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -136,7 +136,7 @@ public class RobotMap { public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID); public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID); - // public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless); + public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless); // Create motor CANSparkMax void configureShooterMotorControllers() { @@ -163,9 +163,9 @@ public class RobotMap { shooterFalconRight.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, ShooterConstants.SHOOTER_TIMEOUT_MS); shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS); - /* Turret Subsytem */ - shooterFalconRight.configStatorCurrentLimit(new StatorCurrentLimitConfiguration(true, 6, 9, 4.2)); // TODO: dont pull numbers out of our ass anymore - shooterFalconLeft.configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, 12, 13, 0.4)); // TODO: dont pull numbers out of our ass anymore + // /* Turret Subsytem */ + // shooterFalconRight.configStatorCurrentLimit(new StatorCurrentLimitConfiguration(true, 6, 9, 4.2)); // TODO: dont pull numbers out of our ass anymore + // shooterFalconLeft.configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, 12, 13, 0.4)); // TODO: dont pull numbers out of our ass anymore } diff --git a/src/main/java/frc4388/robot/commands/Shoot.java b/src/main/java/frc4388/robot/commands/Shoot.java index 8fc6500..be04921 100644 --- a/src/main/java/frc4388/robot/commands/Shoot.java +++ b/src/main/java/frc4388/robot/commands/Shoot.java @@ -4,12 +4,17 @@ package frc4388.robot.commands; +import edu.wpi.first.hal.simulation.SimulatorJNI; +import edu.wpi.first.wpilibj.simulation.LinearSystemSim; +import edu.wpi.first.wpilibj.simulation.SimHooks; import edu.wpi.first.wpilibj2.command.CommandBase; - +import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.subsystems.BoomBoom; import frc4388.robot.subsystems.Hood; import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.Turret; +import frc4388.utility.DummySensor; +import frc4388.utility.Gains; public class Shoot extends CommandBase { @@ -34,17 +39,24 @@ public class Shoot extends CommandBase { // pid public double error; public double prevError; + public Gains shootGains = ShooterConstants.SHOOT_GAINS; public double kP, kI, kD; public double proportional, integral, derivative; public double time; public double output; public double tolerance = 5.0; - // // dummy motor - // public WPI_TalonFX dummy = new WPI_TalonFX(69 - 420); - // public TalonFXConfiguration dummyConfiguration = new TalonFXConfiguration(); + // testing + public DummySensor dummy = new DummySensor(0); - /** Creates a new Shoot. */ + /** + * Creates a new shoot command, allowing the robot to aim and be ready to fire a ball + * TODO: Velocity Correction + * @param sDrive Drive Train + * @param sShooter Shooter Drum + * @param sTurret Shooter Turret + * @param sHood Shooter Hood + */ public Shoot(SwerveDrive sDrive, BoomBoom sShooter, Turret sTurret, Hood sHood) { // Use addRequirements() here to declare subsystem dependencies. m_swerve = sDrive; @@ -54,14 +66,16 @@ public class Shoot extends CommandBase { addRequirements(m_swerve, m_boomBoom, m_turret, m_hood); - kP = 0.1; - kI = 0.0; - kD = 0.0; + kP = shootGains.kP; + kI = shootGains.kI; + kD = shootGains.kD; proportional = 0; integral = 0; derivative = 0; time = 0.02; + + DummySensor.resetAll(); } /** @@ -83,8 +97,13 @@ public class Shoot extends CommandBase { // get targets (shooter tables) m_targetVel = m_boomBoom.getVelocity(m_distance); m_targetHood = m_boomBoom.getHood(m_distance); + + // target angle tests + m_gyroAngle = 0; + m_odoX = -1; + m_odoY = 1; + m_targetAngle = ((Math.atan2(m_odoY, m_odoX) * (180./Math.PI) - m_gyroAngle) + 180. + 360.) % 360.; - m_driveTargetAngle = m_swerve.getRegGyro().getDegrees(); // deadzone processing if (AimToCenter.isHardwareDeadzone(m_targetAngle)) { @@ -93,37 +112,14 @@ public class Shoot extends CommandBase { if (AimToCenter.isDigitalDeadzone(m_targetAngle)) { // this should rotate the entire swerve drive by 20 degrees, so shoot can now proceed like normal. idk if this will work - m_swerve.driveWithInput(0, 0, Math.cos(m_driveTargetAngle + 20), Math.sin(m_driveTargetAngle + 20), true); + m_swerve.driveWithInput(0, 0, Math.cos(m_gyroAngle + 20), Math.sin(m_gyroAngle + 20), true); } - - // // normal (i think) PID stuff - // dummyConfiguration.primaryPID.selectedFeedbackSensor = TalonFXFeedbackDevice.IntegratedSensor.toFeedbackDevice(); - // dummyConfiguration.remoteFilter0.remoteSensorDeviceID = dummy.getDeviceID(); - // dummyConfiguration.remoteFilter0.remoteSensorSource = RemoteSensorSource.TalonFX_SelectedSensor; - - // dummyConfiguration.slot0.kP = 0.1; - // dummyConfiguration.slot0.kI = 0; - // dummyConfiguration.slot0.kD = 0; - // dummyConfiguration.slot0.kF = 0; - - // // weird PID stuff - // dummyConfiguration.auxiliaryPID.selectedFeedbackSensor = TalonFXFeedbackDevice.SoftwareEmulatedSensor.toFeedbackDevice(); - // dummyConfiguration.remoteFilter1.remoteSensorDeviceID = ShooterConstants.TURRET_MOTOR_CAN_ID; - // dummyConfiguration.remoteFilter1.remoteSensorSource = RemoteSensorSource.TalonFX_SelectedSensor; - // // dummyConfiguration.auxiliaryPID.selectedFeedbackCoefficient = 0; - - // dummyConfiguration.slot1.kP = 0.1; - // dummyConfiguration.slot1.kI = 0; - // dummyConfiguration.slot1.kD = 0; - // dummyConfiguration.slot1.kF = 0; - - // dummy.configAllSettings(dummyConfiguration); - + // initial error updateError(); + System.out.println("Error: " + error); prevError = error; } - /** * Run custom PID. */ @@ -140,17 +136,12 @@ public class Shoot extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - // dummy.selectProfileSlot(0, 0); - // dummy.selectProfileSlot(1, 1); - // dummy.set(TalonFXControlMode.Position, m_driveTargetAngle, DemandType.AuxPID, m_targetAngle); - // m_swerve.driveWithInput(0, 0, m_driveTargetAngle, true); - // m_swerve.driveWithInput(0, 0, Math.cos(m_driveTargetAngle), Math.sin(m_driveTargetAngle), true); // only works for new DWI in swerve branch // custom pid runPID(); - m_swerve.driveWithInput(0, 0, output, true); // i have no idea if this is how you rotate the - // entire swerve drive or its the commented line below - // m_swerve.driveWithInput(0, 0, Math.cos(output), Math.sin(output), true); + // m_swerve.driveWithInput(0, 0, output, true); // i have no idea if this is how you rotate the + // entire swerve drive or its the line below + m_swerve.driveWithInput(0, 0, Math.cos(output), Math.sin(output), true); m_hood.runAngleAdjustPID(m_targetHood); m_boomBoom.runDrumShooterVelocityPID(m_targetVel); diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index c02de5e..e1286e5 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -181,10 +181,10 @@ public class BoomBoom extends SubsystemBase { public void setShooterGains() { m_shooterFalconLeft.selectProfileSlot(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.SHOOTER_PID_LOOP_IDX); - m_shooterFalconLeft.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterFalconLeft.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterFalconLeft.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterFalconLeft.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalconLeft.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.kF, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalconLeft.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.kP, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalconLeft.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.kI, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalconLeft.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.kD, ShooterConstants.SHOOTER_TIMEOUT_MS); } public void runDrumShooterVelocityPID(double targetVel) { diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java index 5b1d2ff..c9e963f 100644 --- a/src/main/java/frc4388/robot/subsystems/Hood.java +++ b/src/main/java/frc4388/robot/subsystems/Hood.java @@ -21,31 +21,31 @@ import frc4388.utility.Gains; public class Hood extends SubsystemBase { public BoomBoom m_shooterSubsystem; - // public CANSparkMax m_angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); + public CANSparkMax m_angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); public SparkMaxLimitSwitch m_hoodUpLimitSwitch; public SparkMaxLimitSwitch m_hoodDownLimitSwitch; public static Gains m_angleAdjusterGains = ShooterConstants.SHOOTER_ANGLE_GAINS; - // public RelativeEncoder m_angleEncoder = m_angleAdjusterMotor.getEncoder(); + public RelativeEncoder m_angleEncoder = m_angleAdjusterMotor.getEncoder(); - // public SparkMaxPIDController m_angleAdjusterPIDController = m_angleAdjusterMotor.getPIDController(); + public SparkMaxPIDController m_angleAdjusterPIDController = m_angleAdjusterMotor.getPIDController(); - //public boolean m_isHoodReady = false; + public boolean m_isHoodReady = false; public double m_fireAngle; /** Creates a new Hood. */ public Hood() { - // m_angleAdjusterMotor.setIdleMode(IdleMode.kBrake); + m_angleAdjusterMotor.setIdleMode(IdleMode.kBrake); - // m_hoodUpLimitSwitch = m_angleAdjusterMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - // m_hoodDownLimitSwitch = m_angleAdjusterMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - // m_hoodUpLimitSwitch.enableLimitSwitch(true); - // m_hoodDownLimitSwitch.enableLimitSwitch(true); + m_hoodUpLimitSwitch = m_angleAdjusterMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + m_hoodDownLimitSwitch = m_angleAdjusterMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + m_hoodUpLimitSwitch.enableLimitSwitch(true); + m_hoodDownLimitSwitch.enableLimitSwitch(true); - // m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.HOOD_FORWARD_LIMIT); - // m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.HOOD_REVERSE_LIMIT); + m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.HOOD_FORWARD_LIMIT); + m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.HOOD_REVERSE_LIMIT); setHoodSoftLimits(true); } @@ -60,30 +60,30 @@ public double m_fireAngle; * @param set Boolean to set soft limits to. */ public void setHoodSoftLimits(boolean set) { - // m_angleAdjusterMotor.enableSoftLimit(SoftLimitDirection.kForward, set); - // m_angleAdjusterMotor.enableSoftLimit(SoftLimitDirection.kReverse, set); + m_angleAdjusterMotor.enableSoftLimit(SoftLimitDirection.kForward, set); + m_angleAdjusterMotor.enableSoftLimit(SoftLimitDirection.kReverse, set); } public void runAngleAdjustPID(double targetAngle) { //Set PID Coefficients - // m_angleAdjusterPIDController.setP(m_angleAdjusterGains.m_kP); - // m_angleAdjusterPIDController.setI(m_angleAdjusterGains.m_kI); - // m_angleAdjusterPIDController.setD(m_angleAdjusterGains.m_kD); - // m_angleAdjusterPIDController.setIZone(m_angleAdjusterGains.m_kIzone); - // m_angleAdjusterPIDController.setFF(m_angleAdjusterGains.m_kF); - // m_angleAdjusterPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_angleAdjusterGains.m_kPeakOutput); + m_angleAdjusterPIDController.setP(m_angleAdjusterGains.kP); + m_angleAdjusterPIDController.setI(m_angleAdjusterGains.kI); + m_angleAdjusterPIDController.setD(m_angleAdjusterGains.kD); + m_angleAdjusterPIDController.setIZone(m_angleAdjusterGains.kIzone); + m_angleAdjusterPIDController.setFF(m_angleAdjusterGains.kF); + m_angleAdjusterPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_angleAdjusterGains.kPeakOutput); - // m_angleAdjusterPIDController.setReference(targetAngle, ControlType.kPosition); + m_angleAdjusterPIDController.setReference(targetAngle, ControlType.kPosition); } public void runHood(double input) { - // m_angleAdjusterMotor.set(input); + m_angleAdjusterMotor.set(input); } public void resetGyroAngleAdj(){ - // m_angleEncoder.setPosition(0); + m_angleEncoder.setPosition(0); } public double getAnglePosition(){ diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 44afb70..d72199b 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -47,9 +47,9 @@ public class SwerveModule extends SubsystemBase { TalonFXConfiguration angleTalonFXConfiguration = new TalonFXConfiguration(); - angleTalonFXConfiguration.slot0.kP = m_swerveGains.m_kP; - angleTalonFXConfiguration.slot0.kI = m_swerveGains.m_kI; - angleTalonFXConfiguration.slot0.kD = m_swerveGains.m_kD; + angleTalonFXConfiguration.slot0.kP = m_swerveGains.kP; + angleTalonFXConfiguration.slot0.kI = m_swerveGains.kI; + angleTalonFXConfiguration.slot0.kD = m_swerveGains.kD; // Use the CANCoder as the remote sensor for the primary TalonFX PID angleTalonFXConfiguration.remoteFilter0.remoteSensorDeviceID = canCoder.getDeviceID(); diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 3a00f02..7a0f45e 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -63,12 +63,12 @@ public class Turret extends SubsystemBase { m_boomBoomRotateMotor.setInverted(false); - m_boomBoomRotatePIDController.setP(m_shooterTGains.m_kP); - m_boomBoomRotatePIDController.setI(m_shooterTGains.m_kI); - m_boomBoomRotatePIDController.setD(m_shooterTGains.m_kD); - m_boomBoomRotatePIDController.setFF(m_shooterTGains.m_kF); - m_boomBoomRotatePIDController.setIZone(m_shooterTGains.m_kIzone); - m_boomBoomRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTGains.m_kPeakOutput); + m_boomBoomRotatePIDController.setP(m_shooterTGains.kP); + m_boomBoomRotatePIDController.setI(m_shooterTGains.kI); + m_boomBoomRotatePIDController.setD(m_shooterTGains.kD); + m_boomBoomRotatePIDController.setFF(m_shooterTGains.kF); + m_boomBoomRotatePIDController.setIZone(m_shooterTGains.kIzone); + m_boomBoomRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTGains.kPeakOutput); } @Override diff --git a/src/main/java/frc4388/utility/DummySensor.java b/src/main/java/frc4388/utility/DummySensor.java new file mode 100644 index 0000000..dadef80 --- /dev/null +++ b/src/main/java/frc4388/utility/DummySensor.java @@ -0,0 +1,74 @@ +// 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; + +import java.util.ArrayList; + +public class DummySensor { + + private double value; + public double start; + public static ArrayList instances = new ArrayList(); + + /** + * Creates a new DummySensor, which is a helper class for conducting PID tests without a physical robot. + * @param init The start "position" of the sensor (default is 0). + */ + public DummySensor(double init) { + value = init; + start = init; + instances.add(this); + } + + /** + * Creates a new DummySensor, which is a helper class for conducting PID tests without a physical robot. + */ + public DummySensor() { + value = 0; + start = 0; + instances.add(this); + } + + /** + * Reset the "position" of the DummySensor to its starting value. + */ + public void reset() { + value = start; + } + + /** + * Reset the "position" of the DummySensor to a given value. + * @param val The "position" to reset the DummySensor to. + */ + public void reset(double val) { + value = val; + } + + /** + * Reset all instances of DummySensor to their starting values. + */ + public static void resetAll() { + for (DummySensor instance : instances) { + instance.reset(); + } + } + + /** + * Get the "position" of the DummySensor. + * @return The current "position". + */ + public double get() { + return value; + } + + /** + * Apply an input to the DummySensor, changing its "position". + * @param input The input to apply. + */ + public void apply(double input) { + value = value + input; + } + +} diff --git a/src/main/java/frc4388/utility/Gains.java b/src/main/java/frc4388/utility/Gains.java index 930e34f..13144a4 100644 --- a/src/main/java/frc4388/utility/Gains.java +++ b/src/main/java/frc4388/utility/Gains.java @@ -6,14 +6,14 @@ package frc4388.utility; /** Add your docs here. */ public class Gains { - public double m_kP; - public double m_kI; - public double m_kD; - public double m_kF; - public int m_kIzone; - public double m_kPeakOutput; - public double m_kmaxOutput; - public double m_kminOutput; + 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 @@ -26,14 +26,14 @@ public class Gains { */ public Gains(double kP, double kI, double kD, double kF, int kIzone, double kPeakOutput) { - m_kP = kP; - m_kI = kI; - m_kD = kD; - m_kF = kF; - m_kIzone = kIzone; - m_kPeakOutput = kPeakOutput; - m_kmaxOutput = m_kPeakOutput; - m_kminOutput = -m_kPeakOutput; + this.kP = kP; + this.kI = kI; + this.kD = kD; + this.kF = kF; + this.kIzone = kIzone; + this.kPeakOutput = kPeakOutput; + this.kmaxOutput = this.kPeakOutput; + this.kminOutput = -this.kPeakOutput; } /** @@ -48,13 +48,13 @@ public class Gains { */ public Gains(double kP, double kI, double kD, double kF, int kIzone, double kMinOutput, double kMaxOutput) { - m_kP = kP; - m_kI = kI; - m_kD = kD; - m_kF = kF; - m_kIzone = kIzone; - m_kminOutput = kMinOutput; - m_kmaxOutput = kMaxOutput; - m_kPeakOutput = (Math.abs(m_kminOutput) > Math.abs(m_kmaxOutput)) ? Math.abs(m_kminOutput) : Math.abs(m_kmaxOutput); + this.kP = kP; + this.kI = kI; + this.kD = kD; + this.kF = kF; + this.kIzone = kIzone; + this.kminOutput = kMinOutput; + this.kmaxOutput = kMaxOutput; + this.kPeakOutput = (Math.abs(this.kminOutput) > Math.abs(this.kmaxOutput)) ? Math.abs(this.kminOutput) : Math.abs(this.kmaxOutput); } }