diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 5cc4947..f3dfd97 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -219,7 +219,7 @@ public final class Constants { // 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; + public static final double LIME_ANGLE = 50; public static final String NAME = "photonCamera"; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 313a0d3..c9a3ea2 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -104,7 +104,7 @@ public class RobotContainer { public final Storage m_robotStorage = new Storage(m_robotMap.storageMotor); // private final LED m_robotLED = new LED(m_robotMap.LEDController); - // public final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight); + public final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight); // public final Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor); public final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); private final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret); @@ -151,7 +151,7 @@ public class RobotContainer { //getDriverController().getRightX(), getDriverController().getRightX(), // getDriverController().getRightY(), - false), + true), m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand")); // Intake with Triggers m_robotIntake.setDefaultCommand( @@ -251,10 +251,10 @@ public class RobotContainer { // .whileHeld(new RunCommand(() -> m_robotHood.runAngleAdjustPID(-55.55))) // .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); - // new JoystickButton(getOperatorController(), XboxController.Button.kX.value) //20ft - // .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.525))) + new JoystickButton(getOperatorController(), XboxController.Button.kX.value) //20ft + .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.1))) // .whileHeld(new RunCommand(() -> m_robotHood.runAngleAdjustPID(-96))) - // .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); + .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value) .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(-0.9), m_robotStorage)) diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 03c7e03..5caebaa 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -36,7 +36,7 @@ public class RobotMap { public RobotMap() { // configureLEDMotorControllers(); configureSwerveMotorControllers(); - // configureShooterMotorControllers(); + configureShooterMotorControllers(); configureIntakeMotors(); configureExtenderMotors(); configureSerializerMotors(); @@ -190,59 +190,60 @@ public class RobotMap { // // Shooter Config // /* Boom Boom Subsystem */ -// public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID); -// public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID); + public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID); + public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID); // // turret subsystem public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless); -// // hood subsystem -// public CANSparkMax angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); + // hood subsystem + public CANSparkMax angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); -// // Create motor CANSparkMax -// void configureShooterMotorControllers() { + // Create motor CANSparkMax + void configureShooterMotorControllers() { -// // LEFT FALCON -// shooterFalconLeft.configFactoryDefault(); -// shooterFalconLeft.setNeutralMode(NeutralMode.Coast); -// shooterFalconLeft.setInverted(true); -// shooterFalconLeft.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); -// shooterFalconLeft.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); -// shooterFalconLeft.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS); -// shooterFalconLeft.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, -// ShooterConstants.SHOOTER_TIMEOUT_MS); -// shooterFalconLeft.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, -// ShooterConstants.SHOOTER_TIMEOUT_MS); -// shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG_SHOOTER, -// ShooterConstants.SHOOTER_TIMEOUT_MS); -// shooterFalconLeft.configStatorCurrentLimit(ShooterConstants.STATOR_CURRENT_LIMIT_CONFIG_SHOOTER, -// ShooterConstants.SHOOTER_TIMEOUT_MS); + // LEFT FALCON + shooterFalconLeft.configFactoryDefault(); + shooterFalconLeft.setNeutralMode(NeutralMode.Coast); + shooterFalconLeft.setInverted(true); + shooterFalconLeft.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconLeft.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconLeft.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconLeft.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, + ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconLeft.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, + ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG_SHOOTER, + ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconLeft.configStatorCurrentLimit(ShooterConstants.STATOR_CURRENT_LIMIT_CONFIG_SHOOTER, + ShooterConstants.SHOOTER_TIMEOUT_MS); -// // RIGHT FALCON -// shooterFalconRight.setInverted(false); -// shooterFalconRight.setNeutralMode(NeutralMode.Coast); -// shooterFalconRight.configFactoryDefault(); -// shooterFalconRight.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); -// shooterFalconRight.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); -// // m_shooterFalconRight.configPeakOutputForward(0, -// // ShooterConstants.SHOOTER_TIMEOUT_MS);(comment it in if necessary) -// shooterFalconRight.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, -// ShooterConstants.SHOOTER_TIMEOUT_MS); -// shooterFalconRight.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, -// ShooterConstants.SHOOTER_TIMEOUT_MS); -// shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG_SHOOTER, -// ShooterConstants.SHOOTER_TIMEOUT_MS); -// shooterFalconRight.configStatorCurrentLimit(ShooterConstants.STATOR_CURRENT_LIMIT_CONFIG_SHOOTER, -// ShooterConstants.SHOOTER_TIMEOUT_MS); + // RIGHT FALCON + shooterFalconRight.setInverted(false); + shooterFalconRight.setNeutralMode(NeutralMode.Coast); + shooterFalconRight.configFactoryDefault(); + shooterFalconRight.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconRight.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); + // m_shooterFalconRight.configPeakOutputForward(0, + // ShooterConstants.SHOOTER_TIMEOUT_MS);(comment it in if necessary) + shooterFalconRight.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, + ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconRight.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, + ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG_SHOOTER, + ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconRight.configStatorCurrentLimit(ShooterConstants.STATOR_CURRENT_LIMIT_CONFIG_SHOOTER, + ShooterConstants.SHOOTER_TIMEOUT_MS); -// shooterFalconRight.follow(shooterFalconLeft); + shooterFalconRight.follow(shooterFalconLeft); + } // // /* Turret Subsytem */ -// // shooterFalconRight.configStatorCurrentLimit(new -// // StatorCurrentLimitConfiguration(true, 6, 9, 4.2)); // TODO: dont pull numbers -// // out of our ass anymore -// // shooterFalconLeft.configSupplyCurrentLimit(new -// // SupplyCurrentLimitConfiguration(true, 12, 13, 0.4)); // TODO: dont pull + // shooterFalconRight.configStatorCurrentLimit(new + // StatorCurrentLimitConfiguration(true, 6, 9, 4.2)); // TODO: dont pull numbers + // out of our ass anymore + // shooterFalconLeft.configSupplyCurrentLimit(new + // SupplyCurrentLimitConfiguration(true, 12, 13, 0.4)); // TODO: dont pull // // numbers out of our ass anymore // // hood subsystem diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java index cdb8629..057bf2e 100644 --- a/src/main/java/frc4388/robot/subsystems/Hood.java +++ b/src/main/java/frc4388/robot/subsystems/Hood.java @@ -12,6 +12,7 @@ import com.revrobotics.CANSparkMax.ControlType; import com.revrobotics.CANSparkMax.SoftLimitDirection; import com.revrobotics.SparkMaxRelativeEncoder.Type; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ShooterConstants; @@ -49,7 +50,7 @@ public class Hood extends SubsystemBase { m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kForward, (float) ShooterConstants.HOOD_FORWARD_LIMIT); m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ShooterConstants.HOOD_REVERSE_LIMIT); - setHoodSoftLimits(true); + setHoodSoftLimits(false); } @@ -86,6 +87,7 @@ public class Hood extends SubsystemBase { */ public void runHood(double input) { m_angleAdjusterMotor.set(input); + SmartDashboard.putNumber("Hood Angle", m_angleAdjusterMotor.getAlternateEncoder(1024).getPosition()); } public void resetGyroAngleAdj(){ diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index b133d6f..2fdfef4 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -56,7 +56,7 @@ public class Turret extends SubsystemBase { m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, (float) ShooterConstants.TURRET_FORWARD_LIMIT); m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ShooterConstants.TURRET_REVERSE_LIMIT); - setTurretSoftLimits(true); + setTurretSoftLimits(false); m_boomBoomRotateMotor.setInverted(true); @@ -89,6 +89,7 @@ public class Turret extends SubsystemBase { public void runTurretWithInput(double input) { m_boomBoomRotateMotor.set(input * ShooterConstants.TURRET_SPEED_MULTIPLIER); + SmartDashboard.putNumber("Turret Angle", m_boomBoomRotateMotor.getAlternateEncoder(1024).getPosition()); } public void runshooterRotatePID(double targetAngle) {