From e237f14537bf94b695fe1e676ce380d382d32a5c Mon Sep 17 00:00:00 2001 From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com> Date: Thu, 10 Mar 2022 16:47:32 -0700 Subject: [PATCH] FIX --- src/main/java/frc4388/robot/Constants.java | 3 +- .../java/frc4388/robot/RobotContainer.java | 45 ++++++++++++++----- .../frc4388/robot/subsystems/BoomBoom.java | 3 +- .../frc4388/robot/subsystems/Climber.java | 26 +++++++++++ .../frc4388/robot/subsystems/SwerveDrive.java | 28 +++++------- .../DeadbandedRawXboxController.java | 3 +- .../controller/DeadbandedXboxController.java | 3 +- 7 files changed, 80 insertions(+), 31 deletions(-) create mode 100644 src/main/java/frc4388/robot/subsystems/Climber.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index ae01bf8..4c4ef0a 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 = 0.5; + public static final double ROTATION_SPEED = 4.0; 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; @@ -150,6 +150,7 @@ public final class Constants { public static final int XBOX_OPERATOR_ID = 1; public static final double LEFT_AXIS_DEADBAND = 0.1; public static final double RIGHT_AXIS_DEADBAND = 0.6; + public static final boolean SKEW_STICKS = true; } public static final class ShooterConstants { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index e9d97b2..ec1630e 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -29,12 +29,16 @@ import java.util.regex.Matcher; import java.util.regex.Pattern; import java.util.stream.Collectors; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.diffplug.common.base.Errors; import com.pathplanner.lib.PathPlanner; import com.pathplanner.lib.PathPlannerTrajectory; import com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState; import com.pathplanner.lib.commands.PPSwerveControllerCommand; +import org.ejml.dense.row.decomposition.hessenberg.TridiagonalDecompositionHouseholderOrig_DDRM; + import edu.wpi.first.math.Pair; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; @@ -62,6 +66,7 @@ import frc4388.robot.commands.AimToCenter; import frc4388.robot.commands.Shoot; import frc4388.robot.commands.TrackTarget; import frc4388.robot.subsystems.BoomBoom; +import frc4388.robot.subsystems.Climber; import frc4388.robot.subsystems.Hood; import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.LED; @@ -98,8 +103,11 @@ public class RobotContainer { 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);*/ + private final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret); + private final WPI_TalonFX testElbowMotor = new WPI_TalonFX(30); + private final WPI_TalonFX testSoulderMotor = new WPI_TalonFX(31); + private final Climber m_robotClimber = new Climber(testElbowMotor); /* Controllers */ private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); private final XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); @@ -124,6 +132,7 @@ public class RobotContainer { */ public RobotContainer() { configureButtonBindings(); + testSoulderMotor.setNeutralMode(NeutralMode.Brake); /* Default Commands */ // Swerve Drive with Input @@ -133,7 +142,7 @@ public class RobotContainer { getDriverController().getLeftY(), //getDriverController().getRightX(), getDriverController().getRightX(), - //getDriverController().getRightY(), + // getDriverController().getRightY(), false), m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand")); // Intake with Triggers @@ -142,6 +151,9 @@ public class RobotContainer { getOperatorController().getLeftTriggerAxis(), getOperatorController().getRightTriggerAxis()), m_robotIntake).withName("Intake runWithTriggers defaultCommand")); + m_robotClimber.setDefaultCommand( + new RunCommand(() -> m_robotClimber.runWithInput(getOperatorController().getRightY() * 0.6), m_robotClimber) + ); // Storage Management /*m_robotStorage.setDefaultCommand( new RunCommand(() -> m_robotStorage.manageStorage(), @@ -155,7 +167,7 @@ public class RobotContainer { 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)); + new RunCommand(() -> m_robotHood.runHood(getOperatorController().getLeftY() * 0.15), m_robotHood)); // m_robotTurret.setDefaultCommand( // new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry)); @@ -190,6 +202,14 @@ public class RobotContainer { // Right Bumper > Shift Up new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value) .whenPressed(() -> m_robotSwerveDrive.highSpeed(true)); + + new JoystickButton(getDriverController(), XboxController.Button.kA.value) + .whenPressed(() -> m_robotBoomBoom.increaseSpeed(0.025)); + + new JoystickButton(getDriverController(), XboxController.Button.kB.value) + .whenPressed(() -> m_robotBoomBoom.increaseSpeed(-0.025)); + // .whileHeld(new RunCommand(() -> testElbowMotor.set(-0.2))) + // .whenReleased(new RunCommand(() -> testElbowMotor.set(0))); // new JoystickButton(getDriverController(), XboxController.Button.kA.value) // .whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0)))); @@ -198,7 +218,7 @@ public class RobotContainer { // .whenPressed(() -> m_robotMap.leftFront.reset()) // .whenPressed(() -> m_robotMap.rightFront.reset()) // .whenPressed(() -> m_robotMap.leftBack.reset()) - // .whenPressed(() -> m_robotMap.rightBack.reset()); + // .whenPressed(() -> m_robotMap.rightBack.reset()); /* Operator Buttons */ @@ -231,10 +251,11 @@ public class RobotContainer { .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))) + .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.9), m_robotStorage)) @@ -251,12 +272,14 @@ public class RobotContainer { // new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value) // .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED))) // .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0))); + // A > Shoot with Odo /*new JoystickButton(getOperatorController(), XboxController.Button.kA.value) - .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood)); - // B > Shoot with Lime - new JoystickButton(getOperatorController(), XboxController.Button.kB.value) - .whenPressed(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotSwerveDrive, m_robotVisionOdometry));*/ + .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood));*/ + + //B > Shoot with Lime + // new JoystickButton(getOperatorController(), XboxController.Button.kB.value) + // .whenPressed(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotSwerveDrive, m_robotVisionOdometry)); } /** diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index 65493c8..80a7cfd 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -18,6 +18,7 @@ import java.util.stream.IntStream; import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -50,7 +51,6 @@ public class BoomBoom extends SubsystemBase { } private ShooterTableEntry[] m_shooterTable; - /* * Creates new BoomBoom subsystem, has drum shooter and angle adjuster */ @@ -165,6 +165,7 @@ public class BoomBoom extends SubsystemBase { @Override public void periodic() { // This method will be called once per scheduler run + // speed2 = SmartDashboard.getNumber("Shooter Offset", 0.0); } public void passRequiredSubsystem(Hood subsystem0, Turret subsystem1) { diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java new file mode 100644 index 0000000..a55ece0 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -0,0 +1,26 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.subsystems; + +import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import com.revrobotics.CANSparkMax; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Climber extends SubsystemBase { + WPI_TalonFX m_climberElbow; + /** Creates a new Climber. */ + public Climber(WPI_TalonFX climberElbow) { + m_climberElbow = climberElbow; + } + + public void runWithInput(double input){ + m_climberElbow.set(input); + } + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index d782706..36dc773 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -37,14 +37,10 @@ public class SwerveDrive extends SubsystemBase { public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS; - Translation2d m_frontLeftLocation = new Translation2d(Units.inchesToMeters(halfHeight), - 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), - Units.inchesToMeters(halfWidth)); + Translation2d m_frontLeftLocation = new Translation2d(Units.inchesToMeters(halfHeight), 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), Units.inchesToMeters(halfWidth)); public SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation); @@ -108,29 +104,29 @@ public class SwerveDrive extends SubsystemBase { ignoreAngles = true; else ignoreAngles = false; - Translation2d speed = new Translation2d(-speedY, -speedX); + Translation2d speed = new Translation2d(speedX, speedY); double mag = speed.getNorm(); speed = speed.times(mag * speedAdjust); - double xSpeedMetersPerSecond = -speed.getX(); + double xSpeedMetersPerSecond = speed.getX(); double ySpeedMetersPerSecond = speed.getY(); chassisSpeeds = fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, - rot * SwerveDriveConstants.ROTATION_SPEED * 8, m_gyro.getRotation2d()) - : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, - -rot * SwerveDriveConstants.ROTATION_SPEED * 8); + -rot * SwerveDriveConstants.ROTATION_SPEED, new Rotation2d((360 - m_gyro.getRotation2d().getDegrees() + 90) * (Math.PI/180))) + : new ChassisSpeeds(ySpeedMetersPerSecond, -xSpeedMetersPerSecond, + -rot * SwerveDriveConstants.ROTATION_SPEED); SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(chassisSpeeds); setModuleStates(states); } public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative) { ignoreAngles = leftX == 0 && leftY == 0 && rightX == 0 && rightY == 0; - Translation2d speed = new Translation2d(-leftX, leftY); + 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).plus(new Rotation2d(0, 1)); + rotTarget = new Rotation2d(rightY, rightX); double rot = rotTarget.minus(m_gyro.getRotation2d()).getRadians(); - double xSpeedMetersPerSecond = -speed.getX(); + double xSpeedMetersPerSecond = speed.getX(); double ySpeedMetersPerSecond = speed.getY(); chassisSpeeds = fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, diff --git a/src/main/java/frc4388/utility/controller/DeadbandedRawXboxController.java b/src/main/java/frc4388/utility/controller/DeadbandedRawXboxController.java index e1679dd..177ef4e 100644 --- a/src/main/java/frc4388/utility/controller/DeadbandedRawXboxController.java +++ b/src/main/java/frc4388/utility/controller/DeadbandedRawXboxController.java @@ -3,6 +3,7 @@ package frc4388.utility.controller; import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND; import edu.wpi.first.math.geometry.Translation2d; +import frc4388.robot.Constants.OIConstants; public class DeadbandedRawXboxController extends RawXboxController { public DeadbandedRawXboxController(int port) { super(port); } @@ -13,7 +14,7 @@ public class DeadbandedRawXboxController extends RawXboxController { public static Translation2d skewToDeadzonedCircle(double x, double y) { Translation2d translation2d = new Translation2d(x, y); double magnitude = translation2d.getNorm(); - if (magnitude >= 1) return translation2d.div(magnitude); + if (OIConstants.SKEW_STICKS && magnitude >= 1) return translation2d.div(magnitude); if (magnitude < LEFT_AXIS_DEADBAND) return new Translation2d(0,0); return translation2d; } diff --git a/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java b/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java index 1c6fe5f..5b1bc97 100644 --- a/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java +++ b/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java @@ -4,6 +4,7 @@ import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.XboxController; +import frc4388.robot.Constants.OIConstants; public class DeadbandedXboxController extends XboxController { public DeadbandedXboxController(int port) { super(port); } @@ -14,7 +15,7 @@ public class DeadbandedXboxController extends XboxController { public static Translation2d skewToDeadzonedCircle(double x, double y) { Translation2d translation2d = new Translation2d(x, y); double magnitude = translation2d.getNorm(); - if (magnitude >= 1) return translation2d.div(magnitude); + if (OIConstants.SKEW_STICKS && magnitude >= 1) return translation2d.div(magnitude); if (magnitude < LEFT_AXIS_DEADBAND) return new Translation2d(0,0); return translation2d; }