From dce8ab320c8352009d8600989cbfdd412f5cf6b5 Mon Sep 17 00:00:00 2001 From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com> Date: Tue, 8 Mar 2022 16:14:34 -0700 Subject: [PATCH] Monday Fixes --- src/main/java/frc4388/robot/Constants.java | 44 +++++++------- .../java/frc4388/robot/RobotContainer.java | 57 ++++++++++++------- .../frc4388/robot/subsystems/BoomBoom.java | 12 +++- .../java/frc4388/robot/subsystems/Hood.java | 12 ++-- .../frc4388/robot/subsystems/SwerveDrive.java | 25 ++++---- .../robot/subsystems/SwerveModule.java | 1 + 6 files changed, 90 insertions(+), 61 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index e1faffa..ae01bf8 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -31,7 +31,7 @@ import frc4388.utility.LEDPatterns; */ public final class Constants { public static final class SwerveDriveConstants { - public static final double ROTATION_SPEED = 4; + public static final double ROTATION_SPEED = 0.5; public static final double WIDTH = 23.5; public static final double HEIGHT = 23.5; public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11; @@ -40,18 +40,18 @@ public final class Constants { public static final double SPEED_FEET_PER_SECOND_AT_FULL_POWER = 20; // TODO: redundant constant? // IDs - public static final int LEFT_FRONT_STEER_CAN_ID = 2; - public static final int LEFT_FRONT_WHEEL_CAN_ID = 3; - public static final int RIGHT_FRONT_STEER_CAN_ID = 4; - public static final int RIGHT_FRONT_WHEEL_CAN_ID = 5; - public static final int LEFT_BACK_STEER_CAN_ID = 6; - public static final int LEFT_BACK_WHEEL_CAN_ID = 7; - public static final int RIGHT_BACK_STEER_CAN_ID = 8; - public static final int RIGHT_BACK_WHEEL_CAN_ID = 9; - public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10; - public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11; - public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12; - public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13; + public static final int LEFT_FRONT_STEER_CAN_ID = 2; // + public static final int LEFT_FRONT_WHEEL_CAN_ID = 3; // + public static final int RIGHT_FRONT_STEER_CAN_ID = 4; // + public static final int RIGHT_FRONT_WHEEL_CAN_ID = 5; // + public static final int LEFT_BACK_STEER_CAN_ID = 6; // + public static final int LEFT_BACK_WHEEL_CAN_ID = 7; // + public static final int RIGHT_BACK_STEER_CAN_ID = 8; // + public static final int RIGHT_BACK_WHEEL_CAN_ID = 9; // + public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10; // + public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11; // + public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12;// + public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13; // public static final int GYRO_ID = 14; // offsets are in degrees @@ -65,10 +65,10 @@ public final class Constants { // public static final double RIGHT_BACK_ENCODER_OFFSET = 360. + 2.15 - 3.637;// // 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_BACK_ENCODER_OFFSET = (4 * 360. - 189.50 - 90) % 360.; - public static final double RIGHT_BACK_ENCODER_OFFSET = (4 * 360. - 9.31 - 90 - 180) % 360.; + public static final double LEFT_FRONT_ENCODER_OFFSET = (4 * 360. - 232.58 + 180 ) % 360.; + public static final double RIGHT_FRONT_ENCODER_OFFSET = (4 * 360. - 152.05 - 180 ) % 360.; + public static final double LEFT_BACK_ENCODER_OFFSET = (4 * 360. - 189.50) % 360.; + public static final double RIGHT_BACK_ENCODER_OFFSET = (4 * 360. - 9.31 - 180) % 360.; // swerve PID constants public static final int SWERVE_SLOT_IDX = 0; @@ -176,10 +176,10 @@ public final class Constants { // ID public static final int TURRET_MOTOR_CAN_ID = 19; 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 Gains SHOOTER_ANGLE_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 0.7); public static final double SHOOTER_TURRET_MIN = -1.0; - 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 double TURRET_FORWARD_LIMIT = 55.0; // TODO: find + public static final double TURRET_REVERSE_LIMIT = -55.0; // TODO: find public static final Gains DRUM_SHOOTER_GAINS = new Gains(0, 0, 0, 0, 0, 0); // TODO: tune values @@ -187,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 double HOOD_FORWARD_LIMIT = 48.69; // TODO: find - public static final double HOOD_REVERSE_LIMIT = -100; // TODO: find + public static final double HOOD_FORWARD_LIMIT = 0.0; // TODO: find + public static final double HOOD_REVERSE_LIMIT = -150; // TODO: find } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 9e2c886..e9d97b2 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -133,8 +133,8 @@ public class RobotContainer { getDriverController().getLeftY(), //getDriverController().getRightX(), getDriverController().getRightX(), - getDriverController().getRightY(), - true), + //getDriverController().getRightY(), + false), m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand")); // Intake with Triggers m_robotIntake.setDefaultCommand( @@ -147,9 +147,9 @@ public class RobotContainer { 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(getOperatorController().getLeftTriggerAxis() * 0.8),//m_robotSerializer.setSerializerStateWithBeam(), + m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand")); // Turret Manual m_robotTurret.setDefaultCommand( new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), @@ -194,11 +194,11 @@ public class RobotContainer { // 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 */ @@ -209,22 +209,39 @@ 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.kB.value) - .whenPressed(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0))); - - new JoystickButton(getOperatorController(), XboxController.Button.kX.value) - .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.3))) + new JoystickButton(getOperatorController(), XboxController.Button.kA.value) //8ft + // .whenPressed(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0))); + // .whenPressed(() -> m_robotBoomBoom.increaseSpeed(0.025)); + .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.425))) + .whileHeld(new RunCommand(() -> m_robotHood.runAngleAdjustPID(-25))) .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); + + new JoystickButton(getOperatorController(), XboxController.Button.kB.value) //12ft + .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.475))) + .whileHeld(new RunCommand(() -> m_robotHood.runAngleAdjustPID(-47.57))) + .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); + // .whenPressed(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0))); + // .whenPressed(() -> m_robotBoomBoom.increaseSpeed(-0.025)); + + + new JoystickButton(getOperatorController(), XboxController.Button.kY.value) //16ft + .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.5))) + .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))).whileHeld(new RunCommand(() -> m_robotHood.runAngleAdjustPID(-96))) + .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); + // .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.45))) + // .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)) + .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-0.9), 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)) + .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(0.9), m_robotStorage)) .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); // Right Bumper > Storage In // new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value) diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index 7f87f3e..65493c8 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -20,6 +20,7 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ShooterConstants; import frc4388.utility.CSV; @@ -31,6 +32,7 @@ public class BoomBoom extends SubsystemBase { public WPI_TalonFX m_shooterFalconRight; public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS; public static BoomBoom m_boomBoom; + double speed2; double velP; double input; @@ -168,14 +170,15 @@ public class BoomBoom extends SubsystemBase { public void passRequiredSubsystem(Hood subsystem0, Turret subsystem1) { m_hoodSubsystem = subsystem0; m_turretSubsystem = subsystem1; - } + } /** * Runs the Drum motor at a given speed * @param speed percent output form -1.0 to 1.0 */ public void runDrumShooter(double speed) { - m_shooterFalconLeft.set(TalonFXControlMode.PercentOutput, speed); + m_shooterFalconLeft.set(TalonFXControlMode.PercentOutput, speed + speed2); + SmartDashboard.putNumber("BoomBoom percent speed", speed + speed2); } @@ -199,4 +202,9 @@ public class BoomBoom extends SubsystemBase { // feedforward.calculate(targetVel)); // m_shooterFalconLeft.set(m_controller.calculate(m_shooterFalconLeft.get(), targetVel)); } + + public void increaseSpeed(double amount) + { + speed2 = speed2 + amount; + } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java index 2305c86..8ab8a9f 100644 --- a/src/main/java/frc4388/robot/subsystems/Hood.java +++ b/src/main/java/frc4388/robot/subsystems/Hood.java @@ -20,8 +20,8 @@ public class Hood extends SubsystemBase { public BoomBoom m_shooterSubsystem; public CANSparkMax m_angleAdjusterMotor; - public SparkMaxLimitSwitch m_hoodUpLimitSwitch; - public SparkMaxLimitSwitch m_hoodDownLimitSwitch; + // public SparkMaxLimitSwitch m_hoodUpLimitSwitch; + // public SparkMaxLimitSwitch m_hoodDownLimitSwitch; public static Gains m_angleAdjusterGains; public RelativeEncoder m_angleEncoder; @@ -41,10 +41,10 @@ public double m_fireAngle; m_angleAdjusterPIDController = m_angleAdjusterMotor.getPIDController(); m_angleAdjusterGains = ShooterConstants.SHOOTER_ANGLE_GAINS; - m_hoodUpLimitSwitch = m_angleAdjusterMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - m_hoodDownLimitSwitch = m_angleAdjusterMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - m_hoodUpLimitSwitch.enableLimitSwitch(true); - m_hoodDownLimitSwitch.enableLimitSwitch(true); + // m_hoodUpLimitSwitch = m_angleAdjusterMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + // m_hoodDownLimitSwitch = m_angleAdjusterMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + // m_hoodUpLimitSwitch.enableLimitSwitch(true); + // m_hoodDownLimitSwitch.enableLimitSwitch(true); m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kForward, (float) ShooterConstants.HOOD_FORWARD_LIMIT); m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ShooterConstants.HOOD_REVERSE_LIMIT); diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 36e0f52..d782706 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -38,13 +38,13 @@ public class SwerveDrive extends SubsystemBase { public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS; Translation2d m_frontLeftLocation = new Translation2d(Units.inchesToMeters(halfHeight), - Units.inchesToMeters(halfWidth)); + Units.inchesToMeters(-halfWidth)); Translation2d m_frontRightLocation = new Translation2d(Units.inchesToMeters(halfHeight), - Units.inchesToMeters(-halfWidth)); - Translation2d m_backLeftLocation = new Translation2d(Units.inchesToMeters(-halfHeight), Units.inchesToMeters(halfWidth)); - Translation2d m_backRightLocation = new Translation2d(Units.inchesToMeters(-halfHeight), + Translation2d m_backLeftLocation = new Translation2d(Units.inchesToMeters(-halfHeight), Units.inchesToMeters(-halfWidth)); + Translation2d m_backRightLocation = new Translation2d(Units.inchesToMeters(-halfHeight), + Units.inchesToMeters(halfWidth)); public SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation); @@ -77,7 +77,7 @@ public class SwerveDrive extends SubsystemBase { m_rightBack = rightBack; m_gyro = gyro; - modules = new SwerveModule[] { m_leftFront, m_rightFront, m_leftBack, m_rightBack }; + modules = new SwerveModule[] { m_leftFront, m_rightFront, m_leftBack, m_rightBack}; m_poseEstimator = new SwerveDrivePoseEstimator( m_gyro.getRotation2d(), @@ -108,7 +108,7 @@ public class SwerveDrive extends SubsystemBase { ignoreAngles = true; else ignoreAngles = false; - Translation2d speed = new Translation2d(-speedX, speedY); + Translation2d speed = new Translation2d(-speedY, -speedX); double mag = speed.getNorm(); speed = speed.times(mag * speedAdjust); @@ -116,9 +116,9 @@ public class SwerveDrive extends SubsystemBase { double ySpeedMetersPerSecond = speed.getY(); chassisSpeeds = fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, - rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d()) + rot * SwerveDriveConstants.ROTATION_SPEED * 8, m_gyro.getRotation2d()) : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, - rot * SwerveDriveConstants.ROTATION_SPEED); + -rot * SwerveDriveConstants.ROTATION_SPEED * 8); SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(chassisSpeeds); setModuleStates(states); } @@ -128,17 +128,19 @@ public class SwerveDrive extends SubsystemBase { Translation2d speed = new Translation2d(-leftX, leftY); speed = speed.times(speed.getNorm() * speedAdjust); if (Math.abs(rightX) > OIConstants.RIGHT_AXIS_DEADBAND || Math.abs(rightY) > OIConstants.RIGHT_AXIS_DEADBAND) - rotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0, 1)); + rotTarget = new Rotation2d(rightX, -rightY).plus(new Rotation2d(0, 1)); double rot = rotTarget.minus(m_gyro.getRotation2d()).getRadians(); double xSpeedMetersPerSecond = -speed.getX(); double ySpeedMetersPerSecond = speed.getY(); chassisSpeeds = fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, - rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d()) - : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED); + rot * SwerveDriveConstants.ROTATION_SPEED, new Rotation2d(m_gyro.getRotation2d().getDegrees())) + : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED * 8); SwerveModuleState[] states = m_kinematics.toSwerveModuleStates( chassisSpeeds); setModuleStates(states); + // SmartDashboard.putNumber("rot", rot); + // SmartDashboard.putNumber("rotarget", rotTarget.getDegrees()); } /** @@ -149,6 +151,7 @@ public class SwerveDrive extends SubsystemBase { public void setModuleStates(SwerveModuleState[] desiredStates) { SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC)); + // int i = 2; { for (int i = 0; i < desiredStates.length; i++) { SwerveModule module = modules[i]; SwerveModuleState state = desiredStates[i]; diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index bd1d8ee..1a3ebbc 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -120,6 +120,7 @@ public class SwerveModule extends SubsystemBase { double desiredTicks = currentTicks + deltaTicks; if (!ignoreAngle) { + angleMotor.set(TalonFXControlMode.Position, desiredTicks); }