From e4c97000c4414ce2623e68e571b91398772df4a6 Mon Sep 17 00:00:00 2001 From: Programming Date: Fri, 9 Feb 2024 17:09:13 -0800 Subject: [PATCH 01/13] feat: Climber Subsystem uses pneumatics --- src/main/java/frc/robot/RobotContainer.java | 19 ++++++- src/main/java/frc/robot/climberConst.java | 11 ++++ src/main/java/frc/robot/climberSubSystem.java | 55 +++++++++++++++++++ 3 files changed, 84 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/climberConst.java create mode 100644 src/main/java/frc/robot/climberSubSystem.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index de2c9d0..fb30054 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,15 +4,32 @@ package frc.robot; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.XboxController.Button; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import frc.robot.climberSubSystem; public class RobotContainer { + private final XboxController m_controller = new XboxController(0); + private final climberSubSystem climberSubSystem = new climberSubSystem(); public RobotContainer() { configureBindings(); + + + + } - private void configureBindings() {} + private void configureBindings() { + + new JoystickButton(m_controller, Button.kY.value) + .onTrue(new InstantCommand(climberSubSystem::toggle)); + + } public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); diff --git a/src/main/java/frc/robot/climberConst.java b/src/main/java/frc/robot/climberConst.java new file mode 100644 index 0000000..013c70e --- /dev/null +++ b/src/main/java/frc/robot/climberConst.java @@ -0,0 +1,11 @@ +package frc.robot; + +public class climberConst { + public final static int climberForwardChannel = 0; + public final static int climberReverseChannel = 1; + + public final static double minPressure = 50.0; + public final static double maxPressure = 120.0; + + +} diff --git a/src/main/java/frc/robot/climberSubSystem.java b/src/main/java/frc/robot/climberSubSystem.java new file mode 100644 index 0000000..c0c6831 --- /dev/null +++ b/src/main/java/frc/robot/climberSubSystem.java @@ -0,0 +1,55 @@ +package frc.robot; + +import edu.wpi.first.wpilibj.Compressor; +import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj.PneumaticsModuleType; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kForward; +import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kOff; +import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kReverse; + +public class climberSubSystem { + private final DoubleSolenoid m_doubleSolenoid = new DoubleSolenoid(PneumaticsModuleType.REVPH, climberConst.climberForwardChannel, climberConst.climberReverseChannel); + private final Compressor m_compressor = new Compressor(PneumaticsModuleType.REVPH); + private boolean enableCompressor = true; + + + public climberSubSystem(){ + m_doubleSolenoid.set(kOff); + m_compressor.disable(); + m_compressor.enableAnalog(climberConst.minPressure, climberConst.maxPressure); + } + + public void periodic() { + SmartDashboard.putNumber("Pressure", m_compressor.getPressure()); + SmartDashboard.putBoolean("Compressor Enabled", m_compressor.isEnabled()); + SmartDashboard.putBoolean("Startup Compressor", enableCompressor); + } + + public void off(){ + m_doubleSolenoid.set(kOff); + } + + public void forward() { + m_doubleSolenoid.set(kForward); + } + + public void reverse() { + m_doubleSolenoid.set(kReverse); + } + + public void toggle() { + m_doubleSolenoid.toggle(); + } + + public void toggleCompresor(){ + enableCompressor = !enableCompressor; + if (enableCompressor){ + m_compressor.enableAnalog(climberConst.minPressure, climberConst.maxPressure); + } + else{ + m_compressor.disable(); + } + } +} From 759c15f2df710e4f6c4197785819d97e44b819d3 Mon Sep 17 00:00:00 2001 From: Programming Date: Mon, 12 Feb 2024 16:05:34 -0800 Subject: [PATCH 02/13] Docs: Climber --- src/main/java/frc/robot/RobotContainer.java | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fb30054..19b3544 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,14 +4,12 @@ package frc.robot; -import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.XboxController.Button; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; -import frc.robot.climberSubSystem; public class RobotContainer { private final XboxController m_controller = new XboxController(0); @@ -25,7 +23,8 @@ public RobotContainer() { } private void configureBindings() { - + //When Y button is pressed, the climber will toggle up or down + //based on current position, it will be reversed new JoystickButton(m_controller, Button.kY.value) .onTrue(new InstantCommand(climberSubSystem::toggle)); From 831e1fdeb8ba288e74bdde3a03df158cb584f62e Mon Sep 17 00:00:00 2001 From: Programming Date: Tue, 20 Feb 2024 18:07:53 -0800 Subject: [PATCH 03/13] merge: merged main into climber --- build.gradle | 2 +- networktables.json | 1 + simgui-ds.json | 97 +++++ simgui.json | 18 + src/main/java/frc/robot/Main.java | 10 + src/main/java/frc/robot/Robot.java | 50 ++- .../frc/robot/subsystems/DriveSubsystem.java | 232 ++++++++++++ .../frc/robot/subsystems/IntakeSubsystem.java | 62 ++++ .../robot/subsystems/ShooterSubsystem.java | 32 ++ .../frc/robot/subsystems/SwerveModule.java | 118 ++++++ .../frc/robot/subsystems/VisionSubsystem.java | 128 +++++++ vendordeps/NavX.json | 40 +++ vendordeps/Phoenix6.json | 339 ++++++++++++++++++ vendordeps/REVLib.json | 74 ++++ 14 files changed, 1192 insertions(+), 11 deletions(-) create mode 100644 networktables.json create mode 100644 simgui-ds.json create mode 100644 simgui.json create mode 100644 src/main/java/frc/robot/subsystems/DriveSubsystem.java create mode 100644 src/main/java/frc/robot/subsystems/IntakeSubsystem.java create mode 100644 src/main/java/frc/robot/subsystems/ShooterSubsystem.java create mode 100644 src/main/java/frc/robot/subsystems/SwerveModule.java create mode 100644 src/main/java/frc/robot/subsystems/VisionSubsystem.java create mode 100644 vendordeps/NavX.json create mode 100644 vendordeps/Phoenix6.json create mode 100644 vendordeps/REVLib.json diff --git a/build.gradle b/build.gradle index 1478f2b..d141f06 100644 --- a/build.gradle +++ b/build.gradle @@ -68,7 +68,7 @@ dependencies { simulationRelease wpi.sim.enableRelease() testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' - testRuntimeOnly 'org.junit.platform:junit-platform-launcher' + //testRuntimeOnly 'org.junit.platform:junit-platform-launcher' } test { diff --git a/networktables.json b/networktables.json new file mode 100644 index 0000000..fe51488 --- /dev/null +++ b/networktables.json @@ -0,0 +1 @@ +[] diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..69b1a3c --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,97 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ], + "robotJoysticks": [ + { + "guid": "Keyboard0" + } + ] +} diff --git a/simgui.json b/simgui.json new file mode 100644 index 0000000..905d0ec --- /dev/null +++ b/simgui.json @@ -0,0 +1,18 @@ +{ + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo", + "/SmartDashboard/Field": "Field2d" + }, + "windows": { + "/SmartDashboard/Field": { + "window": { + "visible": true + } + } + } + }, + "NetworkTables Info": { + "visible": true + } +} diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java index fe215d7..8776e5d 100644 --- a/src/main/java/frc/robot/Main.java +++ b/src/main/java/frc/robot/Main.java @@ -6,9 +6,19 @@ import edu.wpi.first.wpilibj.RobotBase; +/** + * Do NOT add any static variables to this class, or any initialization at all. Unless you know what + * you are doing, do not modify this file except to change the parameter class to the startRobot + * call. + */ public final class Main { private Main() {} + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ public static void main(String... args) { RobotBase.startRobot(Robot::new); } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b68462c..687a0a0 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -8,66 +8,96 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +/** + * The VM is configured to automatically run this class, and to call the functions corresponding to + * each mode, as described in the TimedRobot documentation. If you change the name of this class or + * the package after creating this project, you must also update the build.gradle file in the + * project. + */ public class Robot extends TimedRobot { private Command m_autonomousCommand; private RobotContainer m_robotContainer; + /** + * This function is run when the robot is first started up and should be used for any + * initialization code. + */ @Override public void robotInit() { + // Instantiate our RobotContainer. This will perform all our button bindings, and put our + // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); } + /** + * This function is called every 20 ms, no matter the mode. Use this for items like diagnostics + * that you want ran during disabled, autonomous, teleoperated and test. + * + *

This runs after the mode specific periodic functions, but before LiveWindow and + * SmartDashboard integrated updating. + */ @Override public void robotPeriodic() { + // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled + // commands, running already-scheduled commands, removing finished or interrupted commands, + // and running subsystem periodic() methods. This must be called from the robot's periodic + // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); } + /** This function is called once each time the robot enters Disabled mode. */ @Override public void disabledInit() {} @Override public void disabledPeriodic() {} - @Override - public void disabledExit() {} - + /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ @Override public void autonomousInit() { m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + // schedule the autonomous command (example) if (m_autonomousCommand != null) { m_autonomousCommand.schedule(); } } + /** This function is called periodically during autonomous. */ @Override public void autonomousPeriodic() {} - @Override - public void autonomousExit() {} - @Override public void teleopInit() { + // This makes sure that the autonomous stops running when + // teleop starts running. If you want the autonomous to + // continue until interrupted by another command, remove + // this line or comment it out. if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } } + /** This function is called periodically during operator control. */ @Override public void teleopPeriodic() {} - @Override - public void teleopExit() {} - @Override public void testInit() { + // Cancels all running commands at the start of test mode. CommandScheduler.getInstance().cancelAll(); } + /** This function is called periodically during test mode. */ @Override public void testPeriodic() {} + /** This function is called once when the robot is first started up. */ + @Override + public void simulationInit() {} + + /** This function is called periodically whilst in simulation. */ @Override - public void testExit() {} + public void simulationPeriodic() {} } diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java new file mode 100644 index 0000000..f606d5b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -0,0 +1,232 @@ +// 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 new file mode 100644 index 0000000..9d55b9c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -0,0 +1,62 @@ +// 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.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.IntakeConstants; + + +public class IntakeSubsystem extends SubsystemBase { + CANSparkFlex m_intakeMotor = new CANSparkFlex(IntakeConstants.kIntakeMotorID, MotorType.kBrushless); + CANSparkFlex m_armMotor = new CANSparkFlex(IntakeConstants.kArmMotorID, MotorType.kBrushless); + + PIDController m_intakeVeloPID = new PIDController(IntakeConstants.kIntakeP, IntakeConstants.kIntakeI, + IntakeConstants.kIntakeD); + PIDController m_armPID = new PIDController(IntakeConstants.kArmP, IntakeConstants.kArmI, IntakeConstants.kArmD); + + DutyCycleEncoder m_armEncoder = new DutyCycleEncoder(IntakeConstants.kArmEncoderCh); + + /** Creates a new intake. */ + public IntakeSubsystem() { + + } + + // Starts intaking the disk + public void intakeDisk() { + m_intakeMotor.set(IntakeConstants.kIntakeSpeed); + } + + //Stops rotating the intake + public void stopIntaking() { + m_intakeMotor.set(0); + } + + /** + * Rotates the arm to a given angle + * @param angle motor to apply to intake + * + */ + public void tiltToAngle(double angle) { + m_armMotor.set(m_armPID.calculate(m_armEncoder.getAbsolutePosition(), angle)); + } + + //Stops rotating the arm + public void stopRotating(){ + m_armMotor.set(0); + } + + @Override + public void periodic() { + SmartDashboard.putBoolean("Intake",m_intakeMotor.get()>0); + + // This method will be called once per scheduler run + } +} diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java new file mode 100644 index 0000000..a47de12 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -0,0 +1,32 @@ +// 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 new file mode 100644 index 0000000..9d98ea8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -0,0 +1,118 @@ +// 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 new file mode 100644 index 0000000..daeea25 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/VisionSubsystem.java @@ -0,0 +1,128 @@ +// 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: + *

    + *
  • +X is north/forward, + *
  • +Y is west/left, + *
  • +Z is up. + *
+ * + *

+ * 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: + *

    + *
  • +X is away from the driver, + *
  • +Y is toward the blue alliance driver's left and to the red alliance + * driver's right + *
  • +Rotation is clockwise + *
+ */ +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/NavX.json b/vendordeps/NavX.json new file mode 100644 index 0000000..e978a5f --- /dev/null +++ b/vendordeps/NavX.json @@ -0,0 +1,40 @@ +{ + "fileName": "NavX.json", + "name": "NavX", + "version": "2024.1.0", + "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", + "frcYear": "2024", + "mavenUrls": [ + "https://dev.studica.com/maven/release/2024/" + ], + "jsonUrl": "https://dev.studica.com/releases/2024/NavX.json", + "javaDependencies": [ + { + "groupId": "com.kauailabs.navx.frc", + "artifactId": "navx-frc-java", + "version": "2024.1.0" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.kauailabs.navx.frc", + "artifactId": "navx-frc-cpp", + "version": "2024.1.0", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": false, + "libName": "navx_frc", + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxraspbian", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json new file mode 100644 index 0000000..2b7d172 --- /dev/null +++ b/vendordeps/Phoenix6.json @@ -0,0 +1,339 @@ +{ + "fileName": "Phoenix6.json", + "name": "CTRE-Phoenix (v6)", + "version": "24.2.0", + "frcYear": 2024, + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json", + "conflictsWith": [ + { + "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a", + "errorMessage": "The combined Phoenix-6-And-5 vendordep is no longer supported. Please remove the vendordep and instead add both the latest Phoenix 6 vendordep and Phoenix 5 vendordep.", + "offlineFileName": "Phoenix6And5.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-java", + "version": "24.2.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "24.2.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "24.2.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "24.2.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonFX", + "version": "24.2.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "24.2.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "24.2.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "24.2.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "24.2.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "24.2.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "24.2.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-cpp", + "version": "24.2.0", + "libName": "CTRE_Phoenix6_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "24.2.0", + "libName": "CTRE_PhoenixTools", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "24.2.0", + "libName": "CTRE_Phoenix6_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "24.2.0", + "libName": "CTRE_PhoenixTools_Sim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "24.2.0", + "libName": "CTRE_SimTalonSRX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonFX", + "version": "24.2.0", + "libName": "CTRE_SimTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "24.2.0", + "libName": "CTRE_SimVictorSPX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "24.2.0", + "libName": "CTRE_SimPigeonIMU", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "24.2.0", + "libName": "CTRE_SimCANCoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "24.2.0", + "libName": "CTRE_SimProTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "24.2.0", + "libName": "CTRE_SimProCANcoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "24.2.0", + "libName": "CTRE_SimProPigeon2", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} \ No newline at end of file diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json new file mode 100644 index 0000000..0f3520e --- /dev/null +++ b/vendordeps/REVLib.json @@ -0,0 +1,74 @@ +{ + "fileName": "REVLib.json", + "name": "REVLib", + "version": "2024.2.0", + "frcYear": "2024", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "https://maven.revrobotics.com/" + ], + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2024.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-java", + "version": "2024.2.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2024.2.0", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-cpp", + "version": "2024.2.0", + "libName": "REVLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2024.2.0", + "libName": "REVLibDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} \ No newline at end of file From 14764433e9485c89b81ee2fdbb8656f6c3795e54 Mon Sep 17 00:00:00 2001 From: Programming Date: Tue, 20 Feb 2024 18:09:14 -0800 Subject: [PATCH 04/13] fix/refactor: rewrote all files related to commit and implemented joystick controls --- src/main/java/frc/robot/Constants.java | 154 ++++++++++++++++++ src/main/java/frc/robot/RobotContainer.java | 83 ++++++++-- src/main/java/frc/robot/climberConst.java | 11 -- src/main/java/frc/robot/climberSubSystem.java | 11 +- 4 files changed, 233 insertions(+), 26 deletions(-) create mode 100644 src/main/java/frc/robot/Constants.java delete mode 100644 src/main/java/frc/robot/climberConst.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java new file mode 100644 index 0000000..e0325e4 --- /dev/null +++ b/src/main/java/frc/robot/Constants.java @@ -0,0 +1,154 @@ +// 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; + +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.numbers.N3; + +/** + * The Constants class provides a convenient place for teams to hold robot-wide + * numerical or boolean + * constants. This class should not be used for any other purpose. All constants + * should be declared + * globally (i.e. public static). Do not put anything functional in this class. + * + *

+ * It is advised to statically import this class (or one of its inner classes) + * wherever the + * constants are needed, to reduce verbosity. + */ +public final class Constants { + /** + * Input/Output constants + */ + public static final class IOConstants { + public static final int kDriverControllerPort = 0; + + public static final double kControllerDeadband = 0.2; + public static final double kSlowModeScalar = 0.8; + } + + public static final class DriveConstants { + // TODO: set motor and encoder constants + public static final int kFrontLeftDriveMotorPort = 32; + public static final int kRearLeftDriveMotorPort = 29; + public static final int kFrontRightDriveMotorPort = 38; + public static final int kRearRightDriveMotorPort = 34; + + public static final int kFrontLeftTurningMotorPort = 28; + public static final int kRearLeftTurningMotorPort = 22; + public static final int kFrontRightTurningMotorPort = 37; + public static final int kRearRightTurningMotorPort = 26; + + public static final int kFrontLeftTurningEncoderPort = 19; + public static final int kRearLeftTurningEncoderPort = 20; + public static final int kFrontRightTurningEncoderPort = 18; + public static final int kRearRightTurningEncoderPort = 17; + + public static final double kFrontLeftTurningEncoderOffset = 0; + public static final double kRearLeftTurningEncoderOffset = 0; + public static final double kFrontRightTurningEncoderOffset = 0; + public static final double kRearRightTurningEncoderOffset = 0; + + // TODO: Test motor orientations before driving on an actual robot + public static final boolean kFrontLeftDriveMotorReversed = false; + public static final boolean kRearLeftDriveMotorReversed = false; + public static final boolean kFrontRightDriveMotorReversed = true; + public static final boolean kRearRightDriveMotorReversed = true; + + /** Distance between centers of right and left wheels on robot (in meters). */ + public static final double kTrackWidth = 0.57785; + + /** Distance between front and back wheels on robot (in meters). */ + public static final double kWheelBase = 0.57785; + + /** Diameter of each wheel in the SDS MK4i swerve module (in meters) */ + public static final double kWheelDiameterMeters = 0.1; + + /** Gear ratio between the motor and the wheel. */ + public static final double kDrivingGearRatio = 8.14; // SDS MK4i's in L1 Configuration + // public static final double kDrivingGearRatio = 6.75; // SDS MK4i's in L2 + // configuration + + // TODO: Tune this PID before running on a robot on the ground + public static final double kPModuleTurningController = -0.3; + + public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics( + new Translation2d(kWheelBase / 2, kTrackWidth / 2), + new Translation2d(kWheelBase / 2, -kTrackWidth / 2), + new Translation2d(-kWheelBase / 2, kTrackWidth / 2), + new Translation2d(-kWheelBase / 2, -kTrackWidth / 2)); + + /** For a a SDS Mk4i L1 swerve base with Neo Vortexes */ + public static final double kMaxSpeedMetersPerSecond = 4.4196; + /** For a a SDS Mk4i L1 swerve base with Neo Vortexes */ + public static final double kMaxAngularSpeedRadiansPerSecond = 10.8164; + // ^^ Calculated using the method taken from the old SDS github example + + /** Heading Correction */ + public static final double kHeadingCorrectionTurningStopTime = 0.2; + // TODO: Tune this PID before running on a robot on the ground + public static final double kPHeadingCorrectionController = 5; + } + + // Intake PID and Encoder Constants + public static class IntakeConstants { + public static final double kIntakeLoweredAngle = 9.0; + public static final double kIntakeRaisedAngle = 9.0; + public static final int kIntakeMotorID = 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; + } + + // Shooter subsystem speed constants + public static class ShooterConstants { + public static final double kSpinSpeedTrue = 0.75; + public static final double kSpinSpeedFalse = 0; + public static int kBottomShooterMotorPort; + public static int kTopShooterMotorPort; + + } + public static class ClimberConstants { + public final static int climberForwardChannel = 0; + public final static int climberReverseChannel = 1; + + public final static double minPressure = 50.0; + public final static double maxPressure = 120.0; + + + } + + public static final class VisionConstants { + // TODO: Update cam pose relative to center of bot + public static final Pose3d kCamPose = new Pose3d(0, 0, 0, new Rotation3d(0, 0, 0)); + public static final double[] kLimelightCamPose = { + kCamPose.getX(), + kCamPose.getY(), + kCamPose.getZ(), + kCamPose.getRotation().getX(), + kCamPose.getRotation().getY(), + kCamPose.getRotation().getZ() }; + + // TODO: Experiment with different std devs in the pose estimator + public static final Vector kOdometrySTDDevs = VecBuilder.fill(0.1, 0.1, 0.1); + public static final Vector kVisionSTDDevs = VecBuilder.fill(0.9, 0.9, 0.9); + + // Field size in meters + public static final double kFieldWidth = 8.21055; + public static final double kFieldLength = 16.54175; + } +} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 19b3544..dd9e541 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,33 +4,96 @@ package frc.robot; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.XboxController.Button; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import frc.robot.Constants.DriveConstants; +import frc.robot.Constants.IOConstants; +import frc.robot.subsystems.DriveSubsystem; +import frc.robot.subsystems.ShooterSubsystem; +/* + * This class is where the bulk of the robot should be declared. Since Command-based is a + * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} + * periodic methods (other than the scheduler calls). Instead, the structure of the robot + * (including subsystems, commands, and button mappings) should be declared here. + */ public class RobotContainer { - private final XboxController m_controller = new XboxController(0); - private final climberSubSystem climberSubSystem = new climberSubSystem(); + // The robot's subsystems and commands are defined here + private final DriveSubsystem m_robotDrive = new DriveSubsystem(); + public final ShooterSubsystem m_shooterSubsystem = new ShooterSubsystem(); + private final ClimberSubsystem m_climberSubsystem = new ClimberSubsystem(); + + private final XboxController m_driverController = new XboxController(IOConstants.kDriverControllerPort); + private final XboxController m_operatorController = new XboxController(IOConstants.kDriverControllerPort); + + /** + * The container for the robot. Contains subsystems, OI devices, and commands. + */ public RobotContainer() { + // Configure the trigger bindings configureBindings(); - - - + 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)); } + /** + * Use this method to define your button->command mappings. + */ private void configureBindings() { - //When Y button is pressed, the climber will toggle up or down - //based on current position, it will be reversed - new JoystickButton(m_controller, Button.kY.value) - .onTrue(new InstantCommand(climberSubSystem::toggle)); + new JoystickButton(m_driverController, Button.kStart.value) + .onTrue(new InstantCommand(m_robotDrive::zeroHeading, m_robotDrive)); + // TODO: Move shoot commands to operator controller + 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)); + new JoystickButton(m_operatorController, Button.kA.value) + .onTrue(new InstantCommand(() -> m_climberSubsystem.toggle(), m_shooterSubsystem)); } + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/climberConst.java b/src/main/java/frc/robot/climberConst.java deleted file mode 100644 index 013c70e..0000000 --- a/src/main/java/frc/robot/climberConst.java +++ /dev/null @@ -1,11 +0,0 @@ -package frc.robot; - -public class climberConst { - public final static int climberForwardChannel = 0; - public final static int climberReverseChannel = 1; - - public final static double minPressure = 50.0; - public final static double maxPressure = 120.0; - - -} diff --git a/src/main/java/frc/robot/climberSubSystem.java b/src/main/java/frc/robot/climberSubSystem.java index c0c6831..b8e6c80 100644 --- a/src/main/java/frc/robot/climberSubSystem.java +++ b/src/main/java/frc/robot/climberSubSystem.java @@ -5,20 +5,21 @@ import edu.wpi.first.wpilibj.PneumaticsModuleType; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.Constants.ClimberConstants; import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kForward; import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kOff; import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kReverse; -public class climberSubSystem { - private final DoubleSolenoid m_doubleSolenoid = new DoubleSolenoid(PneumaticsModuleType.REVPH, climberConst.climberForwardChannel, climberConst.climberReverseChannel); +public class ClimberSubsystem { + private final DoubleSolenoid m_doubleSolenoid = new DoubleSolenoid(PneumaticsModuleType.REVPH, ClimberConstants.climberForwardChannel, ClimberConstants.climberReverseChannel); private final Compressor m_compressor = new Compressor(PneumaticsModuleType.REVPH); private boolean enableCompressor = true; - public climberSubSystem(){ + public ClimberSubsystem(){ m_doubleSolenoid.set(kOff); m_compressor.disable(); - m_compressor.enableAnalog(climberConst.minPressure, climberConst.maxPressure); + m_compressor.enableAnalog(ClimberConstants.minPressure, ClimberConstants.maxPressure); } public void periodic() { @@ -46,7 +47,7 @@ public void toggle() { public void toggleCompresor(){ enableCompressor = !enableCompressor; if (enableCompressor){ - m_compressor.enableAnalog(climberConst.minPressure, climberConst.maxPressure); + m_compressor.enableAnalog(ClimberConstants.minPressure, ClimberConstants.maxPressure); } else{ m_compressor.disable(); From b7889ea0fbcbb11ec683a12d770044d601e27268 Mon Sep 17 00:00:00 2001 From: Programming Date: Tue, 20 Feb 2024 18:36:15 -0800 Subject: [PATCH 05/13] chore: added docs and included both arms in ClimberSubsystem --- src/main/java/frc/robot/Constants.java | 6 ++- src/main/java/frc/robot/climberSubSystem.java | 46 +++++++++++++------ 2 files changed, 37 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e0325e4..93f477c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -123,8 +123,10 @@ public static class ShooterConstants { } public static class ClimberConstants { - public final static int climberForwardChannel = 0; - public final static int climberReverseChannel = 1; + public final static int leftForwardChannel = 0; + public final static int rightForwardChannel = 0; + public final static int leftReverseChannel = 1; + public final static int rightReverseChannel = 1; public final static double minPressure = 50.0; public final static double maxPressure = 120.0; diff --git a/src/main/java/frc/robot/climberSubSystem.java b/src/main/java/frc/robot/climberSubSystem.java index b8e6c80..2c15183 100644 --- a/src/main/java/frc/robot/climberSubSystem.java +++ b/src/main/java/frc/robot/climberSubSystem.java @@ -6,44 +6,64 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Constants.ClimberConstants; -import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kForward; import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kOff; -import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kReverse; public class ClimberSubsystem { - private final DoubleSolenoid m_doubleSolenoid = new DoubleSolenoid(PneumaticsModuleType.REVPH, ClimberConstants.climberForwardChannel, ClimberConstants.climberReverseChannel); + // All + private final DoubleSolenoid m_leftSolenoid = new DoubleSolenoid(PneumaticsModuleType.REVPH, ClimberConstants.leftForwardChannel, ClimberConstants.leftReverseChannel); + private final DoubleSolenoid m_rightSolenoid = new DoubleSolenoid(PneumaticsModuleType.REVPH, ClimberConstants.rightForwardChannel, ClimberConstants.rightReverseChannel); private final Compressor m_compressor = new Compressor(PneumaticsModuleType.REVPH); private boolean enableCompressor = true; - + // public ClimberSubsystem(){ - m_doubleSolenoid.set(kOff); + m_leftSolenoid.set(kOff); + m_rightSolenoid.set(kOff); m_compressor.disable(); m_compressor.enableAnalog(ClimberConstants.minPressure, ClimberConstants.maxPressure); } + //Runs once every tick (~20ms) public void periodic() { - SmartDashboard.putNumber("Pressure", m_compressor.getPressure()); - SmartDashboard.putBoolean("Compressor Enabled", m_compressor.isEnabled()); - SmartDashboard.putBoolean("Startup Compressor", enableCompressor); + // SmartDashboard.putNumber("Pressure", m_compressor.getPressure()); + // SmartDashboard.putBoolean("Compressor Enabled", m_compressor.isEnabled()); + // SmartDashboard.putBoolean("Startup Compressor", enableCompressor); } - + /* + * Sets the state of the solenoid to off + */ public void off(){ - m_doubleSolenoid.set(kOff); + m_leftSolenoid.set(kOff); + m_rightSolenoid.set(kOff); } + //TODO: fix doc + /* + * Extends both arms + */ public void forward() { - m_doubleSolenoid.set(kForward); + m_leftSolenoid.set(kOff); + m_rightSolenoid.set(kOff); } + //TODO:fix doc + /* + * Retracts the arm + */ public void reverse() { - m_doubleSolenoid.set(kReverse); + m_leftSolenoid.set(kOff); + m_rightSolenoid.set(kOff); } + /* + * Toggles the state of the climber + */ public void toggle() { - m_doubleSolenoid.toggle(); + m_leftSolenoid.set(kOff); + m_rightSolenoid.set(kOff); } + //Toggles the state of the compressor (on/off) public void toggleCompresor(){ enableCompressor = !enableCompressor; if (enableCompressor){ From 0230de8268ce65d461e2f0ec889fa70aece55cf5 Mon Sep 17 00:00:00 2001 From: Programming Date: Tue, 20 Feb 2024 18:43:20 -0800 Subject: [PATCH 06/13] fix: seperated buttons --- src/main/java/frc/robot/RobotContainer.java | 4 +++- src/main/java/frc/robot/climberSubSystem.java | 5 +++-- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index dd9e541..84151d9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -85,7 +85,9 @@ private void configureBindings() { .onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(-0.75), m_shooterSubsystem)) .onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem)); new JoystickButton(m_operatorController, Button.kA.value) - .onTrue(new InstantCommand(() -> m_climberSubsystem.toggle(), m_shooterSubsystem)); + .onTrue(new InstantCommand(() -> m_climberSubsystem.forward(), m_shooterSubsystem)); + new JoystickButton(m_operatorController, Button.kB.value) + .onTrue(new InstantCommand(() -> m_climberSubsystem.reverse(), m_shooterSubsystem)); } /** diff --git a/src/main/java/frc/robot/climberSubSystem.java b/src/main/java/frc/robot/climberSubSystem.java index 2c15183..39cfada 100644 --- a/src/main/java/frc/robot/climberSubSystem.java +++ b/src/main/java/frc/robot/climberSubSystem.java @@ -9,13 +9,14 @@ import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kOff; public class ClimberSubsystem { - // All + private final DoubleSolenoid m_leftSolenoid = new DoubleSolenoid(PneumaticsModuleType.REVPH, ClimberConstants.leftForwardChannel, ClimberConstants.leftReverseChannel); private final DoubleSolenoid m_rightSolenoid = new DoubleSolenoid(PneumaticsModuleType.REVPH, ClimberConstants.rightForwardChannel, ClimberConstants.rightReverseChannel); + private final Compressor m_compressor = new Compressor(PneumaticsModuleType.REVPH); + private boolean enableCompressor = true; - // public ClimberSubsystem(){ m_leftSolenoid.set(kOff); m_rightSolenoid.set(kOff); From fba61e6f23181e932f9d87df3943b4aff1ab6b9e Mon Sep 17 00:00:00 2001 From: Programming Date: Tue, 20 Feb 2024 18:46:29 -0800 Subject: [PATCH 07/13] chore: moved ClimberSubsystem to subsystems --- src/main/java/frc/robot/RobotContainer.java | 1 + .../{climberSubSystem.java => subsystems/ClimberSubsystem.java} | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) rename src/main/java/frc/robot/{climberSubSystem.java => subsystems/ClimberSubsystem.java} (98%) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 84151d9..ce342f7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,6 +14,7 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc.robot.Constants.DriveConstants; import frc.robot.Constants.IOConstants; +import frc.robot.subsystems.ClimberSubsystem; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.ShooterSubsystem; diff --git a/src/main/java/frc/robot/climberSubSystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java similarity index 98% rename from src/main/java/frc/robot/climberSubSystem.java rename to src/main/java/frc/robot/subsystems/ClimberSubsystem.java index 39cfada..cbff2b3 100644 --- a/src/main/java/frc/robot/climberSubSystem.java +++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java @@ -1,4 +1,4 @@ -package frc.robot; +package frc.robot.subsystems; import edu.wpi.first.wpilibj.Compressor; import edu.wpi.first.wpilibj.DoubleSolenoid; From c093305a755f23953d6b177e77eb6cf20039915d Mon Sep 17 00:00:00 2001 From: Programming Date: Thu, 22 Feb 2024 16:18:42 -0800 Subject: [PATCH 08/13] feat: Fixed logic for setting and toggeling solenoids --- .../frc/robot/subsystems/ClimberSubsystem.java | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java index cbff2b3..b5fe238 100644 --- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java @@ -6,7 +6,10 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Constants.ClimberConstants; + +import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kForward; import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kOff; +import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kReverse; public class ClimberSubsystem { @@ -43,8 +46,8 @@ public void off(){ * Extends both arms */ public void forward() { - m_leftSolenoid.set(kOff); - m_rightSolenoid.set(kOff); + m_leftSolenoid.set(kForward); + m_rightSolenoid.set(kForward); } //TODO:fix doc @@ -52,16 +55,16 @@ public void forward() { * Retracts the arm */ public void reverse() { - m_leftSolenoid.set(kOff); - m_rightSolenoid.set(kOff); + m_leftSolenoid.set(kReverse); + m_rightSolenoid.set(kReverse); } /* * Toggles the state of the climber */ public void toggle() { - m_leftSolenoid.set(kOff); - m_rightSolenoid.set(kOff); + m_leftSolenoid.toggle();; + m_rightSolenoid.toggle();; } //Toggles the state of the compressor (on/off) From 767bfa538485507c80f90f6de77dd4332c56f9a2 Mon Sep 17 00:00:00 2001 From: ProgrammingSR Date: Fri, 23 Feb 2024 18:05:13 -0800 Subject: [PATCH 09/13] Deleted intake subsystem --- .../frc/robot/subsystems/IntakeSubsystem.java | 62 ------------------- 1 file changed, 62 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/IntakeSubsystem.java diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java deleted file mode 100644 index 9d55b9c..0000000 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ /dev/null @@ -1,62 +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.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.IntakeConstants; - - -public class IntakeSubsystem extends SubsystemBase { - CANSparkFlex m_intakeMotor = new CANSparkFlex(IntakeConstants.kIntakeMotorID, MotorType.kBrushless); - CANSparkFlex m_armMotor = new CANSparkFlex(IntakeConstants.kArmMotorID, MotorType.kBrushless); - - PIDController m_intakeVeloPID = new PIDController(IntakeConstants.kIntakeP, IntakeConstants.kIntakeI, - IntakeConstants.kIntakeD); - PIDController m_armPID = new PIDController(IntakeConstants.kArmP, IntakeConstants.kArmI, IntakeConstants.kArmD); - - DutyCycleEncoder m_armEncoder = new DutyCycleEncoder(IntakeConstants.kArmEncoderCh); - - /** Creates a new intake. */ - public IntakeSubsystem() { - - } - - // Starts intaking the disk - public void intakeDisk() { - m_intakeMotor.set(IntakeConstants.kIntakeSpeed); - } - - //Stops rotating the intake - public void stopIntaking() { - m_intakeMotor.set(0); - } - - /** - * Rotates the arm to a given angle - * @param angle motor to apply to intake - * - */ - public void tiltToAngle(double angle) { - m_armMotor.set(m_armPID.calculate(m_armEncoder.getAbsolutePosition(), angle)); - } - - //Stops rotating the arm - public void stopRotating(){ - m_armMotor.set(0); - } - - @Override - public void periodic() { - SmartDashboard.putBoolean("Intake",m_intakeMotor.get()>0); - - // This method will be called once per scheduler run - } -} From c71b0f80694f69d8cf9d049fe8d08dade0bf15e2 Mon Sep 17 00:00:00 2001 From: ArnoUUU Date: Fri, 23 Feb 2024 18:42:18 -0800 Subject: [PATCH 10/13] feat: implemented climber button bindings and did some cleanup --- src/main/java/frc/robot/RobotContainer.java | 10 ++-- .../robot/subsystems/ClimberSubsystem.java | 52 +++++++++---------- 2 files changed, 32 insertions(+), 30 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d79f912..5c4fb20 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -40,8 +40,7 @@ public class RobotContainer { // The robot's subsystems and commands are defined here private final DriveSubsystem m_robotDrive = new DriveSubsystem(); private final ShooterSubsystem m_shooterSubsystem = new ShooterSubsystem(); - public final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem(); - + private final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem(); private final ClimberSubsystem m_climberSubsystem = new ClimberSubsystem(); private final XboxController m_driverController = new XboxController(IOConstants.kDriverControllerPort); @@ -103,7 +102,7 @@ public RobotContainer() { */ private void configureBindings() { new JoystickButton(m_driverController, Button.kStart.value) - .onTrue(new InstantCommand(m_robotDrive::zeroHeading, m_robotDrive)); + .onTrue(new InstantCommand(() -> m_robotDrive.zeroHeading(), m_robotDrive)); // new JoystickButton(m_driverController, Button.kA.value).whileTrue( // AutoBuilder.pathfindToPose(new Pose2d(2.8, 5.5, new Rotation2d()), new PathConstraints( @@ -116,6 +115,11 @@ private void configureBindings() { .onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(-0.75), m_shooterSubsystem)) .onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem)); + new JoystickButton(m_operatorController, Button.kA.value) + .onTrue(new InstantCommand(() -> m_climberSubsystem.forward(), m_robotDrive)); + new JoystickButton(m_operatorController, Button.kB.value) + .onTrue(new InstantCommand(() -> m_climberSubsystem.reverse(), m_robotDrive)); + new Trigger(() -> { return m_driverController.getRightTriggerAxis() > 0.5; }).whileTrue(new IntakeCommand(m_intakeSubsystem)); diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java index b5fe238..e8818e3 100644 --- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java @@ -3,7 +3,6 @@ import edu.wpi.first.wpilibj.Compressor; import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.PneumaticsModuleType; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Constants.ClimberConstants; @@ -13,36 +12,36 @@ public class ClimberSubsystem { - private final DoubleSolenoid m_leftSolenoid = new DoubleSolenoid(PneumaticsModuleType.REVPH, ClimberConstants.leftForwardChannel, ClimberConstants.leftReverseChannel); - private final DoubleSolenoid m_rightSolenoid = new DoubleSolenoid(PneumaticsModuleType.REVPH, ClimberConstants.rightForwardChannel, ClimberConstants.rightReverseChannel); - + private final DoubleSolenoid m_leftSolenoid = new DoubleSolenoid(PneumaticsModuleType.REVPH, + ClimberConstants.leftForwardChannel, ClimberConstants.leftReverseChannel); + private final DoubleSolenoid m_rightSolenoid = new DoubleSolenoid(PneumaticsModuleType.REVPH, + ClimberConstants.rightForwardChannel, ClimberConstants.rightReverseChannel); + private final Compressor m_compressor = new Compressor(PneumaticsModuleType.REVPH); - + private boolean enableCompressor = true; - - public ClimberSubsystem(){ + + public ClimberSubsystem() { m_leftSolenoid.set(kOff); m_rightSolenoid.set(kOff); m_compressor.disable(); m_compressor.enableAnalog(ClimberConstants.minPressure, ClimberConstants.maxPressure); } - - //Runs once every tick (~20ms) + + // Runs once every tick (~20ms) public void periodic() { - // SmartDashboard.putNumber("Pressure", m_compressor.getPressure()); - // SmartDashboard.putBoolean("Compressor Enabled", m_compressor.isEnabled()); - // SmartDashboard.putBoolean("Startup Compressor", enableCompressor); } - /* - * Sets the state of the solenoid to off + + /** + * Sets the state of the solenoid to off */ - public void off(){ + public void off() { m_leftSolenoid.set(kOff); m_rightSolenoid.set(kOff); } - //TODO: fix doc - /* + // TODO: fix doc after testing + /** * Extends both arms */ public void forward() { @@ -50,9 +49,9 @@ public void forward() { m_rightSolenoid.set(kForward); } - //TODO:fix doc - /* - * Retracts the arm + // TODO:fix doc + /** + * Retracts both arms */ public void reverse() { m_leftSolenoid.set(kReverse); @@ -63,17 +62,16 @@ public void reverse() { */ public void toggle() { - m_leftSolenoid.toggle();; - m_rightSolenoid.toggle();; + m_leftSolenoid.toggle(); + m_rightSolenoid.toggle(); } - //Toggles the state of the compressor (on/off) - public void toggleCompresor(){ + // Toggles the state of the compressor (on/off) + public void toggleCompresor() { enableCompressor = !enableCompressor; - if (enableCompressor){ + if (enableCompressor) { m_compressor.enableAnalog(ClimberConstants.minPressure, ClimberConstants.maxPressure); - } - else{ + } else { m_compressor.disable(); } } From 07dd8c676af47286c8d80b184481bc3e6bedb214 Mon Sep 17 00:00:00 2001 From: ArnoUUU Date: Fri, 23 Feb 2024 18:42:51 -0800 Subject: [PATCH 11/13] chore: removed unused imports --- src/main/java/frc/robot/RobotContainer.java | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5c4fb20..4044dd2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,15 +5,12 @@ package frc.robot; import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.path.PathConstraints; import com.pathplanner.lib.path.PathPlannerPath; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.util.PIDConstants; import com.pathplanner.lib.util.ReplanningConfig; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.XboxController.Button; From b21cbb471aa3085de7192bbfdfe1dc2babbcaecf Mon Sep 17 00:00:00 2001 From: ProgrammingSR Date: Fri, 23 Feb 2024 18:50:25 -0800 Subject: [PATCH 12/13] resolve conflict --- src/main/java/frc/robot/subsystems/ClimberSubsystem.java | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java index b5fe238..35eb245 100644 --- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java @@ -29,9 +29,6 @@ public ClimberSubsystem(){ //Runs once every tick (~20ms) public void periodic() { - // SmartDashboard.putNumber("Pressure", m_compressor.getPressure()); - // SmartDashboard.putBoolean("Compressor Enabled", m_compressor.isEnabled()); - // SmartDashboard.putBoolean("Startup Compressor", enableCompressor); } /* * Sets the state of the solenoid to off From 685fd88ee58cbe9b37b6381a732794ecc6267fbb Mon Sep 17 00:00:00 2001 From: ProgrammingSR Date: Fri, 23 Feb 2024 18:51:20 -0800 Subject: [PATCH 13/13] resolve conflicts --- src/main/java/frc/robot/subsystems/ClimberSubsystem.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java index e8818e3..dbb7830 100644 --- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java @@ -40,7 +40,6 @@ public void off() { m_rightSolenoid.set(kOff); } - // TODO: fix doc after testing /** * Extends both arms */ @@ -49,7 +48,6 @@ public void forward() { m_rightSolenoid.set(kForward); } - // TODO:fix doc /** * Retracts both arms */