diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index ffd39d4..f5adffb 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -32,9 +32,8 @@ import frc4388.utility.LEDPatterns; public final class Constants { public static final class SwerveDriveConstants { public static final double ROTATION_SPEED = 4; - public static final double WHEEL_SPEED = 0.1; - public static final double WIDTH = 15.25; - public static final double HEIGHT = 15.25; + public static final double WIDTH = 23.5; + public static final double HEIGHT = 25.5; public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11; public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 2; public static final double MAX_SPEED_FEET_PER_SEC = 20; // TODO: redundant constant? @@ -67,9 +66,9 @@ public final class Constants { // 180-2.021484375;//0.0;//134.384765625 public static final double RIGHT_FRONT_ENCODER_OFFSET = (4 * 360. - 152.05 - 180 - 90) % 360.; - public static final double LEFT_FRONT_ENCODER_OFFSET = (4 * 360. - 232.58 + 180 - 90) % 360.; + public static final double LEFT_FRONT_ENCODER_OFFSET = (4 * 360. - 232.58 + 180 - 90 ) % 360.; public static final double LEFT_BACK_ENCODER_OFFSET = (4 * 360. - 189.50 - 90) % 360.; - public static final double RIGHT_BACK_ENCODER_OFFSET = (4 * 360. - 9.31 - 180 - 90) % 360.; + public static final double RIGHT_BACK_ENCODER_OFFSET = (4 * 360. - 9.31 - 90 - 180) % 360.; // swerve PID constants public static final int SWERVE_SLOT_IDX = 0; @@ -164,7 +163,7 @@ public final class Constants { true, 60, 40, 0.5); public static final int SHOOTER_FALCON_LEFT_CAN_ID = 21; public static final int SHOOTER_FALCON_RIGHT_CAN_ID = 22; - public static final double TURRET_SPEED_MULTIPLIER = 0.1d; + public static final double TURRET_SPEED_MULTIPLIER = 0.4d; 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; @@ -179,8 +178,8 @@ 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 double TURRET_FORWARD_LIMIT = 150; // TODO: find - public static final double TURRET_REVERSE_LIMIT = 0; // TODO: find + public static final double TURRET_FORWARD_LIMIT = 61.7; // TODO: find + public static final double TURRET_REVERSE_LIMIT = -42.3; // TODO: find public static final Gains DRUM_SHOOTER_GAINS = new Gains(0, 0, 0, 0, 0, 0); // TODO: tune values @@ -188,8 +187,8 @@ public final class Constants { public static final int SHOOTER_ANGLE_ADJUST_ID = 20; 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 float HOOD_FORWARD_LIMIT = 200; // TODO: find - public static final float HOOD_REVERSE_LIMIT = 0; // TODO: find + public static final double HOOD_FORWARD_LIMIT = 48.69; // TODO: find + public static final double HOOD_REVERSE_LIMIT = -100; // TODO: find } diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 0faf4a2..e9d690b 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -128,6 +128,8 @@ public class Robot extends TimedRobot { @Override public void robotPeriodic() { m_robotTime.updateTimes(); + SmartDashboard.putNumber("Turret", m_robotContainer.m_robotMap.shooterTurret.getEncoder().getPosition()); + SmartDashboard.putNumber("Hood", m_robotContainer.m_robotMap.angleAdjusterMotor.getEncoder().getPosition()); // Runs the Scheduler. This is responsible for polling buttons, adding // newly-scheduled // commands, running already-scheduled commands, removing finished or @@ -137,8 +139,8 @@ public class Robot extends TimedRobot { // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); - SmartDashboard.putNumber("Turret Encoder Position", m_robotContainer.m_robotTurret.m_boomBoomRotateEncoder.getPosition()); - SmartDashboard.putNumber("Hood Encoder Position", m_robotContainer.m_robotHood.m_angleEncoder.getPosition()); + //SmartDashboard.putNumber("Turret Encoder Position", m_robotContainer.m_robotTurret.m_boomBoomRotateEncoder.getPosition()); + //SmartDashboard.putNumber("Hood Encoder Position", m_robotContainer.m_robotHood.m_angleEncoder.getPosition()); // odo chooser stuff addOdoChoices(new Pose2d(1, 1, new Rotation2d(Math.PI/6)), diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index aad6098..9e2c886 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -87,17 +87,17 @@ import frc4388.utility.controller.DeadbandedXboxController; public class RobotContainer { private static final Logger LOGGER = Logger.getLogger(RobotContainer.class.getSimpleName()); /* RobotMap */ - private final RobotMap m_robotMap = new RobotMap(); + public final RobotMap m_robotMap = new RobotMap(); // Subsystems public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro); - private final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, /*m_robotMap.serializerShooterBelt,*/ m_robotMap.serializerBeam); - private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.extenderMotor, m_robotSerializer); - // private final Storage m_robotStorage = new Storage(m_robotMap.storageMotor, m_robotMap.beamIntake, m_robotMap.beamShooter); + public final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, /*m_robotMap.serializerShooterBelt,*/ m_robotMap.serializerBeam); + public final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.extenderMotor, m_robotSerializer); + public final Storage m_robotStorage = new Storage(m_robotMap.storageMotor); // private final LED m_robotLED = new LED(m_robotMap.LEDController); - private final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight); - private final Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor); - private final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); + 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);*/ /* Controllers */ @@ -127,31 +127,32 @@ public class RobotContainer { /* Default Commands */ // Swerve Drive with Input - /*m_robotSwerveDrive.setDefaultCommand( + m_robotSwerveDrive.setDefaultCommand( new RunCommand(() -> m_robotSwerveDrive.driveWithInput( getDriverController().getLeftX(), getDriverController().getLeftY(), + //getDriverController().getRightX(), getDriverController().getRightX(), - getDriverController().getRightY(), + getDriverController().getRightY(), true), - m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand"));*/ + m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand")); // Intake with Triggers m_robotIntake.setDefaultCommand( new RunCommand(() -> m_robotIntake.runWithTriggers( - getDriverController().getLeftTriggerAxis(), - getDriverController().getRightTriggerAxis()), + getOperatorController().getLeftTriggerAxis(), + getOperatorController().getRightTriggerAxis()), m_robotIntake).withName("Intake runWithTriggers defaultCommand")); // Storage Management /*m_robotStorage.setDefaultCommand( new RunCommand(() -> m_robotStorage.manageStorage(), m_robotStorage).withName("Storage manageStorage defaultCommand"));*/ // Serializer Management - m_robotSerializer.setDefaultCommand( - new RunCommand(() -> m_robotSerializer.setSerializer(0.8),//m_robotSerializer.setSerializerStateWithBeam(), - m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand")); + // m_robotSerializer.setDefaultCommand( + // new RunCommand(() -> m_robotSerializer.setSerializer(0.8),//m_robotSerializer.setSerializerStateWithBeam(), + // m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand")); // Turret Manual m_robotTurret.setDefaultCommand( - new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX() * 0.1), + new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), m_robotTurret).withName("Turret runTurretWithInput defaultCommand")); m_robotHood.setDefaultCommand( new RunCommand(() -> m_robotHood.runHood(getOperatorController().getRightY() * 0.1), m_robotHood)); @@ -190,22 +191,14 @@ public class RobotContainer { new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value) .whenPressed(() -> m_robotSwerveDrive.highSpeed(true)); - new JoystickButton(getDriverController(), XboxController.Button.kA.value) - .whileHeld( new RunCommand(() -> m_robotSerializer.setSerializer(0.25))) - .whenReleased(new RunCommand(() -> m_robotSerializer.setSerializer(0.0))); - - new JoystickButton(getDriverController(), XboxController.Button.kB.value) - .whileHeld(new RunCommand(() -> m_robotSerializer.setSerializer(-0.25))) - .whenReleased(new RunCommand(() -> m_robotSerializer.setSerializer(0.0))); - // new JoystickButton(getDriverController(), XboxController.Button.kA.value) // .whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0)))); - // new JoystickButton(getDriverController(), XboxController.Button.kX.value) //Temp - // .whenPressed(() -> m_robotMap.leftFront.reset()) - // .whenPressed(() -> m_robotMap.rightFront.reset()) - // .whenPressed(() -> m_robotMap.leftBack.reset()) - // .whenPressed(() -> m_robotMap.rightBack.reset()); + new JoystickButton(getDriverController(), XboxController.Button.kX.value) //Temp + .whenPressed(() -> m_robotMap.leftFront.reset()) + .whenPressed(() -> m_robotMap.rightFront.reset()) + .whenPressed(() -> m_robotMap.leftBack.reset()) + .whenPressed(() -> m_robotMap.rightBack.reset()); /* Operator Buttons */ @@ -216,14 +209,23 @@ public class RobotContainer { new JoystickButton(getOperatorController(), XboxController.Button.kY.value) .whenPressed(() -> m_robotIntake.runExtender(false));*/ - new JoystickButton(getOperatorController(), XboxController.Button.kA.value) - .whenPressed(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0))); + // new JoystickButton(getOperatorController(), XboxController.Button.kA.value) + // .whenPressed(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0))); new JoystickButton(getOperatorController(), XboxController.Button.kB.value) .whenPressed(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0))); new JoystickButton(getOperatorController(), XboxController.Button.kX.value) - .whenPressed(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.05))); + .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.3))) + .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); + + new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value) + .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-0.75), m_robotStorage)) + .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); + + new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value) + .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(0.75), m_robotStorage)) + .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); // Right Bumper > Storage In // new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value) // .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED))) diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 94255ca..0e13aac 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -159,7 +159,7 @@ public class RobotMap { rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - } + } // Shooter Config /* Boom Boom Subsystem */ @@ -204,6 +204,8 @@ public class RobotMap { shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconRight.follow(shooterFalconLeft); + // /* Turret Subsytem */ // shooterFalconRight.configStatorCurrentLimit(new // StatorCurrentLimitConfiguration(true, 6, 9, 4.2)); // TODO: dont pull numbers @@ -229,7 +231,7 @@ public class RobotMap { public final CANSparkMax extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_MOTOR, MotorType.kBrushless); /* Storage Subsystem */ -// public final CANSparkMax storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless); + public final CANSparkMax storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless); // public final DigitalInput beamShooter = new DigitalInput(StorageConstants.BEAM_SENSOR_SHOOTER); // public final DigitalInput beamIntake = new DigitalInput(StorageConstants.BEAM_SENSOR_INTAKE); } diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index e1286e5..7f87f3e 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -189,7 +189,7 @@ public class BoomBoom extends SubsystemBase { public void runDrumShooterVelocityPID(double targetVel) { m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel); // Init - m_shooterFalconRight.follow(m_shooterFalconLeft); + // New BoomBoom controller stuff // Controls a motor with the output of the BangBang controller // Controls a motor with the output of the BangBang conroller and a feedforward diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java index 9f2d765..2305c86 100644 --- a/src/main/java/frc4388/robot/subsystems/Hood.java +++ b/src/main/java/frc4388/robot/subsystems/Hood.java @@ -46,8 +46,8 @@ public double m_fireAngle; m_hoodUpLimitSwitch.enableLimitSwitch(true); m_hoodDownLimitSwitch.enableLimitSwitch(true); - m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.HOOD_FORWARD_LIMIT); - m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.HOOD_REVERSE_LIMIT); + m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kForward, (float) ShooterConstants.HOOD_FORWARD_LIMIT); + m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ShooterConstants.HOOD_REVERSE_LIMIT); setHoodSoftLimits(true); } diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index 3977f5a..dcaddd7 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -19,6 +19,12 @@ public class Storage extends SubsystemBase { m_beamShooter = beamShooter; m_beamIntake = beamIntake; } + + public Storage(CANSparkMax storageMotor) { + m_storageMotor = storageMotor; + m_beamShooter = null; + m_beamIntake = null; + } /** * If The Beam Is Broken, Run Storage * If Else, Stop Running Storage @@ -57,6 +63,6 @@ public class Storage extends SubsystemBase { * Every Robot Tick Manage The Storage */ public void periodic() { - manageStorage(); + //manageStorage(); } } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 44cd6e0..36e0f52 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -152,7 +152,7 @@ public class SwerveDrive extends SubsystemBase { for (int i = 0; i < desiredStates.length; i++) { SwerveModule module = modules[i]; SwerveModuleState state = desiredStates[i]; - module.setDesiredState(state, false); + module.setDesiredState(state, ignoreAngles); } // modules[0].setDesiredState(desiredStates[0], false); } diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index e9dafcc..1a97942 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -29,7 +29,7 @@ public class Turret extends SubsystemBase { public CANSparkMax m_boomBoomRotateMotor;// = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, // MotorType.kBrushless); public static Gains m_shooterTGains = ShooterConstants.SHOOTER_TURRET_GAINS; - SparkMaxLimitSwitch m_boomBoomRightLimit, m_boomBoomLeftLimit; + // SparkMaxLimitSwitch m_boomBoomRightLimit, m_boomBoomLeftLimit; public Gyro m_turretGyro; public double m_targetDistance = 0; @@ -47,18 +47,18 @@ public class Turret extends SubsystemBase { m_boomBoomRotateEncoder = m_boomBoomRotateMotor.getEncoder(); m_boomBoomRotateMotor.setIdleMode(IdleMode.kBrake); - m_boomBoomLeftLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - m_boomBoomRightLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - m_boomBoomRightLimit.enableLimitSwitch(true); - m_boomBoomLeftLimit.enableLimitSwitch(true); - SmartDashboard.putBoolean("Right Limit Switch Enabled", m_boomBoomRightLimit.isLimitSwitchEnabled()); - SmartDashboard.putBoolean("Left Limit Switch Enabled", m_boomBoomLeftLimit.isLimitSwitchEnabled()); + // m_boomBoomLeftLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + // m_boomBoomRightLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + // m_boomBoomRightLimit.enableLimitSwitch(true); + // m_boomBoomLeftLimit.enableLimitSwitch(true); + // SmartDashboard.putBoolean("Right Limit Switch Enabled", m_boomBoomRightLimit.isLimitSwitchEnabled()); + // SmartDashboard.putBoolean("Left Limit Switch Enabled", m_boomBoomLeftLimit.isLimitSwitchEnabled()); m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, (float) ShooterConstants.TURRET_FORWARD_LIMIT); m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ShooterConstants.TURRET_REVERSE_LIMIT); setTurretSoftLimits(true); - m_boomBoomRotateMotor.setInverted(false); + m_boomBoomRotateMotor.setInverted(true); m_boomBoomRotatePIDController.setP(m_shooterTGains.kP); m_boomBoomRotatePIDController.setI(m_shooterTGains.kI);