From bb80945ddd4e2f3dd8354b92907f26ac388e6761 Mon Sep 17 00:00:00 2001 From: cyblazer Date: Wed, 21 Feb 2024 14:56:00 -0800 Subject: [PATCH] Only Intake Subsystem and commands --- build.gradle | 2 +- src/main/java/frc/robot/Constants.java | 13 +- src/main/java/frc/robot/RobotContainer.java | 75 +----- .../frc/robot/commands/IntakeCommand.java | 50 ++++ .../frc/robot/subsystems/DriveSubsystem.java | 232 ------------------ .../frc/robot/subsystems/IntakeSubsystem.java | 99 ++++++-- .../robot/subsystems/ShooterSubsystem.java | 32 --- .../frc/robot/subsystems/SwerveModule.java | 118 --------- .../frc/robot/subsystems/VisionSubsystem.java | 128 ---------- vendordeps/REV2mDistanceSensor.json | 56 +++++ 10 files changed, 201 insertions(+), 604 deletions(-) create mode 100644 src/main/java/frc/robot/commands/IntakeCommand.java delete mode 100644 src/main/java/frc/robot/subsystems/DriveSubsystem.java delete mode 100644 src/main/java/frc/robot/subsystems/ShooterSubsystem.java delete mode 100644 src/main/java/frc/robot/subsystems/SwerveModule.java delete mode 100644 src/main/java/frc/robot/subsystems/VisionSubsystem.java create mode 100644 vendordeps/REV2mDistanceSensor.json diff --git a/build.gradle b/build.gradle index d141f06..a34fb4c 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.1.1" + id "edu.wpi.first.GradleRIO" version "2024.2.1" } java { diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index a963b8d..6b8bf5c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -100,18 +100,15 @@ public static final class DriveConstants { // Intake PID and Encoder Constants public static class IntakeConstants { + //TODO: figure out constants + //public static final boolean kPivotMotorInverted = true; public static final double kIntakeLoweredAngle = 9.0; - public static final double kIntakeRaisedAngle = 9.0; + public static final double kIntakeRaisedAngle = 60.0; public static final int kIntakeMotorID = 0; + public static final double kIntakeMotorSpeed = -3.0; public static final int kArmMotorID = 0; - public static final double kIntakeP = 0; - public static final double kIntakeI = 0; - public static final double kIntakeD = 0; - public static final double kArmP = 0; - public static final double kArmI = 0; - public static final double kArmD = 0; public static final int kArmEncoderCh = 0; - public static double kIntakeSpeed; + public static double kIntakeSpeed = 3.0; } // Shooter subsystem speed constants diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 535c73b..5606606 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,11 +13,10 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc.robot.Constants.IntakeConstants; +import frc.robot.commands.IntakeCommand; import frc.robot.Constants.DriveConstants; import frc.robot.Constants.IOConstants; -import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.IntakeSubsystem; -import frc.robot.subsystems.ShooterSubsystem; /* * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -26,84 +25,34 @@ * (including subsystems, commands, and button mappings) should be declared here. */ public class RobotContainer { - - private final DriveSubsystem m_robotDrive = new DriveSubsystem(); - public final ShooterSubsystem m_shooterSubsystem = new ShooterSubsystem(); public final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem(); - private boolean IntakeDropped = false; - private boolean lastAButton = false; - private final XboxController m_driverController = new XboxController(IOConstants.kDriverControllerPort); public RobotContainer() { // Configure the trigger bindings configureBindings(); - m_intakeSubsystem.setDefaultCommand(m_driverController.getLeftTriggerAxis() > 0.5 - ? new InstantCommand(m_intakeSubsystem::intakeDisk, m_intakeSubsystem) - : new InstantCommand(m_intakeSubsystem::stopIntaking, m_intakeSubsystem)); - - m_robotDrive.setDefaultCommand( - new RunCommand( - () -> m_robotDrive.drive( - MathUtil.applyDeadband( - -m_driverController.getLeftY(), - IOConstants.kControllerDeadband) - * DriveConstants.kMaxSpeedMetersPerSecond - * (1 - m_driverController - .getLeftTriggerAxis() - * IOConstants.kSlowModeScalar) - * 0.8, - MathUtil.applyDeadband( - -m_driverController.getLeftX(), - IOConstants.kControllerDeadband) - * DriveConstants.kMaxSpeedMetersPerSecond - * (1 - m_driverController - .getLeftTriggerAxis() - * IOConstants.kSlowModeScalar) - * 0.8, - MathUtil.applyDeadband( - m_driverController.getRightX(), - IOConstants.kControllerDeadband) - * DriveConstants.kMaxAngularSpeedRadiansPerSecond - * (1 - m_driverController - .getLeftTriggerAxis() - * IOConstants.kSlowModeScalar) - / 2, - !m_driverController.getRightBumper()), - m_robotDrive)); - } public void periodic() { - if (m_driverController.getAButton()) { - lastAButton = true; - if (!lastAButton) - IntakeDropped = !IntakeDropped; - } else { - lastAButton = false; - } } /** * Use this method to define your button->command mappings. */ private void configureBindings() { - // TODO: Move shoot commands to operator controller - new JoystickButton(m_driverController, Button.kY.value) - .onTrue(new InstantCommand(() -> m_intakeSubsystem.tiltToAngle(IntakeConstants.kIntakeLoweredAngle), - m_intakeSubsystem)) - .onFalse(new InstantCommand(m_intakeSubsystem::stopRotating, m_intakeSubsystem)); new JoystickButton(m_driverController, Button.kX.value) - .onTrue(new InstantCommand(() -> m_intakeSubsystem.tiltToAngle(IntakeConstants.kIntakeRaisedAngle), - m_intakeSubsystem)) - .onFalse(new InstantCommand(m_intakeSubsystem::stopRotating, m_intakeSubsystem)); - new JoystickButton(m_driverController, Button.kB.value) - .onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(0.75), m_shooterSubsystem)) - .onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem)); - new JoystickButton(m_driverController, Button.kA.value) - .onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(-0.75), m_shooterSubsystem)) - .onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem)); + .whileTrue( new IntakeCommand(m_intakeSubsystem) ); + // TODO: Move shoot commands to operator controller + // new JoystickButton(m_driverController, Button.kY.value) + // .onTrue(new InstantCommand(() -> m_intakeSubsystem.tiltToAngle(IntakeConstants.kIntakeLoweredAngle), + // m_intakeSubsystem)) + // .onFalse(new InstantCommand(m_intakeSubsystem::stopRotating, m_intakeSubsystem)); + // new JoystickButton(m_driverController, Button.kX.value) + // .onTrue(new InstantCommand(() -> m_intakeSubsystem.tiltToAngle(IntakeConstants.kIntakeRaisedAngle), + // m_intakeSubsystem)) + // .onFalse(new InstantCommand(m_intakeSubsystem::stopRotating, m_intakeSubsystem)); + } /** diff --git a/src/main/java/frc/robot/commands/IntakeCommand.java b/src/main/java/frc/robot/commands/IntakeCommand.java new file mode 100644 index 0000000..39c081a --- /dev/null +++ b/src/main/java/frc/robot/commands/IntakeCommand.java @@ -0,0 +1,50 @@ +// 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 frc.robot.commands; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj.DutyCycleEncoder; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.IntakeSubsystem; + +public class IntakeCommand extends Command { + private IntakeSubsystem m_intakeSubsystem; + private Timer m_timer = new Timer(); + + + /** Creates a new intakeCommand. */ + public IntakeCommand(IntakeSubsystem subsystem) { + m_intakeSubsystem = subsystem; + addRequirements(m_intakeSubsystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_timer.reset(); + m_timer.start(); + m_intakeSubsystem.turnOn(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if (m_intakeSubsystem.getDistanceSensor() < 5){ + m_intakeSubsystem.turnOff(); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + m_intakeSubsystem.turnOff(); + } + + @Override + public boolean isFinished(){ + return m_timer.hasElapsed(5); + } +} diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java deleted file mode 100644 index f606d5b..0000000 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ /dev/null @@ -1,232 +0,0 @@ -// 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 frc.robot.subsystems; - -import com.kauailabs.navx.frc.AHRS; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.DriveConstants; -import frc.robot.Constants.VisionConstants; -import frc.robot.Robot; - -public class DriveSubsystem extends SubsystemBase { - private final SwerveModule m_frontLeft = new SwerveModule( - DriveConstants.kFrontLeftDriveMotorPort, - DriveConstants.kFrontLeftTurningMotorPort, - DriveConstants.kFrontLeftTurningEncoderPort, - DriveConstants.kFrontLeftDriveMotorReversed, - DriveConstants.kFrontLeftTurningEncoderOffset); - - private final SwerveModule m_rearLeft = new SwerveModule( - DriveConstants.kRearLeftDriveMotorPort, - DriveConstants.kRearLeftTurningMotorPort, - DriveConstants.kRearLeftTurningEncoderPort, - DriveConstants.kRearLeftDriveMotorReversed, - DriveConstants.kRearLeftTurningEncoderOffset); - - private final SwerveModule m_frontRight = new SwerveModule( - DriveConstants.kFrontRightDriveMotorPort, - DriveConstants.kFrontRightTurningMotorPort, - DriveConstants.kFrontRightTurningEncoderPort, - DriveConstants.kFrontRightDriveMotorReversed, - DriveConstants.kFrontRightTurningEncoderOffset); - - private final SwerveModule m_rearRight = new SwerveModule( - DriveConstants.kRearRightDriveMotorPort, - DriveConstants.kRearRightTurningMotorPort, - DriveConstants.kRearRightTurningEncoderPort, - DriveConstants.kRearRightDriveMotorReversed, - DriveConstants.kRearRightTurningEncoderOffset); - - private final AHRS m_gyro = new AHRS(); - private double m_gyroAngle; - - private final Timer m_headingCorrectionTimer = new Timer(); - private final PIDController m_headingCorrectionPID = new PIDController(DriveConstants.kPHeadingCorrectionController, - 0, 0); - private SwerveModulePosition[] m_swerveModulePositions = new SwerveModulePosition[] { - m_frontLeft.getPosition(), - m_frontRight.getPosition(), - m_rearLeft.getPosition(), - m_rearRight.getPosition() - }; - - private final SwerveDrivePoseEstimator m_poseEstimator = new SwerveDrivePoseEstimator(DriveConstants.kDriveKinematics, - m_gyro.getRotation2d(), m_swerveModulePositions, new Pose2d(), VisionConstants.kOdometrySTDDevs, - VisionConstants.kVisionSTDDevs); - - private final Field2d m_field = new Field2d(); - - /** Creates a new DriveSubsystem. */ - public DriveSubsystem() { - SmartDashboard.putData("Field", m_field); - m_headingCorrectionTimer.restart(); - m_headingCorrectionPID.enableContinuousInput(-Math.PI, Math.PI); - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - - m_swerveModulePositions = new SwerveModulePosition[] { - m_frontLeft.getPosition(), - m_frontRight.getPosition(), - m_rearLeft.getPosition(), - m_rearRight.getPosition() - }; - - m_poseEstimator.update(Robot.isReal() ? m_gyro.getRotation2d() : new Rotation2d(m_gyroAngle), - m_swerveModulePositions); - - m_field.setRobotPose(m_poseEstimator.getEstimatedPosition()); - - SmartDashboard.putNumber("gyro angle", m_gyro.getAngle()); - SmartDashboard.putNumber("odometryX", m_poseEstimator.getEstimatedPosition().getX()); - SmartDashboard.putNumber("odometryY", m_poseEstimator.getEstimatedPosition().getY()); - - // AdvantageScope Logging - double[] logData = { - m_frontLeft.getPosition().angle.getDegrees(), m_frontLeft.driveOutput, - m_frontRight.getPosition().angle.getDegrees(), m_frontRight.driveOutput, - m_rearLeft.getPosition().angle.getDegrees(), m_rearLeft.driveOutput, - m_rearRight.getPosition().angle.getDegrees(), m_rearRight.driveOutput, - }; - SmartDashboard.putNumberArray("AdvantageScope Swerve States", logData); - } - - /** - * Returns the currently-estimated pose of the robot. - * - * @return The pose. - */ - public Pose2d getPose() { - return m_poseEstimator.getEstimatedPosition(); - } - - /** - * Method to drive the robot using joystick info. - * - * @param xSpeed Speed of the robot in the x direction (forward). - * @param ySpeed Speed of the robot in the y direction (sideways). - * @param rotation Angular rotation speed of the robot. - * @param fieldRelative Whether the provided x and y speeds are relative to the - * field. - */ - public void drive(double xSpeed, double ySpeed, double rotation, boolean fieldRelative) { - // If we are rotating, reset the timer - if (rotation != 0) { - m_headingCorrectionTimer.reset(); - } - - /* - * Heading correction helps maintain the same heading and - * prevents rotational drive while our robot is translating - * - * For heading correction we use a timer to ensure that we - * lose all rotational momentum before saving the heading - * that we want to maintain - */ - - // TODO: Test heading correction without timer - // TODO: Test heading correction using gyro's rotational velocity (if it is 0 - // then set heading instead of timer) - - // Save our desired rotation to a variable we can add our heading correction - // adjustments to - double calculatedRotation = rotation; - - double currentAngle = MathUtil.angleModulus(m_gyro.getRotation2d().getRadians()); - - // If we are not translating or if not enough time has passed since the last - // time we rotated - if ((xSpeed == 0 && ySpeed == 0) - || m_headingCorrectionTimer.get() < DriveConstants.kHeadingCorrectionTurningStopTime) { - // Update our desired angle - m_headingCorrectionPID.setSetpoint(currentAngle); - } else { - // If we are translating or if we have not rotated for a long enough time - // then maintain our desired angle - calculatedRotation = m_headingCorrectionPID.calculate(currentAngle); - } - - // Depending on whether the robot is being driven in field relative, calculate - // the desired states for each of the modules - SwerveModuleState[] swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates( - fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, calculatedRotation, - Robot.isReal() ? m_gyro.getRotation2d() : new Rotation2d(m_gyroAngle)) - : new ChassisSpeeds(xSpeed, ySpeed, calculatedRotation)); - - setModuleStates(swerveModuleStates); - } - - /** - * Resets the odometry to the specified pose. - * - * @param pose The pose to which to set the odometry. - */ - public void resetOdometry(Pose2d pose) { - m_poseEstimator.resetPosition( - Robot.isReal() ? m_gyro.getRotation2d() : new Rotation2d(m_gyroAngle), - new SwerveModulePosition[] { - m_frontLeft.getPosition(), - m_frontRight.getPosition(), - m_rearLeft.getPosition(), - m_rearRight.getPosition() - }, - pose); - } - - /** Zeroes the heading of the robot. */ - public void zeroHeading() { - m_gyro.reset(); - m_gyroAngle = 0; - } - - public void addVisionMeasurement(Pose2d pose, double timestamp) { - m_poseEstimator.addVisionMeasurement(pose, timestamp); - } - - /** - * Sets the swerve ModuleStates. - * - * @param desiredStates The desired SwerveModule states. - */ - public void setModuleStates(SwerveModuleState[] desiredStates) { - SwerveDriveKinematics.desaturateWheelSpeeds( - desiredStates, DriveConstants.kMaxSpeedMetersPerSecond); - m_frontLeft.setDesiredState(desiredStates[0]); - m_frontRight.setDesiredState(desiredStates[1]); - m_rearLeft.setDesiredState(desiredStates[2]); - m_rearRight.setDesiredState(desiredStates[3]); - - // AdvantageScope Logging - double[] logData = { - desiredStates[0].angle.getDegrees(), desiredStates[0].speedMetersPerSecond, - desiredStates[1].angle.getDegrees(), desiredStates[1].speedMetersPerSecond, - desiredStates[2].angle.getDegrees(), desiredStates[2].speedMetersPerSecond, - desiredStates[3].angle.getDegrees(), desiredStates[3].speedMetersPerSecond, - }; - SmartDashboard.putNumberArray("AdvantageScope Swerve Desired States", logData); - - // Takes the integral of the rotation speed to find the current angle for the - // simulator - m_gyroAngle += DriveConstants.kDriveKinematics.toChassisSpeeds(desiredStates).omegaRadiansPerSecond - * Robot.kDefaultPeriod; - } - -} diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 9d55b9c..15e145c 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -4,59 +4,114 @@ package frc.robot.subsystems; +import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue; import com.revrobotics.CANSparkFlex; +import com.revrobotics.Rev2mDistanceSensor; +import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.Rev2mDistanceSensor.Port; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj.DutyCycleEncoder; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.Constants; import frc.robot.Constants.IntakeConstants; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class IntakeSubsystem extends SubsystemBase { - CANSparkFlex m_intakeMotor = new CANSparkFlex(IntakeConstants.kIntakeMotorID, MotorType.kBrushless); - CANSparkFlex m_armMotor = new CANSparkFlex(IntakeConstants.kArmMotorID, MotorType.kBrushless); + boolean deployed = true; + + private double m_pivotSpeed = 0; + private double m_intakeSpeed = 0; + + CANSparkFlex m_topFeeder = new CANSparkFlex(0, MotorType.kBrushless); + CANSparkFlex m_bottomFeeder = new CANSparkFlex(0, MotorType.kBrushless); + CANSparkFlex m_pivotMotor = new CANSparkFlex(0, MotorType.kBrushless); + + PIDController m_pivotPID = new PIDController(0.5, 0, 0); - PIDController m_intakeVeloPID = new PIDController(IntakeConstants.kIntakeP, IntakeConstants.kIntakeI, - IntakeConstants.kIntakeD); - PIDController m_armPID = new PIDController(IntakeConstants.kArmP, IntakeConstants.kArmI, IntakeConstants.kArmD); + DutyCycleEncoder m_pivotEncoder = new DutyCycleEncoder(0); - DutyCycleEncoder m_armEncoder = new DutyCycleEncoder(IntakeConstants.kArmEncoderCh); + Rev2mDistanceSensor m_distanceSensor; /** Creates a new intake. */ public IntakeSubsystem() { + //m_pivotMotor.setInverted(Constants.IntakeConstants.kPivotMotorInverted); + + // TODO: honestly no idea waht to set + //m_pivotEncoder.configMagnetOffset(Constants.IntakeConstants.kPivotEncoderOffset); + //m_pivotEncoder.configAbsoluteSensorRange(); + + m_topFeeder.setIdleMode(IdleMode.kBrake); + m_bottomFeeder.setIdleMode(IdleMode.kBrake); + + m_pivotMotor.setIdleMode(IdleMode.kBrake); + + m_pivotPID.setTolerance(2); + m_distanceSensor = new Rev2mDistanceSensor(Port.kOnboard); //onboard I2C port + m_distanceSensor.setAutomaticMode(true); } - // Starts intaking the disk - public void intakeDisk() { - m_intakeMotor.set(IntakeConstants.kIntakeSpeed); + public void turnOn(){ + m_pivotPID.setSetpoint(Constants.IntakeConstants.kIntakeLoweredAngle); + + deployed = true; } - //Stops rotating the intake - public void stopIntaking() { - m_intakeMotor.set(0); + public void turnOff(){ + m_pivotPID.setSetpoint(Constants.IntakeConstants.kIntakeRaisedAngle); + + deployed = false; } /** - * Rotates the arm to a given angle - * @param angle motor to apply to intake + * Gets distance from Rev 2m sensor * */ - public void tiltToAngle(double angle) { - m_armMotor.set(m_armPID.calculate(m_armEncoder.getAbsolutePosition(), angle)); + public double getDistanceSensor() { + return m_distanceSensor.getRange(); } - //Stops rotating the arm - public void stopRotating(){ - m_armMotor.set(0); + /* + * Helper function to calculate motor speeds + * + */ + private void calculateSpeeds(){ + if (m_pivotEncoder.getAbsolutePosition() > IntakeConstants.kIntakeLoweredAngle && m_pivotEncoder.getAbsolutePosition() < IntakeConstants.kIntakeRaisedAngle){ + m_pivotSpeed = MathUtil.clamp(m_pivotPID.calculate(m_pivotEncoder.getAbsolutePosition()), -0.5, 0.5); + } + else{ + if (m_pivotPID.atSetpoint()){ + m_pivotSpeed = 0.0; + } + } + + if (m_pivotEncoder.getAbsolutePosition() < 15.0 && deployed){ + m_intakeSpeed = Constants.IntakeConstants.kIntakeSpeed; //TODO: i have no idea which speed + } else { + m_intakeSpeed = 0.0; + } } @Override public void periodic() { - SmartDashboard.putBoolean("Intake",m_intakeMotor.get()>0); + calculateSpeeds(); + + m_pivotMotor.set(m_pivotSpeed); + + m_bottomFeeder.set(m_intakeSpeed); + m_topFeeder.set(m_intakeSpeed); + } + + public double getPivotPosition(){ + return m_pivotEncoder.getAbsolutePosition(); + } - // This method will be called once per scheduler run + public boolean readyForShooter(){ + return (m_pivotEncoder.getAbsolutePosition() < Constants.IntakeConstants.kIntakeRaisedAngle + 5) + || (m_pivotEncoder.getAbsolutePosition() > Constants.IntakeConstants.kIntakeRaisedAngle - 5); } } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java deleted file mode 100644 index a47de12..0000000 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ /dev/null @@ -1,32 +0,0 @@ -// 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 frc.robot.subsystems; - -import com.revrobotics.CANSparkFlex; -import com.revrobotics.CANSparkLowLevel.MotorType; - -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.ShooterConstants; - -public class ShooterSubsystem extends SubsystemBase { - /** Creates a new ShooterSubsystem. */ - CANSparkFlex m_bottom = new CANSparkFlex(ShooterConstants.kBottomShooterMotorPort, MotorType.kBrushless); - CANSparkFlex m_top = new CANSparkFlex(ShooterConstants.kTopShooterMotorPort, MotorType.kBrushless); - - public ShooterSubsystem() { - - } - - public void spin(double speed) { - m_bottom.set(speed); - m_top.set(speed); - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - // SmartDashboard.putNumber("Speed", m_bottom.); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java deleted file mode 100644 index 9d98ea8..0000000 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ /dev/null @@ -1,118 +0,0 @@ -// 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 frc.robot.subsystems; - -import com.ctre.phoenix6.configs.CANcoderConfigurator; -import com.ctre.phoenix6.configs.MagnetSensorConfigs; -import com.ctre.phoenix6.hardware.CANcoder; -import com.revrobotics.CANSparkBase.IdleMode; -import com.revrobotics.CANSparkLowLevel.MotorType; -import com.revrobotics.CANSparkMax; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import frc.robot.Constants.DriveConstants; -import frc.robot.Robot; - -public class SwerveModule { - private final CANSparkMax m_driveMotor; - private final CANSparkMax m_turningMotor; - - private final CANcoder m_turningEncoder; - private final CANcoderConfigurator m_turningEncoderConfigurator; - - private final PIDController m_turningPIDController = new PIDController(DriveConstants.kPModuleTurningController, 0, - 0); - - private SwerveModuleState m_state = new SwerveModuleState(); - private double m_distance; - - public double driveOutput; - public double turnOutput; - - /** - * Constructs a {@link SwerveModule}. - * - * @param driveMotorPort The port of the drive motor. - * @param turningMotorPort The port of the turning motor. - * @param turningEncoderPort The port of the turning encoder. - * @param driveMotorReversed Whether the drive motor is reversed. - * @param turningEncoderOffset Offset of the turning encoder. - */ - public SwerveModule( - int driveMotorPort, - int turningMotorPort, - int turningEncoderPort, - boolean driveMotorReversed, - double turningEncoderOffset) { - m_driveMotor = new CANSparkMax(driveMotorPort, MotorType.kBrushless); - m_turningMotor = new CANSparkMax(turningMotorPort, MotorType.kBrushless); - m_turningEncoder = new CANcoder(turningEncoderPort); - m_turningEncoderConfigurator = m_turningEncoder.getConfigurator(); - - // converts default units to meters per second - m_driveMotor.getEncoder().setVelocityConversionFactor( - DriveConstants.kWheelDiameterMeters * Math.PI / 60 / DriveConstants.kDrivingGearRatio); - - m_driveMotor.setInverted(driveMotorReversed); - - m_turningMotor.setIdleMode(IdleMode.kBrake); - - // TODO: CANcoder offsets are now set on the device manually using Pheonix Tuner - // (or maybe Pheonix X) - m_turningEncoderConfigurator.apply(new MagnetSensorConfigs().withMagnetOffset(-turningEncoderOffset)); - - m_turningPIDController.enableContinuousInput(-Math.PI, Math.PI); - } - - /** - * Returns the current position of the module. - * - * @return The current position of the module. - */ - public SwerveModulePosition getPosition() { - m_distance += m_state.speedMetersPerSecond * Robot.kDefaultPeriod; - - // If the robot is real, then return the swerve module state by reading from the - // actual encoders - // If the robot is simulated, then return the swerve module state using the - // expected values - return Robot.isReal() - ? new SwerveModulePosition(m_driveMotor.getEncoder().getPosition(), - getEncoderAngle(m_turningEncoder)) - : new SwerveModulePosition(m_distance, m_state.angle); - } - - /** - * Sets the desired state for the module. - * - * @param desiredState Desired state with speed and angle. - */ - public void setDesiredState(SwerveModuleState desiredState) { - m_state = SwerveModuleState.optimize(desiredState, getEncoderAngle(m_turningEncoder)); - driveOutput = m_state.speedMetersPerSecond / DriveConstants.kMaxSpeedMetersPerSecond; - - turnOutput = m_turningPIDController.calculate(getEncoderAngle(m_turningEncoder).getRadians(), - m_state.angle.getRadians()); - - m_driveMotor.set(driveOutput); - m_turningMotor.set(turnOutput); - } - - /** - * Returns the angle of a CANcoder - * - * The CANcoder now gives values in rotations which is useless, so this method - * translates the CANcoder output into a Rotation2D - * - * @param encoder The encoder to get the absolute angle of. - * @return A Rotation2d of the absolute angle. - */ - public static Rotation2d getEncoderAngle(CANcoder encoder) { - return new Rotation2d(encoder.getAbsolutePosition().getValueAsDouble() * 2 * Math.PI); - } -} diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java deleted file mode 100644 index daeea25..0000000 --- a/src/main/java/frc/robot/subsystems/VisionSubsystem.java +++ /dev/null @@ -1,128 +0,0 @@ -// 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 frc.robot.subsystems; - -import java.util.Arrays; -import java.util.Optional; - -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.networktables.DoubleArraySubscriber; -import edu.wpi.first.networktables.DoubleSubscriber; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.TimestampedDoubleArray; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.VisionConstants; - -/** - *

- * In 3D poses and transforms: - *

- * - *

- * On the field (0, 0, 0) in 3D space is the right corner of the blue alliance - * driver station - * Therefore, from the blue driver station: +X is forward, +Y is left, +Z is up - * - *

- * In 2D poses and transforms: - *

- */ -public class VisionSubsystem extends SubsystemBase { - - NetworkTable m_visionNetworkTable = NetworkTableInstance.getDefault().getTable("limelight"); - - private final DoubleArraySubscriber m_botPose; - private final DoubleSubscriber m_cl; - private final DoubleSubscriber m_tl; - - /** Creates a new Limelight. */ - public VisionSubsystem() { - // Provide the limelight with the camera pose relative to the center of the - // robot - m_visionNetworkTable.getEntry("camerapose_robotspace_set").setDoubleArray(VisionConstants.kLimelightCamPose); - - // Create subscribers to get values from the limelight - m_botPose = m_visionNetworkTable.getDoubleArrayTopic("botpose_wpiblue").subscribe(null); - m_cl = m_visionNetworkTable.getDoubleTopic("cl").subscribe(0); - m_tl = m_visionNetworkTable.getDoubleTopic("tl").subscribe(0); - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - } - - public Optional getMeasurement() { - TimestampedDoubleArray[] updates = m_botPose.readQueue(); - - // If we have had no updates since the last time this method ran then return - // nothing - if (updates.length == 0) { - return Optional.empty(); - } - - TimestampedDoubleArray update = updates[updates.length - 1]; - - // If the latest update is empty then return nothing - if (Arrays.equals(update.value, new double[6])) { - return Optional.empty(); - } - - double x = update.value[0]; - double y = update.value[1]; - double z = update.value[2]; - double roll = Units.degreesToRadians(update.value[3]); - double pitch = Units.degreesToRadians(update.value[4]); - double yaw = Units.degreesToRadians(update.value[5]); - - double latency = m_cl.get() + m_tl.get(); - - double timestamp = (update.timestamp * 1e-6) - (latency * 1e-3); - Pose3d pose = new Pose3d(new Translation3d(x, y, z), new Rotation3d(roll, pitch, yaw)); - - /* - * The limelight returns 3D field poses where (0, 0, 0) is located at the center - * of the field - * - * So to input this pose into our pose estimator we need to tranform so that (0, - * 0, 0) is the right corner of the blue driver stations - */ - // TODO: Check if we actually need to do this... - // pose.transformBy(new Transform3d(new Translation3d(VisionConstants.kFieldLength, VisionConstants.kFieldWidth, 0.0), new Rotation3d())); - - return Optional.of(new Measurement( - timestamp, - pose, - VisionConstants.kVisionSTDDevs)); - } - - public static class Measurement { - public double timestamp; - public Pose3d pose; - public Matrix stdDeviation; - - public Measurement(double timestamp, Pose3d pose, Matrix stdDeviation) { - this.timestamp = timestamp; - this.pose = pose; - this.stdDeviation = stdDeviation; - } - } -} diff --git a/vendordeps/REV2mDistanceSensor.json b/vendordeps/REV2mDistanceSensor.json new file mode 100644 index 0000000..adcfa42 --- /dev/null +++ b/vendordeps/REV2mDistanceSensor.json @@ -0,0 +1,56 @@ +{ + "fileName": "REV2mDistanceSensor.json", + "name": "REV2mDistanceSensor", + "version": "0.4.0", + "frcYear": "2024", + "uuid": "9e352acd-4eec-40f7-8490-3357b5ed65ae", + "mavenUrls": [ + "https://www.revrobotics.com/content/sw/max/sdk/maven/" + ], + "jsonUrl": "https://www.revrobotics.com/content/sw/max/sdk/Rev2mDistanceSensor.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "DistanceSensor-java", + "version": "0.4.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "DistanceSensor-driver", + "version": "0.4.0", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "linuxathena" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "DistanceSensor-cpp", + "version": "0.4.0", + "libName": "libDistanceSensor", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "DistanceSensor-driver", + "version": "0.4.0", + "libName": "libDistanceSensorDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena" + ] + } + ] +} \ No newline at end of file