diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index f81591d..8f67b12 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -93,7 +93,7 @@ public final class Constants { //public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.4, 0.0005, 13, 0.05, 0, 1.0); public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.0, 0.0, 0, 0.0453, 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 Gains SHOOTER_ANGLE_GAINS = new Gains(0.5, 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/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 5339635..b65ee53 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -59,6 +59,7 @@ import frc4388.robot.commands.StorageOutake; import frc4388.robot.commands.StoragePrepAim; import frc4388.robot.subsystems.Camera; import frc4388.robot.subsystems.Leveler; +import frc4388.robot.subsystems.LimeLight; import frc4388.robot.subsystems.Storage; import frc4388.utility.LEDPatterns; import frc4388.utility.controller.IHandController; @@ -85,6 +86,7 @@ public class RobotContainer { /* Cameras */ private final Camera m_robotCameraFront = new Camera("front", 0, 160, 120, 40); private final Camera m_robotCameraBack = new Camera("back", 1, 160, 120, 40); + private final LimeLight m_robotLime = new LimeLight(); /* Controllers */ private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); @@ -111,6 +113,8 @@ public class RobotContainer { m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getDriverController())); // moves the drum not m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooter(0), m_robotShooter)); + //turns limelight off + //m_robotLime.setDefaultCommand(new RunCommand(() -> m_robotLime.limeOff(), m_robotLime)); } @@ -140,10 +144,11 @@ public class RobotContainer { // shoots one ball new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON) - .whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false) - .whenReleased(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(0, m_robotShooter.m_shooterFalcon.getSelectedSensorVelocity()))); - //.whenReleased(new InstantCommand(() -> m_robotShooterAim.runShooterWithInput(0), m_robotShooterAim)); - + .whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false); + //.whenReleased(new RunCommand(() -> m_robotLime.limeOff())); + //.whenReleased(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(0, m_robotShooter.m_shooterFalcon.getSelectedSensorVelocity()))) + //.whenReleased(new RunCommand(() -> m_robotShooterAim.limeOff())) + //.whenReleased(new RunCommand(() -> m_robotShooterAim.runShooterWithInput(0))); // extends or retracts the extender new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) .whenPressed(new RunExtenderOutIn(m_robotIntake)); diff --git a/src/main/java/frc4388/robot/commands/HoldTarget.java b/src/main/java/frc4388/robot/commands/HoldTarget.java index 8449078..cbe43cf 100644 --- a/src/main/java/frc4388/robot/commands/HoldTarget.java +++ b/src/main/java/frc4388/robot/commands/HoldTarget.java @@ -10,6 +10,7 @@ package frc4388.robot.commands; import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.Constants.VisionConstants; import frc4388.robot.subsystems.Drive; +import frc4388.robot.subsystems.LimeLight; import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.ShooterAim; import frc4388.utility.controller.IHandController; @@ -48,8 +49,7 @@ public class HoldTarget extends CommandBase { @Override public void initialize() { //Vision Processing Mode - NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0); - NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3); + LimeLight.limeOn(); } @@ -89,8 +89,7 @@ public class HoldTarget extends CommandBase { @Override public void end(boolean interrupted) { //Drive Camera Mode - NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1); - NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(0); + LimeLight.limeOff(); } // Returns true when the command should end. diff --git a/src/main/java/frc4388/robot/commands/TrackTarget.java b/src/main/java/frc4388/robot/commands/TrackTarget.java index db5e3a1..647341b 100644 --- a/src/main/java/frc4388/robot/commands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/TrackTarget.java @@ -9,23 +9,26 @@ package frc4388.robot.commands; import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.Constants.VisionConstants; +import frc4388.robot.subsystems.Camera; import frc4388.robot.subsystems.Drive; +import frc4388.robot.subsystems.LimeLight; 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.wpilibj2.command.RunCommand; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class TrackTarget extends CommandBase { - //Setup + // Setup NetworkTableEntry xEntry; ShooterAim m_shooterAim; Shooter m_shooter; IHandController m_driverController; - //Aiming + // Aiming double turnAmount = 0; double xAngle = 0; double yAngle = 0; @@ -47,38 +50,40 @@ public class TrackTarget extends CommandBase { // Called when the command is initially scheduled. @Override public void initialize() { - //Vision Processing Mode - NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0); - NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3); + // Vision Processing Mode + LimeLight.limeOn(); } - - // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { target = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0); xAngle = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0); yAngle = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0); - - if (target == 1.0){ //If target in view - //Aiming Left/Right - turnAmount = (xAngle/VisionConstants.FOV)*VisionConstants.TURN_P_VALUE; - if (Math.abs(xAngle) < VisionConstants.X_ANGLE_ERROR){turnAmount = 0;} //Angle Error Zone - //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_shooterAim.runShooterWithInput(turnAmount/3); - //Finding Distance - distance = VisionConstants.TARGET_HEIGHT/Math.tan((VisionConstants.LIME_ANGLE + yAngle)*(Math.PI/180)); + if (target == 1.0) { // If target in view + // Aiming Left/Right + turnAmount = (xAngle / VisionConstants.FOV) * VisionConstants.TURN_P_VALUE; + if (Math.abs(xAngle) < VisionConstants.X_ANGLE_ERROR) { + turnAmount = 0; + } // Angle Error Zone + // 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_shooterAim.runShooterWithInput(turnAmount / 3); + + // Finding Distance + distance = VisionConstants.TARGET_HEIGHT / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180)); SmartDashboard.putNumber("Distance to Target", distance); - double yVel = Math.sqrt(2*VisionConstants.GRAV*VisionConstants.TARGET_HEIGHT); - double xVel = (distance*VisionConstants.GRAV)/(yVel); + double yVel = Math.sqrt(2 * VisionConstants.GRAV * VisionConstants.TARGET_HEIGHT); + double xVel = (distance * VisionConstants.GRAV) / (yVel); - fireVel = Math.sqrt((Math.pow(xVel, 2))+(Math.pow(yVel,2))); - fireAngle = Math.atan(yVel/xVel); + fireVel = Math.sqrt((Math.pow(xVel, 2)) + (Math.pow(yVel, 2))); + fireAngle = Math.atan(yVel / xVel); m_shooter.m_fireVel = fireVel; m_shooter.m_fireAngle = fireAngle; @@ -88,9 +93,8 @@ public class TrackTarget extends CommandBase { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - //Drive Camera Mode - NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1); - NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(0); + // Drive Camera Mode + LimeLight.limeOff(); } // Returns true when the command should end. diff --git a/src/main/java/frc4388/robot/subsystems/Camera.java b/src/main/java/frc4388/robot/subsystems/Camera.java index e483009..332bb46 100644 --- a/src/main/java/frc4388/robot/subsystems/Camera.java +++ b/src/main/java/frc4388/robot/subsystems/Camera.java @@ -11,6 +11,7 @@ import edu.wpi.cscore.MjpegServer; import edu.wpi.cscore.UsbCamera; import edu.wpi.cscore.VideoSource; import edu.wpi.first.cameraserver.CameraServer; +import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -38,8 +39,7 @@ public class Camera extends SubsystemBase { catch(Exception e){ System.err.println("Camera broken, pls nerf"); } - - } + } @Override public void periodic() { diff --git a/src/main/java/frc4388/robot/subsystems/LimeLight.java b/src/main/java/frc4388/robot/subsystems/LimeLight.java new file mode 100644 index 0000000..70c53ae --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/LimeLight.java @@ -0,0 +1,35 @@ +/*----------------------------------------------------------------------------*/ +/* 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 edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class LimeLight extends SubsystemBase { + /** + * Creates a new LimeLight. + */ + public LimeLight() { + + } + + public static void limeOff(){ + NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1); + NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(0); + } + + public static void limeOn(){ + NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0); + NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index 3de7f36..8648ff9 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -100,9 +100,9 @@ public class Shooter extends SubsystemBase { } /* Angle Adjustment PID Control */ - public void runAngleAdjustPID(double targetAngle) + public void runAngleAdjustPID(double mmtargetAngle) { - targetAngle = addFireAngle(); + double targetAngle = addFireAngle(); // Set PID Coefficients m_angleAdjustPIDController.setP(m_angleAdjustGains.m_kP); m_angleAdjustPIDController.setI(m_angleAdjustGains.m_kI); diff --git a/src/main/java/frc4388/robot/subsystems/ShooterAim.java b/src/main/java/frc4388/robot/subsystems/ShooterAim.java index 1640937..b6ffef2 100644 --- a/src/main/java/frc4388/robot/subsystems/ShooterAim.java +++ b/src/main/java/frc4388/robot/subsystems/ShooterAim.java @@ -13,6 +13,8 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.ControlType; import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMaxLowLevel.MotorType; + +import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Gains; import frc4388.robot.Constants.ShooterConstants; diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index 3d460fe..5dbcc44 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -94,4 +94,15 @@ public class Storage extends SubsystemBase { public void setStoragePID(double position){ m_storagePIDController.setReference(position , ControlType.kPosition); } + + + /* + If pressing aim + Run until hitting bottom beam + dont run intake if balls not at bottom + else + run unti; hitting top beam + + 2 beamms total + */ }