mirror of
https://github.com/Team4388/2022NoWayHome.git
synced 2026-06-09 00:38:05 -06:00
Merge branch 'swerve' of https://github.com/Team4388/2022NoWayHome into swerve
This commit is contained in:
evan
@@ -153,6 +153,9 @@ 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
|
||||
public static final int TURRET_MOTOR_CAN_ID = 30;
|
||||
|
||||
@@ -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;
|
||||
@@ -86,16 +87,12 @@ public class RobotContainer {
|
||||
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack,
|
||||
m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro);
|
||||
|
||||
/*
|
||||
* 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);
|
||||
* /* Controllers
|
||||
*/
|
||||
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_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);
|
||||
|
||||
@@ -182,17 +179,17 @@ 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));
|
||||
*/
|
||||
}
|
||||
|
||||
|
||||
@@ -161,61 +161,53 @@ public class RobotMap {
|
||||
|
||||
// Shooter Config
|
||||
/* Boom Boom Subsystem */
|
||||
/*
|
||||
* 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);
|
||||
*
|
||||
* // Create motor CANSparkMax
|
||||
* void configureShooterMotorControllers() {
|
||||
*
|
||||
* // LEFT FALCON
|
||||
* shooterFalconLeft.configFactoryDefault();
|
||||
* shooterFalconLeft.setNeutralMode(NeutralMode.Coast);
|
||||
* shooterFalconLeft.setInverted(true);
|
||||
* shooterFalconLeft.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
* shooterFalconLeft.configClosedloopRamp(0.75,
|
||||
* ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
* shooterFalconLeft.configPeakOutputReverse(0,
|
||||
* ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
* shooterFalconLeft.setSelectedSensorPosition(0,
|
||||
* ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
* shooterFalconLeft.configClosedLoopPeriod(0,
|
||||
* ShooterConstants.CLOSED_LOOP_TIME_MS, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
* shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.
|
||||
* SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
*
|
||||
* // RIGHT FALCON
|
||||
* shooterFalconRight.setInverted(false);
|
||||
* shooterFalconRight.setNeutralMode(NeutralMode.Coast);
|
||||
* shooterFalconRight.configFactoryDefault();
|
||||
* shooterFalconRight.configOpenloopRamp(1,
|
||||
* ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
* shooterFalconRight.configClosedloopRamp(0.75,
|
||||
* ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
* // m_shooterFalconRight.configPeakOutputForward(0,
|
||||
* ShooterConstants.SHOOTER_TIMEOUT_MS);(comment it in if necessary)
|
||||
* shooterFalconRight.setSelectedSensorPosition(0,
|
||||
* ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
* 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
|
||||
*/
|
||||
// }
|
||||
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);
|
||||
|
||||
// Create motor CANSparkMax
|
||||
void configureShooterMotorControllers() {
|
||||
|
||||
// LEFT FALCON
|
||||
shooterFalconLeft.configFactoryDefault();
|
||||
shooterFalconLeft.setNeutralMode(NeutralMode.Coast);
|
||||
shooterFalconLeft.setInverted(true);
|
||||
shooterFalconLeft.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
shooterFalconLeft.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
shooterFalconLeft.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
shooterFalconLeft.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX,
|
||||
ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
shooterFalconLeft.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS,
|
||||
ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG,
|
||||
ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
|
||||
// RIGHT FALCON
|
||||
shooterFalconRight.setInverted(false);
|
||||
shooterFalconRight.setNeutralMode(NeutralMode.Coast);
|
||||
shooterFalconRight.configFactoryDefault();
|
||||
shooterFalconRight.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
shooterFalconRight.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
// m_shooterFalconRight.configPeakOutputForward(0,
|
||||
// ShooterConstants.SHOOTER_TIMEOUT_MS);(comment it in if necessary)
|
||||
shooterFalconRight.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX,
|
||||
ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
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
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user