Team4388/RiseOfRidgebotics2020
1
0
Fork 0
mirror of https://github.com/Team4388/RiseOfRidgebotics2020.git synced 2026-06-09 00:38:00 -06:00
Code Issues Packages Projects Releases Wiki Activity

Add ShooterHood Subsystem

Browse Source
This commit is contained in:
Keenan D. Buckley
2020-03-02 23:56:05 -07:00
parent 6d501b6bb9
commit 8f6578a47b
11 changed files with 179 additions and 93 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+7 -1
View File
@@ -51,6 +51,7 @@ import frc4388.robot.subsystems.LimeLight;
import frc4388.robot.subsystems.Pneumatics;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterAim;
import frc4388.robot.subsystems.ShooterHood;
import frc4388.robot.subsystems.Storage;
import frc4388.utility.controller.IHandController;
import frc4388.utility.controller.XboxController;
@@ -71,6 +72,7 @@ public class RobotContainer {
private final Intake m_robotIntake = new Intake();
private final Shooter m_robotShooter = new Shooter();
private final ShooterAim m_robotShooterAim = new ShooterAim();
private final ShooterHood m_robotShooterHood = new ShooterHood();
private final Climber m_robotClimber = new Climber();
private final Leveler m_robotLeveler = new Leveler();
private final Storage m_robotStorage = new Storage();
@@ -92,6 +94,10 @@ public class RobotContainer {
/* Passing Drive and Pneumatics Subsystems */
m_robotPneumatics.passRequiredSubsystem(m_robotDrive);
m_robotDrive.passRequiredSubsystem(m_robotPneumatics);
m_robotShooter.passRequiredSubsystem(m_robotShooterHood, m_robotShooterAim);
m_robotShooterHood.passRequiredSubsystem(m_robotShooter);
m_robotShooterAim.passRequiredSubsystem(m_robotShooter);
configureButtonBindings();
@@ -195,7 +201,7 @@ public class RobotContainer {
//Calibrates turret and hood
new JoystickButton(getOperatorJoystick(), XboxController.START_BUTTON)
.whileHeld(new CalibrateShooter(m_robotShooter, m_robotShooterAim));
.whileHeld(new CalibrateShooter(m_robotShooter, m_robotShooterAim, m_robotShooterHood));
//Prepares storage for intaking
new XboxTriggerButton(m_operatorXbox, XboxTriggerButton.LEFT_TRIGGER)
src/main/java/frc4388/robot/commands/shooter/CalibrateShooter.java
+12 -8
View File
@@ -13,19 +13,23 @@ import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterAim;
import frc4388.robot.subsystems.ShooterHood;
public class CalibrateShooter extends CommandBase {
Shooter m_shooter;
ShooterAim m_shooterAim;
ShooterHood m_shooterHood;
/**
* Calibrates the turret by moving the hood all the way down and moving the turret all the way right, then reseting the encoders
* @param shootSub The Shooter subsystem
* @param aimSub The ShooterAim subsystem
*/
public CalibrateShooter(Shooter shootSub, ShooterAim aimSub) {
public CalibrateShooter(Shooter shootSub, ShooterAim aimSub, ShooterHood hoodSub) {
m_shooter = shootSub;
m_shooterAim = aimSub;
addRequirements(m_shooter, m_shooterAim);
m_shooterHood = hoodSub;
addRequirements(m_shooter, m_shooterHood, m_shooterAim);
}
// Called when the command is initially scheduled.
@@ -36,10 +40,10 @@ public class CalibrateShooter extends CommandBase {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_shooter.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, false);
m_shooter.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, false);
m_shooter.m_angleEncoder.setPosition(0);
m_shooter.m_angleAdjustMotor.set(-ShooterConstants.HOOD_CALIBRATE_SPEED);
m_shooterHood.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, false);
m_shooterHood.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, false);
m_shooterHood.m_angleEncoder.setPosition(0);
m_shooterHood.m_angleAdjustMotor.set(-ShooterConstants.HOOD_CALIBRATE_SPEED);
m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, false);
m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, false);
@@ -50,8 +54,8 @@ public class CalibrateShooter extends CommandBase {
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
m_shooter.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, true);
m_shooter.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
m_shooterHood.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, true);
m_shooterHood.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, true);
m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
src/main/java/frc4388/robot/commands/shooter/HoldTarget.java
+6 -3
View File
@@ -14,15 +14,17 @@ import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.VisionConstants;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterAim;
import frc4388.robot.subsystems.ShooterHood;
import frc4388.utility.controller.IHandController;
public class HoldTarget extends CommandBase {
//Setup
// Setup
NetworkTableEntry xEntry;
ShooterAim m_shooterAim;
Shooter m_shooter;
ShooterHood m_shooterHood;
IHandController m_driverController;
//Aiming
// Aiming
double turnAmount = 0;
double xAngle = 0;
double yAngle = 0;
@@ -42,6 +44,7 @@ public class HoldTarget extends CommandBase {
public HoldTarget(Shooter shooterSubsystem, ShooterAim aimSubsystem) {
m_shooterAim = aimSubsystem;
m_shooter = shooterSubsystem;
m_shooterHood = m_shooter.m_shooterHoodSubsystem;
addRequirements(m_shooterAim);
NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
}
@@ -99,7 +102,7 @@ public class HoldTarget extends CommandBase {
m_shooter.m_fireVel = fireVel;
m_shooter.m_fireAngle = fireAngle;// + m_shooter.shooterTrims.m_hoodTrim;
m_shooterHood.m_fireAngle = fireAngle;// + m_shooter.shooterTrims.m_hoodTrim;
}
}
src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java
+7 -6
View File
@@ -10,16 +10,17 @@ package frc4388.robot.commands.shooter;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterHood;
public class HoodPositionPID extends CommandBase {
Shooter m_shooter;
double firingAngle;
private ShooterHood m_shooterHood;
/**
* Creates a new HoodPositionPID.
*/
public HoodPositionPID(Shooter subSystem) {
m_shooter = subSystem;
//addRequirements(m_shooter);
public HoodPositionPID(ShooterHood subSystem) {
m_shooterHood = subSystem;
addRequirements(m_shooterHood);
}
// Called when the command is initially scheduled.
@@ -35,7 +36,7 @@ public class HoodPositionPID extends CommandBase {
firingAngle = (-slope*m_shooter.addFireAngle())+b;*/
//SmartDashboard.putNumber("Shoot Angle From Equation", m_shooter.addFireAngle());
//SmartDashboard.putNumber("Fire Angle", firingAngle);
m_shooter.runAngleAdjustPID(firingAngle);
m_shooterHood.runAngleAdjustPID(firingAngle);
}
// Called once the command ends or is interrupted.
@@ -46,7 +47,7 @@ public class HoodPositionPID extends CommandBase {
// Returns true when the command should end.
@Override
public boolean isFinished() {
double encoderPos = m_shooter.m_angleAdjustMotor.getEncoder().getPosition();
double encoderPos = m_shooterHood.m_angleAdjustMotor.getEncoder().getPosition();
if(encoderPos < firingAngle + 1 || encoderPos < firingAngle - 1){
return false;
}
src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java
+4 -1
View File
@@ -12,6 +12,7 @@ import edu.wpi.first.wpilibj2.command.RunCommand;
import frc4388.robot.commands.storage.StorageRun;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterAim;
import frc4388.robot.subsystems.ShooterHood;
import frc4388.robot.subsystems.Storage;
// NOTE: Consider using this command inline, rather than writing a subclass. For more
@@ -25,9 +26,11 @@ public class ShootFireGroup extends ParallelRaceGroup {
* @param m_storage The Storage subsytem
*/
public ShootFireGroup(Shooter m_shooter, ShooterAim m_shooterAim, Storage m_storage) {
ShooterHood m_shooterHood = m_shooter.m_shooterHoodSubsystem;
addCommands(
new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel())),
new RunCommand(() -> m_shooter.runAngleAdjustPID(m_shooter.addFireAngle())),
new RunCommand(() -> m_shooterHood.runAngleAdjustPID(m_shooterHood.addFireAngle())),
new HoldTarget(m_shooter, m_shooterAim),
new StorageRun(m_storage)
);
src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java
+7 -3
View File
@@ -11,18 +11,22 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterHood;
public class ShooterVelocityControlPID extends CommandBase {
Shooter m_shooter;
double m_targetVel;
double m_actualVel;
private ShooterHood m_shooterHood;
/**
* Runs the drum at a velocity
* @param subsystem The Shooter subsytem
*/
public ShooterVelocityControlPID(Shooter subsystem) {
public ShooterVelocityControlPID(Shooter subsystem, ShooterHood subHood) {
m_shooter = subsystem;
addRequirements(m_shooter);
m_shooterHood = subHood;
addRequirements(m_shooter, m_shooterHood);
}
// Called when the command is initially scheduled.
@@ -35,7 +39,7 @@ public class ShooterVelocityControlPID extends CommandBase {
@Override
public void execute() {
m_shooter.runDrumShooterVelocityPID(/*m_shooter.addFireVel()*/13000);
m_shooter.runAngleAdjustPID(m_shooter.addFireAngle());
m_shooterHood.runAngleAdjustPID(m_shooterHood.addFireAngle());
//SmartDashboard.putNumber("Fire Velocity", m_shooter.addFireVel());
//SmartDashboard.putNumber("Fire Angle", m_shooter.addFireAngle());
}
src/main/java/frc4388/robot/commands/shooter/TrackTarget.java
+4 -1
View File
@@ -14,6 +14,7 @@ import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.VisionConstants;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.ShooterAim;
import frc4388.robot.subsystems.ShooterHood;
import frc4388.utility.controller.IHandController;
public class TrackTarget extends CommandBase {
@@ -33,6 +34,7 @@ public class TrackTarget extends CommandBase {
public double m_hoodTrim;
public double m_turretTrim;
private ShooterHood m_shooterHood;
/**
* Uses the Limelight to track the target
@@ -42,6 +44,7 @@ public class TrackTarget extends CommandBase {
public TrackTarget(Shooter shooterSubsystem, ShooterAim aimSubsystem) {
m_shooterAim = aimSubsystem;
m_shooter = shooterSubsystem;
m_shooterHood = m_shooter.m_shooterHoodSubsystem;
addRequirements(m_shooterAim);
NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
}
@@ -97,7 +100,7 @@ public class TrackTarget extends CommandBase {
m_shooter.m_fireVel = fireVel;
m_shooter.m_fireAngle = fireAngle;// + m_shooter.shooterTrims.m_hoodTrim;
m_shooterHood.m_fireAngle = fireAngle;// + m_shooter.shooterTrims.m_hoodTrim;
}
}
src/main/java/frc4388/robot/subsystems/CANDigitalInput.java
View File
src/main/java/frc4388/robot/subsystems/Shooter.java
+12 -55
View File
@@ -10,15 +10,6 @@ package frc4388.robot.subsystems;
import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.revrobotics.CANDigitalInput;
import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
import com.revrobotics.CANEncoder;
import com.revrobotics.CANPIDController;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMax.IdleMode;
import com.revrobotics.CANSparkMax.SoftLimitDirection;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import com.revrobotics.ControlType;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -31,13 +22,8 @@ import frc4388.utility.controller.IHandController;
public class Shooter extends SubsystemBase {
public WPI_TalonFX m_shooterFalcon = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_ID);
public CANSparkMax m_angleAdjustMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless);
CANPIDController m_angleAdjustPIDController = m_angleAdjustMotor.getPIDController();
public CANEncoder m_angleEncoder = m_angleAdjustMotor.getEncoder();
public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS;
public static Gains m_angleAdjustGains = ShooterConstants.SHOOTER_ANGLE_GAINS;
public static Shooter m_shooter;
public static IHandController m_controller;
@@ -48,10 +34,11 @@ public class Shooter extends SubsystemBase {
public boolean velReached;
public double m_fireVel;
public double m_fireAngle;
CANDigitalInput m_hoodUpLimit, m_hoodDownLimit;
public Trims shooterTrims;
public ShooterHood m_shooterHoodSubsystem;
public ShooterAim m_shooterAimSubsystem;
/*
* Creates a new Shooter subsystem, with the drum shooter and the angle adjsuter.
@@ -64,7 +51,6 @@ public class Shooter extends SubsystemBase {
m_shooterFalcon.configFactoryDefault();
m_shooterFalcon.setNeutralMode(NeutralMode.Coast);
m_shooterFalcon.setInverted(true);
m_angleAdjustMotor.setIdleMode(IdleMode.kBrake);
m_shooterFalcon.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalcon.configClosedloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS);
setShooterGains();
@@ -87,17 +73,6 @@ public class Shooter extends SubsystemBase {
//SmartDashboard.putNumber("CSV A 10", m_shooterTable.getAngleDisplacement(10));
//SmartDashboard.putNumber("CSV A 5", m_shooterTable.getAngleDisplacement(5));
//SmartDashboard.putNumber("CSV A 30", m_shooterTable.getAngleDisplacement(30));
m_hoodUpLimit = m_angleAdjustMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
m_hoodDownLimit = m_angleAdjustMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
m_hoodUpLimit.enableLimitSwitch(true);
m_hoodDownLimit.enableLimitSwitch(true);
m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, true);
m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.HOOD_UP_SOFT_LIMIT);
m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.HOOD_DOWN_SOFT_LIMIT);
}
@Override
@@ -107,7 +82,6 @@ public class Shooter extends SubsystemBase {
SmartDashboard.putNumber("Drum Velocity", m_shooterFalcon.getSelectedSensorVelocity());
SmartDashboard.putNumber("Drum Velocity CSV", m_fireVel);
SmartDashboard.putNumber("Fire Angle CSV", m_fireAngle);
//SmartDashboard.putNumber("Hood Position", m_shooter.getAnglePosition());
}
@@ -118,13 +92,18 @@ public class Shooter extends SubsystemBase {
}
}
/**
* Passes subsystem needed.
* @param subsystem Subsystem needed.
*/
public void passRequiredSubsystem(ShooterHood subsystem0, ShooterAim subsystem1) {
m_shooterHoodSubsystem = subsystem0;
m_shooterAimSubsystem = subsystem1;
}
public double addFireVel() {
return m_fireVel;
}
public double addFireAngle() {
return m_fireAngle;
}
/**
* Runs drum shooter motor.
@@ -145,20 +124,6 @@ public class Shooter extends SubsystemBase {
m_shooterFalcon.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS);
}
/* Angle Adjustment PID Control */
public void runAngleAdjustPID(double targetAngle)
{
// Set PID Coefficients
m_angleAdjustPIDController.setP(m_angleAdjustGains.m_kP);
m_angleAdjustPIDController.setI(m_angleAdjustGains.m_kI);
m_angleAdjustPIDController.setD(m_angleAdjustGains.m_kD);
m_angleAdjustPIDController.setIZone(m_angleAdjustGains.m_kIzone);
m_angleAdjustPIDController.setFF(m_angleAdjustGains.m_kF);
m_angleAdjustPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_angleAdjustGains.m_kPeakOutput);
m_angleAdjustPIDController.setReference(targetAngle, ControlType.kPosition);
}
/**
* Runs drum shooter velocity PID.
* @param falcon Motor to use
@@ -168,12 +133,4 @@ public class Shooter extends SubsystemBase {
System.out.println("dddddddddddddddddddddddd" + targetVel);
m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID
}
public void resetGyroAngleAdj(){
m_angleEncoder.setPosition(0);
}
public double getAnglePosition(){
return m_angleEncoder.getPosition();
}
}
src/main/java/frc4388/robot/subsystems/ShooterAim.java
+25 -15
View File
@@ -22,14 +22,15 @@ import frc4388.robot.Constants.ShooterConstants;
import frc4388.utility.Gains;
public class ShooterAim extends SubsystemBase {
public Shooter m_shooterSubsystem;
public CANSparkMax m_shooterRotateMotor = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, MotorType.kBrushless);
public static Gains m_shooterTurretGains = ShooterConstants.SHOOTER_TURRET_GAINS;
CANDigitalInput m_shooterRightLimit, m_shooterLeftLimit;
// Configure PID Controllers
CANPIDController m_shooterRotatePIDController = m_shooterRotateMotor.getPIDController();
public CANEncoder m_shooterRotateEncoder = m_shooterRotateMotor.getEncoder();
// Configure PID Controllers
CANPIDController m_shooterRotatePIDController = m_shooterRotateMotor.getPIDController();
public CANEncoder m_shooterRotateEncoder = m_shooterRotateMotor.getEncoder();
/**
* Creates a subsytem for the turret aiming
@@ -49,6 +50,19 @@ public class ShooterAim extends SubsystemBase {
m_shooterRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.TURRET_LEFT_SOFT_LIMIT);
}
@Override
public void periodic() {
// This method will be called once per scheduler run
}
/**
* Passes subsystem needed.
* @param subsystem Subsystem needed.
*/
public void passRequiredSubsystem(Shooter subsystem) {
m_shooterSubsystem = subsystem;
}
public void runShooterWithInput(double input) {
m_shooterRotateMotor.set(input*ShooterConstants.TURRET_SPEED_MULTIPLIER);
}
@@ -71,17 +85,13 @@ public class ShooterAim extends SubsystemBase {
m_shooterRotatePIDController.setReference(targetAngle, ControlType.kPosition);
}
public void resetGyroShooterRotate()
{
m_shooterRotateEncoder.setPosition(0);
}
public void resetGyroShooterRotate()
{
m_shooterRotateEncoder.setPosition(0);
}
public double getShooterRotatePosition(){
return m_shooterRotateMotor.getEncoder().getPosition();
}
@Override
public void periodic() {
// This method will be called once per scheduler run
public double getShooterRotatePosition()
{
return m_shooterRotateMotor.getEncoder().getPosition();
}
}
src/main/java/frc4388/robot/subsystems/ShooterHood.java
+95
View File
@@ -0,0 +1,95 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.subsystems;
import com.revrobotics.CANDigitalInput;
import com.revrobotics.CANEncoder;
import com.revrobotics.CANPIDController;
import com.revrobotics.CANSparkMax;
import com.revrobotics.ControlType;
import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
import com.revrobotics.CANSparkMax.IdleMode;
import com.revrobotics.CANSparkMax.SoftLimitDirection;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.utility.Gains;
import frc4388.utility.controller.IHandController;
public class ShooterHood extends SubsystemBase {
public Shooter m_shooterSubsystem;
public CANSparkMax m_angleAdjustMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless);
public static Gains m_angleAdjustGains = ShooterConstants.SHOOTER_ANGLE_GAINS;
public CANPIDController m_angleAdjustPIDController = m_angleAdjustMotor.getPIDController();
public CANEncoder m_angleEncoder = m_angleAdjustMotor.getEncoder();
public double m_fireAngle;
public CANDigitalInput m_hoodUpLimit, m_hoodDownLimit;
/**
* Creates a new ShooterHood.
*/
public ShooterHood() {
m_angleAdjustMotor.setIdleMode(IdleMode.kBrake);
m_hoodUpLimit = m_angleAdjustMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
m_hoodDownLimit = m_angleAdjustMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
m_hoodUpLimit.enableLimitSwitch(true);
m_hoodDownLimit.enableLimitSwitch(true);
m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, true);
m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.HOOD_UP_SOFT_LIMIT);
m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.HOOD_DOWN_SOFT_LIMIT);
}
@Override
public void periodic() {
// This method will be called once per scheduler run
SmartDashboard.putNumber("Fire Angle CSV", m_fireAngle);
}
/**
* Passes subsystem needed.
* @param subsystem Subsystem needed.
*/
public void passRequiredSubsystem(Shooter subsystem) {
m_shooterSubsystem = subsystem;
}
public double addFireAngle() {
return m_fireAngle;
}
/* Angle Adjustment PID Control */
public void runAngleAdjustPID(double targetAngle)
{
// Set PID Coefficients
m_angleAdjustPIDController.setP(m_angleAdjustGains.m_kP);
m_angleAdjustPIDController.setI(m_angleAdjustGains.m_kI);
m_angleAdjustPIDController.setD(m_angleAdjustGains.m_kD);
m_angleAdjustPIDController.setIZone(m_angleAdjustGains.m_kIzone);
m_angleAdjustPIDController.setFF(m_angleAdjustGains.m_kF);
m_angleAdjustPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_angleAdjustGains.m_kPeakOutput);
m_angleAdjustPIDController.setReference(targetAngle, ControlType.kPosition);
}
public void resetGyroAngleAdj(){
m_angleEncoder.setPosition(0);
}
public double getAnglePosition(){
return m_angleEncoder.getPosition();
}
}