mirror of
https://github.com/Team4388/2022NoWayHome.git
synced 2026-06-09 00:38:05 -06:00
DummySensor class, shoot is tested
This commit is contained in:
aarav18
@@ -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
|
||||
|
||||
@@ -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<File> 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));
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -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
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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) {
|
||||
|
||||
@@ -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(){
|
||||
|
||||
@@ -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();
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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<DummySensor> instances = new ArrayList<DummySensor>();
|
||||
|
||||
/**
|
||||
* 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;
|
||||
}
|
||||
|
||||
}
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user