diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 118740a..2bb8bb7 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -94,8 +94,14 @@ public final class Constants { public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = new SupplyCurrentLimitConfiguration(true, 60, 40, 0.5); public static final int SHOOTER_FALCON_LEFT_CAN_ID = 0; public static final int SHOOTER_FALCON_RIGHT_CAN_ID = 0; - public static final int SHOOTER_ROTATE_ID = "unknown value" //figure out later - public static final int SHOOTER_TURRET_GAINS = "'" //""// + public static final int SHOOTER_ROTATE_ID = 0; //"unknown value" //figure out later + public static final int TURRET_RIGHT_SOFT_LIMIT = 0; //""// + public static final int TURRET_SPEED_MULTIPLIER = 0; //""// + public static final int DEGREES_PER_ROT = 0; //""// + public static final int TURRET_MOTOR_POS_AT_ZERO_ROT = 0; //""// + public static final int TURRET_MOTOR_ROTS_PER_ROT = 0; //""// + + } /* Turret Constants */ //ID @@ -104,10 +110,10 @@ public final class Constants { 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 int SHOOTER_FALCON_BALLER_ID = 0; //unknown value, fix later// + public static final int SHOOTER_FALCON_BALLER_FOLLOWER_ID = 0; //"// - 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 + public static final Gains DRUM_SHOOTER_GAINS = new Gains(0,0,0,0,0);//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 diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 428fe00..48a6556 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -4,21 +4,98 @@ package frc4388.robot.subsystems; +import edu.wpi.first.wpilibj.drive.RobotDriveBase.MotorType; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.ShooterConstants; +import frc4388.utility.Gains; public class Turret extends SubsystemBase { /** Creates a new Turret. */ - public CANSparkMax m_turretMotor; + public BoomBoom m_boomBoomSubsystem; + public SwerveDrive m_sDriveSubsystem; + + public CAnsparkMax m_boomBoomRotateMotor = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, MotorType.kBrushless); + public static Gains m_shooterTGains = ShooterConstants.SHOOTER_TURRET_GAINS; + CANDigitalInput m_boomBoomRightLimit, m_boomBoomLeftLimit; + public GyroBase m_turretGyro; + + public double m_targetDistance = 0; + + public boolean m_isAimReady = false; + + CANPIDController m_boomBoomRotatePIDController = m_boomBoomRotateMotor.getPIDController(); + public CANEncoder m_boomBoomRotateEncoder = m_shooterRotateMotor.getEncoder(); + + public + public CANSparkMax m_turretMotor; + public boolean m_isAimReady; + public double m_targetDistance; //Variables - public Turret() { + public Turret() { + + m_boomBoomRotateMotor.setIdleMode(IdleMode.kBrake); + + m_turretGyro = getGyroInterface(); + m_boomBoomLeftLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen); + m_boomBoomRightLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen); + m_boomBoomRightLimit.enableLimitSwitch(true); + m_boomBoomLeftLimit.enableLimitSwitch(true); + + m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, false); + m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); + m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.TURRET_RIGHT_SOFT_LIMIT); + + m_boomBoomRotateMotor.setInverted(false); m_turretMotor = turretMotor; } @Override public void periodic() { + SmartDashboard.putNumber("Turret Angle Raw", getboomBoomRotatePosition()); + + SmartDashboard.putData("Turret Angle", m_turretGyro); + + SmartDashboard.putBoolean("Turret Aimed", m_isAimReady); // This method will be called once per scheduler run } + public void passRequiredSubsystem( BoomBoom subsystem0, SwerveDrive subsystem1){ + m_boomBoomSubsystem = subsystem0; + m_sDriveSubsystem = subsystem1; + } + + public void runShooterWithInput(double input) { + m_boomBoomRotateMotor.set(input*ShooterConstants.TURRET_SPEED_MULTIPLIER); + } + + public void runshooterRotatePID(double targetAngle) { + m_boomBoomRotatePIDController.setP(m_shooterTGains.m_kP); + m_boomBoomRotatePIDController.setI(m_shooterTGains.m_kI); + m_boomBoomRotatePIDController.setD(m_shooterTGains.m_kD); + m_boomBoomRotatePIDController.setFF(m_shooterTGains.m_kF); + m_boomBoomRotatePIDController.setIZone(m_shooterTGains.m_kIzone); + m_boomBoomRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTGains.m_kPeakOutput); + + targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT; + + m_boomBoomRotatePIDController.setReference(targetAngle,ControlType.kPosition); + } + + public void resetGyroShooterRotate() + { + m_boomBoomRotateEncoder.setPosition(0); + } + + public double getboomBoomRotatePosition() + { + return m_boomBoomRotateEncoder.getPosition(); + } + + public double getAnglePositionDegrees() { + return (m_boomBoomRotateEncoder.getPosition() - ShooterConstants.TURRET_MOTOR_POS_AT_ZERO_ROT) * 360/ShooterConstants. TURRET_MOTOR_ROTS_PER_ROT; + } + public void turnWithJoystick (double input) { m_turretMotor.set(input); diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index 3785439..85ee3e6 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -98,7 +98,7 @@ public void checkFinished(){ } public void limeOff(){ - NetworkTableInstance.getDefault.getTable("limelight").getEntry("camMode").setNumber(0); + NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0); NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(1); }