From 0b77117f50bfa47e69b0e83a65c6b6b2dd3515aa Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Sat, 22 Feb 2020 11:23:57 -0700 Subject: [PATCH] Shooter Groups Framework --- src/main/java/frc4388/robot/Constants.java | 3 +- .../robot/commands/ShootFireGroup.java | 24 ++++++++++++++ .../robot/commands/ShootFullGroup.java | 25 +++++++++++++++ .../robot/commands/ShootPrepGroup.java | 23 +++++++++++++ .../frc4388/robot/subsystems/Shooter.java | 32 +++++++++++++++++-- .../frc4388/robot/subsystems/ShooterAim.java | 30 ----------------- 6 files changed, 103 insertions(+), 34 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/ShootFireGroup.java create mode 100644 src/main/java/frc4388/robot/commands/ShootFullGroup.java create mode 100644 src/main/java/frc4388/robot/commands/ShootPrepGroup.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 8852e44..3a44301 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -91,7 +91,8 @@ public final class Constants { public static final int SHOOTER_PID_LOOP_IDX = 1; public static final int SHOOTER_TIMEOUT_MS = 30; public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.4, 0.0005, 13, 0.05, 0, 1.0); - public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0); public static final double SHOOTER_TURRET_MIN = -1.0; public static final double ENCODER_TICKS_PER_REV = 2048; public static final double NEO_UNITS_PER_REV = 42; diff --git a/src/main/java/frc4388/robot/commands/ShootFireGroup.java b/src/main/java/frc4388/robot/commands/ShootFireGroup.java new file mode 100644 index 0000000..155e384 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/ShootFireGroup.java @@ -0,0 +1,24 @@ +/*----------------------------------------------------------------------------*/ +/* 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.commands; + +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html +public class ShootFireGroup extends ParallelRaceGroup { + /** + * Creates a new ShootFireGroup. + */ + public ShootFireGroup() { + // Add your commands in the super() call, e.g. + // super(new FooCommand(), new BarCommand()); + super(); + } +} diff --git a/src/main/java/frc4388/robot/commands/ShootFullGroup.java b/src/main/java/frc4388/robot/commands/ShootFullGroup.java new file mode 100644 index 0000000..1bb7956 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/ShootFullGroup.java @@ -0,0 +1,25 @@ +/*----------------------------------------------------------------------------*/ +/* 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.commands; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html +public class ShootFullGroup extends SequentialCommandGroup { + /** + * Creates a new ShootFullGroup. + */ + public ShootFullGroup() { + super( + new ShootPrepGroup(), + new ShootFireGroup() + ); + } +} diff --git a/src/main/java/frc4388/robot/commands/ShootPrepGroup.java b/src/main/java/frc4388/robot/commands/ShootPrepGroup.java new file mode 100644 index 0000000..58ab9f1 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/ShootPrepGroup.java @@ -0,0 +1,23 @@ +/*----------------------------------------------------------------------------*/ +/* 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.commands; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html +public class ShootPrepGroup extends ParallelCommandGroup { + /** + * Creates a new ShootPrepGroup. + */ + public ShootPrepGroup() { + // Add your commands in the super() call, e.g. + // super(new FooCommand(), new BarCommand());super(); + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index 37be2a5..ecd2a88 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -29,12 +29,16 @@ 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(); + 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; - + double velP; double input; public boolean velReached; @@ -46,11 +50,11 @@ public class Shooter extends SubsystemBase { */ public Shooter() { //Testing purposes reseting gyros - + resetGyroAngleAdj(); m_shooterFalcon.configFactoryDefault(); m_shooterFalcon.setNeutralMode(NeutralMode.Coast); m_shooterFalcon.setInverted(false); - + m_angleAdjustMotor.setIdleMode(IdleMode.kBrake); setShooterGains(); m_shooterFalcon.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS); @@ -94,6 +98,23 @@ 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); + + // Convert input angle in degrees to rotations of the motor + targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT; + + m_angleAdjustPIDController.setReference(targetAngle, ControlType.kPosition); + } + /** * Runs drum shooter velocity PID. * @param falcon Motor to use @@ -123,4 +144,9 @@ public class Shooter extends SubsystemBase { velReached = false; } } + + public void resetGyroAngleAdj() + { + m_angleEncoder.setPosition(0); + } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/ShooterAim.java b/src/main/java/frc4388/robot/subsystems/ShooterAim.java index 9de24bd..1640937 100644 --- a/src/main/java/frc4388/robot/subsystems/ShooterAim.java +++ b/src/main/java/frc4388/robot/subsystems/ShooterAim.java @@ -13,53 +13,29 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.ControlType; import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMaxLowLevel.MotorType; - import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Gains; import frc4388.robot.Constants.ShooterConstants; public class ShooterAim extends SubsystemBase { - public CANSparkMax m_angleAdjustMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); public CANSparkMax m_shooterRotateMotor = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, MotorType.kBrushless); - public static Gains m_shooterTurretGains = ShooterConstants.SHOOTER_TURRET_GAINS; // Configure PID Controllers - CANPIDController m_angleAdjustPIDController = m_angleAdjustMotor.getPIDController(); CANPIDController m_shooterRotatePIDController = m_shooterRotateMotor.getPIDController(); - - CANEncoder m_angleEncoder = m_angleAdjustMotor.getEncoder(); CANEncoder m_shooterRotateEncoder = m_shooterRotateMotor.getEncoder(); /** * Creates a new ShooterAim. */ public ShooterAim() { - resetGyroAngleAdj(); resetGyroShooterRotate(); m_shooterRotateMotor.setIdleMode(IdleMode.kBrake); - m_angleAdjustMotor.setIdleMode(IdleMode.kBrake); } public void runShooterWithInput(double input) { m_shooterRotateMotor.set(input); } - /* Angle Adjustment PID Control */ - public void runAngleAdjustPID(double targetAngle) - { - // Set PID Coefficients - m_angleAdjustPIDController.setP(m_shooterTurretGains.m_kP); - m_angleAdjustPIDController.setI(m_shooterTurretGains.m_kI); - m_angleAdjustPIDController.setD(m_shooterTurretGains.m_kD); - m_angleAdjustPIDController.setIZone(m_shooterTurretGains.m_kIzone); - m_angleAdjustPIDController.setFF(m_shooterTurretGains.m_kF); - m_angleAdjustPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTurretGains.m_kPeakOutput); - - // Convert input angle in degrees to rotations of the motor - targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT; - - m_angleAdjustPIDController.setReference(targetAngle, ControlType.kPosition); - } /* Rotate Shooter PID Control */ public void runshooterRotatePID(double targetAngle) @@ -78,12 +54,6 @@ public class ShooterAim extends SubsystemBase { m_shooterRotatePIDController.setReference(targetAngle, ControlType.kPosition); } - - /* For Testing Purposes, reseting gyro for angle adjuster */ - public void resetGyroAngleAdj() - { - m_angleEncoder.setPosition(0); - } /* For Testing Purposes, reseting gyro for shooter rotation */ public void resetGyroShooterRotate()