diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 52b1ddd..3f0dfef 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -106,14 +106,19 @@ public final class Constants { 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 later + /* Hood Constants */ + public static final int SHOOTER_ANGLE_ADJUST_ID = 10; + public static final double HOOD_MOTOR_ROTS_PER_ROT = 1; //TODO: Find + public static final double HOOD_MOTOR_POS_AT_ZERO_ROT = 0; //TODO: Find - - public static final class VisionConstants { - public static final double TURN_P_VALUE = "unknown" // needs to be figured out after testing - public static final double X_ANGLE_ERROR = "alsoUnknown" //""// - public static final double TURN_P_VALUE = "alsoAlsoUnknown" //""// - public static final double GRAV = "alsoAlsoAlsoUnknown" //""// - public static final double TARGET_HEIGHT = "alsoAlsoAlsoAlsoUnknown" //""// + } + public static final class VisionConstants { + public static final double TURN_P_VALUE = 0.8; + public static final double X_ANGLE_ERROR = 0.5; + public static final double GRAV = 385.83; + public static final double TARGET_HEIGHT = 67.5; + public static final double FOV = 29.8; //Field of view limelight + public static final double LIME_ANGLE = 24.7; @@ -123,6 +128,4 @@ public final class Constants { } - - } } diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index 6c84711..8a7a485 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -14,7 +14,6 @@ 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 { @@ -127,7 +126,7 @@ public void setShooterGains() { //Controls a motor with the output of the BangBang controller //Controls a motor with the output of the BangBang conroller and a feedforward //Shrinks the feedforward slightly to avoid over speeding the shooter - motor.set(controller.calculate(encoder.getRate(), setpoint) + 0.9 * feedforward.calculate(setpoint)); */ + m_shooterFalconLeft.set(controller.calculate(encoder.getRate(), targetVel) + 0.9 * feedforward.calculate(targetVel)); */ } diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index 76ba03d..3785439 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -6,9 +6,10 @@ package frc4388.robot.subsystems; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.wpilibj2.command.commandBase; -import frc4388.robot.constants.VisionConstants; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.VisionConstants; import frc4388.robot.subsystems.BoomBoom; import frc4388.robot.subsystems.Turret; import frc4388.robot.subsystems.Hood; @@ -35,14 +36,11 @@ 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; +public Vision(Turret aimSubsystem, BoomBoom boomBoom) { + m_turret = aimSubsystem; + m_boomBoom = boomBoom; m_hood = m_boomBoom.m_hoodSubsystem; - m_limeLight = limeLight; - addRequirements(m_turret); + //addRequirements(m_turret); limeOff(); changePipeline(0); NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0); @@ -50,9 +48,9 @@ public TrackTarget(ShooterAim aimSubsystem, LimeLight limeLight) { } public void track(){ - target = getV() - xAngle = getX() - yAngle = getY() + target = getV(); + xAngle = getX(); + yAngle = getY(); //find distance distance = (VisionConstants.TARGET_HEIGHT) / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180)); @@ -68,10 +66,10 @@ public void track(){ turnAmount = 0.1; } else if (turnAmount < 0 && turnAmount > -0.1){ - turnAmount = -0.1 + turnAmount = -0.1; } } - m_turret.runShooterWithInput(-turnAmount); + m_turret.turnWithJoystick(-turnAmount); SmartDashboard.putNumber("Disance to Target", realDistance);