Team4388/RiseOfRidgebotics2020
1
0
Fork 0
mirror of https://github.com/Team4388/RiseOfRidgebotics2020.git synced 2026-06-09 08:48:01 -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/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();
}
}