diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 80a21ee..95209ee 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -82,6 +82,10 @@ public final class Constants { public static final int XBOX_DRIVER_ID = 0; public static final int XBOX_OPERATOR_ID = 1; } + + + + public static final class ShooterConstants { /* PID Constants Shooter */ public static final int SHOOTER_TIMEOUT_MS = 30; @@ -91,6 +95,10 @@ public final class Constants { public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.6, 0.0, 0.0, 0.0, 0, 1.0); public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.05, 0.0, 0.0, 0.0, 0, 0.3); public static final double SHOOTER_TURRET_MIN = -1.0; - + public static final int SHOOTER_FALCON_BALLER_ID =; //unknown value, fix later// + public static final int SHOOTER_FALCON_BALLER_FOLLOWER_ID =; //"// + + public static final Gains DRUM_SHOOTER_GAINS = new Gains(x,y,z,a,b);//x,y,z,a,b are not actual values, fix laterS + } } diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index ceb7569..6c84711 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -8,16 +8,42 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import edu.wpi.first.wpilibj.SmartDashboard.SmartDashboard; import edu.wpi.first.math.controller.BangBangController; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ShooterConstants; import frc4388.utility.ShooterTables; +import frc4388.utility.Gains; +import frc4388.utility.Trims; +import frc4388.utility.controller.IHandController; public class BoomBoom extends SubsystemBase { -public WPI_TalonFX m_shooterFalconLeft; -public WPI_TalonFX m_shooterFalconRight; +public WPI_TalonFX m_shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_BALLER_ID); +public WPI_TalonFX m_shooterFalconRight= new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_BALLER_FOLLOWER_ID); public ShooterTables m_shooterTable; +public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS; +public static BoomBoom m_boomBoom; +public static IHandController m_driverController; //not sure if driverController in 2022 = m_controller in 2020 +double velP; +double input; + +public boolean m_isDrumReady = false; +public double m_fireVel; + +public Trims shooterTrims; + +public Hood m_hoodSubsystem; +public Turret m_turretSubsystem; + +/* +* Creates new BoomBoom subsystem, has drum shooter and angle adjuster +*/ +public BoomBoom(){ +//Testing purposes resetting gyros +//resetGryoAngleADj(); +shooterTrims = new Trims(0,0); +} /** Creates a new BoomBoom. */ public BoomBoom(WPI_TalonFX shooterFalconLeft, WPI_TalonFX shooterFalconRight) { m_shooterFalconLeft = shooterFalconLeft; @@ -56,12 +82,42 @@ public ShooterTables m_shooterTable; public void periodic() { // This method will be called once per scheduler run // Abhi was here +try { + // SmartDashboard.putNumber("Drum Velocity", m_shooterFalconLeft.getSelectedSensorVelocity()); + + // SmartDashboard.putNumber("Drum Velocity CSV", m_fireVel); + + // SmartDashboard.putNumber("Shooter Temp C", m_shooterFalconLeft.getTemperature()); //all these values should be "defined" elsewhere, fix this + + // SmartDashboard.putNumber("Shooter Current", m_shooterFalconLeft.getSupplyCurrent()); + + // SmartDashboard.putNumber("Drum Ready", m_isDrumReady); +} catch (Exception e) { + //TODO: handle exception +} + + } + + public void passRequiredSubsystem(Hood subsystem0, Turret subsystem1) { + m_hoodSubsystem = subsystem0; + m_turretSubsystem = subsystem1; + } + + public double addFireVel(){ + return m_fireVel; } public void runDrumShooter(double speed) { m_shooterFalconLeft.set(TalonFXControlMode.PercentOutput, speed); m_shooterFalconRight.follow(m_shooterFalconLeft); } +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); +} public void runDrumShooterVelocityPID(double targetVel) { m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel); //Init diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index afbb9c5..f6cc008 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -6,10 +6,40 @@ package frc4388.robot.subsystems; import edu.wpi.first.wpilibj2.command.SubsystemBase; -public class Vision extends SubsystemBase { - /** Creates a new Vision. */ - public Vision() {} + + +public class Vision extends SubsystemBase { +//setup + Turret m_turret; +BoomBoom m_boomBoom; +Hood m_hood; + +NetworkTableEntry xEntry; +IHandController m_driverController; +//Aiming +double turnAmount = 0; +double xAngle = 0; +double yAngle = 0; +double target = 0; +public double distance; +public double realDistance; +public static double fireVel; +public static double fireAngle; + +public double m_hoodTrim; +public double m_turretTrim; + +LimeLight m_limeLight; + +public TrackTarget(ShooterAim aimSubsystem, LimeLight limeLight) { + m_turret = turretSubsystem; + m_boomBoom = m_turret.m_turretSubsystem; + m_hood = m_boomBoom.m_hoodSubsystem; + m_limeLight = limeLight; + addRequirements(m_turret); + +} @Override public void periodic() { // This method will be called once per scheduler run