diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index f14c5d9..de8c407 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -44,13 +44,14 @@ import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.Shooter; +import frc4388.robot.subsystems.ShooterAim; import frc4388.robot.subsystems.Climber; import frc4388.robot.commands.RunLevelerWithJoystick; import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.LED; import frc4388.robot.commands.TrackTarget; -import frc4388.robot.commands.storageOutake; +import frc4388.robot.commands.StorageOutake; import frc4388.robot.subsystems.Camera; import frc4388.robot.subsystems.Leveler; import frc4388.robot.subsystems.Storage; @@ -71,6 +72,7 @@ public class RobotContainer { private final LED m_robotLED = new LED(); private final Intake m_robotIntake = new Intake(); private final Shooter m_robotShooter = new Shooter(); + private final ShooterAim m_robotShooterAim = new ShooterAim(); private final Climber m_robotClimber = new Climber(); private final Leveler m_robotLeveler = new Leveler(); private final Storage m_robotStorage = new Storage(); @@ -99,7 +101,7 @@ public class RobotContainer { // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); // runs the drum shooter in idle mode - m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runShooterWithInput(m_operatorXbox.getLeftXAxis()), m_robotShooter)); + m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooterAim.runShooterWithInput(m_operatorXbox.getLeftXAxis()), m_robotShooter)); // drives the leveler with an axis input from the driver controller m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getDriverController())); @@ -137,7 +139,7 @@ public class RobotContainer { // aims the turret new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) - .whileHeld(new TrackTarget(m_robotShooter)); + .whileHeld(new TrackTarget(m_robotShooter, m_robotShooterAim)); //.whenPressed(new RunCommand(() -> m_robotStorage.storeAim())); // extends or retracts the extender @@ -151,7 +153,7 @@ public class RobotContainer { /* Storage Neo PID Test */ new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) - .whileHeld(new TrackTarget(m_robotShooter)); + .whileHeld(new TrackTarget(m_robotShooter, m_robotShooterAim)); //Prepares storage for intaking new JoystickButton(getOperatorJoystick(), XboxController.LEFT_TRIGGER_AXIS) @@ -159,7 +161,7 @@ public class RobotContainer { //Runs storage to outtake new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_TRIGGER_AXIS) - .whileHeld(new storageOutake(m_robotStorage)); + .whileHeld(new StorageOutake(m_robotStorage)); } /** diff --git a/src/main/java/frc4388/robot/commands/StorageIntakeGroup.java b/src/main/java/frc4388/robot/commands/StorageIntakeGroup.java index 767ed69..4b6cfee 100644 --- a/src/main/java/frc4388/robot/commands/StorageIntakeGroup.java +++ b/src/main/java/frc4388/robot/commands/StorageIntakeGroup.java @@ -20,7 +20,7 @@ public class StorageIntakeGroup extends SequentialCommandGroup { */ public StorageIntakeGroup(Intake m_intake, Storage m_storage) { addCommands( - new storagePrepIntake(m_intake, m_storage), + new StoragePrepIntake(m_intake, m_storage), new storageIntake(m_intake, m_storage), new StorageIntakeFinal(m_storage)); } diff --git a/src/main/java/frc4388/robot/commands/TrackTarget.java b/src/main/java/frc4388/robot/commands/TrackTarget.java index 8102f13..374f04c 100644 --- a/src/main/java/frc4388/robot/commands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/TrackTarget.java @@ -11,6 +11,7 @@ import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.Constants.VisionConstants; import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Shooter; +import frc4388.robot.subsystems.ShooterAim; import frc4388.utility.controller.IHandController; import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.networktables.NetworkTable; @@ -21,6 +22,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class TrackTarget extends CommandBase { //Setup NetworkTableEntry xEntry; + ShooterAim m_shooterAim; Shooter m_shooter; IHandController m_driverController; //Aiming @@ -35,7 +37,8 @@ public class TrackTarget extends CommandBase { /** * Uses the Limelight to track the target */ - public TrackTarget(Shooter shooterSubsystem) { + public TrackTarget(Shooter shooterSubsystem, ShooterAim aimSubsystem) { + m_shooterAim = aimSubsystem; m_shooter = shooterSubsystem; addRequirements(m_shooter); NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1); @@ -65,7 +68,7 @@ public class TrackTarget extends CommandBase { //Deadzones else if(turnAmount > 0 && turnAmount < VisionConstants.MOTOR_DEAD_ZONE){turnAmount = VisionConstants.MOTOR_DEAD_ZONE;} else if(turnAmount < 0 && turnAmount > -VisionConstants.MOTOR_DEAD_ZONE){turnAmount = -VisionConstants.MOTOR_DEAD_ZONE;} - m_shooter.runShooterWithInput(turnAmount/5); + m_shooterAim.runShooterWithInput(turnAmount/5); //Finding Distance distance = VisionConstants.TARGET_HEIGHT/Math.tan((VisionConstants.LIME_ANGLE + yAngle)*(Math.PI/180)); diff --git a/src/main/java/frc4388/robot/commands/storageOutake.java b/src/main/java/frc4388/robot/commands/storageOutake.java index 4820dc0..48a28ba 100644 --- a/src/main/java/frc4388/robot/commands/storageOutake.java +++ b/src/main/java/frc4388/robot/commands/storageOutake.java @@ -10,12 +10,12 @@ package frc4388.robot.commands; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.subsystems.Storage; -public class storageOutake extends CommandBase { +public class StorageOutake extends CommandBase { Storage m_storage; /** * Creates a new storageOutake. */ - public storageOutake(Storage storeSub) { + public StorageOutake(Storage storeSub) { m_storage = storeSub; addRequirements(m_storage); } diff --git a/src/main/java/frc4388/robot/commands/storagePrepAim.java b/src/main/java/frc4388/robot/commands/storagePrepAim.java index c8b3bad..1449ced 100644 --- a/src/main/java/frc4388/robot/commands/storagePrepAim.java +++ b/src/main/java/frc4388/robot/commands/storagePrepAim.java @@ -10,12 +10,12 @@ package frc4388.robot.commands; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.subsystems.Storage; -public class storagePrepAim extends CommandBase { +public class StoragePrepAim extends CommandBase { Storage m_storage; /** * Creates a new storagePrepAim. */ - public storagePrepAim(Storage storeSub) { + public StoragePrepAim(Storage storeSub) { m_storage = storeSub; addRequirements(m_storage); } diff --git a/src/main/java/frc4388/robot/commands/storagePrepIntake.java b/src/main/java/frc4388/robot/commands/storagePrepIntake.java index 7fab016..3c0d1b5 100644 --- a/src/main/java/frc4388/robot/commands/storagePrepIntake.java +++ b/src/main/java/frc4388/robot/commands/storagePrepIntake.java @@ -11,13 +11,13 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.Storage; -public class storagePrepIntake extends CommandBase { +public class StoragePrepIntake extends CommandBase { public Intake m_intake; public Storage m_storage; /** * Creates a new storagePrepIntake. */ - public storagePrepIntake(Intake inSub, Storage storeSub) { + public StoragePrepIntake(Intake inSub, Storage storeSub) { m_intake = inSub; m_storage = storeSub; addRequirements(m_intake); diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index c4487ce..37be2a5 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -29,23 +29,12 @@ 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); - public CANSparkMax m_shooterRotateMotor = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, MotorType.kBrushless); - public static Gains m_shooterTurretGains = ShooterConstants.SHOOTER_TURRET_GAINS; public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS; public static Shooter m_shooter; public static IHandController m_controller; - - // 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(); - double velP; double input; public boolean velReached; @@ -57,11 +46,8 @@ public class Shooter extends SubsystemBase { */ public Shooter() { //Testing purposes reseting gyros - resetGyroAngleAdj(); - resetGyroShooterRotate(); m_shooterFalcon.configFactoryDefault(); - m_shooterRotateMotor.setIdleMode(IdleMode.kBrake); m_shooterFalcon.setNeutralMode(NeutralMode.Coast); m_shooterFalcon.setInverted(false); @@ -102,11 +88,12 @@ public class Shooter extends SubsystemBase { */ public void setShooterGains() { m_shooterFalcon.selectProfileSlot(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.SHOOTER_PID_LOOP_IDX); - m_shooterFalcon.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterTurretGains.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterFalcon.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterTurretGains.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterFalcon.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterTurretGains.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterFalcon.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterTurretGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalcon.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalcon.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalcon.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalcon.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS); } + /** * Runs drum shooter velocity PID. * @param falcon Motor to use @@ -136,55 +123,4 @@ public class Shooter extends SubsystemBase { velReached = false; } } - - - 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) - { - // Set PID Coefficients - m_shooterRotatePIDController.setP(m_shooterTurretGains.m_kP); - m_shooterRotatePIDController.setI(m_shooterTurretGains.m_kI); - m_shooterRotatePIDController.setD(m_shooterTurretGains.m_kD); - m_shooterRotatePIDController.setFF(m_shooterTurretGains.m_kF); - m_shooterRotatePIDController.setIZone(m_shooterTurretGains.m_kIzone); - m_shooterRotatePIDController.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_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() - { - m_shooterRotateEncoder.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 new file mode 100644 index 0000000..9de24bd --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/ShooterAim.java @@ -0,0 +1,98 @@ +/*----------------------------------------------------------------------------*/ +/* 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.CANEncoder; +import com.revrobotics.CANPIDController; +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) + { + // Set PID Coefficients + m_shooterRotatePIDController.setP(m_shooterTurretGains.m_kP); + m_shooterRotatePIDController.setI(m_shooterTurretGains.m_kI); + m_shooterRotatePIDController.setD(m_shooterTurretGains.m_kD); + m_shooterRotatePIDController.setFF(m_shooterTurretGains.m_kF); + m_shooterRotatePIDController.setIZone(m_shooterTurretGains.m_kIzone); + m_shooterRotatePIDController.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_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() + { + m_shooterRotateEncoder.setPosition(0); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +}