From 3599b00810e8ae236cd7d58ff5efd0a7cbfbcb21 Mon Sep 17 00:00:00 2001 From: Anay Nagar Date: Tue, 19 Mar 2024 23:44:37 -0700 Subject: [PATCH 01/15] Increased rotation speed --- src/main/java/frc/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5746247..2f124c3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -156,7 +156,7 @@ public RobotContainer() { * (1 - m_driverController .getRightTriggerAxis() * IOConstants.kSlowModeScalar) - / 2, + * 0.75, !m_driverController.getLeftBumper()), m_robotDrive)); } From 7711cac6367572654647d044aca7d9b079afb6e5 Mon Sep 17 00:00:00 2001 From: Anay Nagar Date: Wed, 20 Mar 2024 00:12:20 -0700 Subject: [PATCH 02/15] Scale joysticks for full speed drivign on diagonals --- src/main/java/frc/robot/RobotContainer.java | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2f124c3..7422d9a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -134,7 +134,7 @@ public RobotContainer() { new RunCommand( () -> m_robotDrive.drive( MathUtil.applyDeadband( - -m_driverController.getLeftY(), + scaleJoysticks(-m_driverController.getLeftY(), -m_driverController.getLeftX()), IOConstants.kControllerDeadband) * DriveConstants.kMaxSpeedMetersPerSecond * (1 - m_driverController @@ -142,7 +142,7 @@ public RobotContainer() { * IOConstants.kSlowModeScalar), // * 0.8, MathUtil.applyDeadband( - -m_driverController.getLeftX(), + scaleJoysticks(-m_driverController.getLeftX(), -m_driverController.getLeftY()), IOConstants.kControllerDeadband) * DriveConstants.kMaxSpeedMetersPerSecond * (1 - m_driverController @@ -161,6 +161,17 @@ public RobotContainer() { m_robotDrive)); } + /** Scales joystick values so that diagonal driving is faster + * + * @see https://www.desmos.com/calculator/uycqqtkumk + * + * @param a The primary joystick axis value being scaled + * @param b The other joystick axis value being scaled + */ + private double scaleJoysticks(double a, double b) { + return a * Math.min(1/Math.abs(a),1/Math.abs(b))*Math.sqrt(a*a+b*b); + } + /** * Use this method to define your button->command mappings. */ From 0844af822ed7c459c992e764c2d7241cdbe2089a Mon Sep 17 00:00:00 2001 From: ProgrammingSR Date: Thu, 21 Mar 2024 16:16:54 -0700 Subject: [PATCH 03/15] Tested at stemnasium --- .../pathplanner/autos/2-SourceSide-Leave.auto | 18 +++++++++++++++ .../pathplanner/autos/3-Center-Stays.auto | 6 +++++ .../pathplanner/autos/4-Center-Stays.auto | 6 ----- .../pathplanner/paths/LeftNote-Center.path | 12 +++++----- .../paths/Middle5thNote-Right.path | 22 +++++++++---------- .../pathplanner/paths/RightNote-Center.path | 10 ++++----- src/main/java/frc/robot/Constants.java | 2 +- .../frc/robot/subsystems/IntakeSubsystem.java | 17 +++++++++----- 8 files changed, 59 insertions(+), 34 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/2-SourceSide-Leave.auto b/src/main/deploy/pathplanner/autos/2-SourceSide-Leave.auto index 9c80858..8f294b1 100644 --- a/src/main/deploy/pathplanner/autos/2-SourceSide-Leave.auto +++ b/src/main/deploy/pathplanner/autos/2-SourceSide-Leave.auto @@ -32,10 +32,28 @@ "data": { "name": "Intake" } + }, + { + "type": "named", + "data": { + "name": "Reset Distance Sensor" + } } ] } }, + { + "type": "named", + "data": { + "name": "Reset Distance Sensor" + } + }, + { + "type": "named", + "data": { + "name": "Intake in" + } + }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/autos/3-Center-Stays.auto b/src/main/deploy/pathplanner/autos/3-Center-Stays.auto index 3c0c573..c2fcc87 100644 --- a/src/main/deploy/pathplanner/autos/3-Center-Stays.auto +++ b/src/main/deploy/pathplanner/autos/3-Center-Stays.auto @@ -17,6 +17,12 @@ "name": "Shoot" } }, + { + "type": "named", + "data": { + "name": "Intake out" + } + }, { "type": "deadline", "data": { diff --git a/src/main/deploy/pathplanner/autos/4-Center-Stays.auto b/src/main/deploy/pathplanner/autos/4-Center-Stays.auto index 56651e6..602b83d 100644 --- a/src/main/deploy/pathplanner/autos/4-Center-Stays.auto +++ b/src/main/deploy/pathplanner/autos/4-Center-Stays.auto @@ -23,12 +23,6 @@ "name": "Intake out" } }, - { - "type": "deadline", - "data": { - "commands": [] - } - }, { "type": "deadline", "data": { diff --git a/src/main/deploy/pathplanner/paths/LeftNote-Center.path b/src/main/deploy/pathplanner/paths/LeftNote-Center.path index 51473cb..1be90db 100644 --- a/src/main/deploy/pathplanner/paths/LeftNote-Center.path +++ b/src/main/deploy/pathplanner/paths/LeftNote-Center.path @@ -16,15 +16,15 @@ }, { "anchor": { - "x": 2.75, + "x": 2.9, "y": 6.5 }, "prevControl": { - "x": 2.760368651393246, - "y": 5.421660255102424 + "x": 2.909614940148025, + "y": 5.5000462246053825 }, "nextControl": { - "x": 2.740133057189191, + "x": 2.890133057189191, "y": 7.526162052324105 }, "isLocked": false, @@ -32,11 +32,11 @@ }, { "anchor": { - "x": 1.35, + "x": 1.4, "y": 5.52 }, "prevControl": { - "x": 3.249810371265829, + "x": 3.2998103712658287, "y": 5.52 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/Middle5thNote-Right.path b/src/main/deploy/pathplanner/paths/Middle5thNote-Right.path index 6bcd6d6..a389143 100644 --- a/src/main/deploy/pathplanner/paths/Middle5thNote-Right.path +++ b/src/main/deploy/pathplanner/paths/Middle5thNote-Right.path @@ -3,28 +3,28 @@ "waypoints": [ { "anchor": { - "x": 0.75, - "y": 4.35 + "x": 0.8, + "y": 4.4 }, "prevControl": null, "nextControl": { - "x": 1.2709445330007911, - "y": 1.3955767409633761 + "x": 1.3209445330007905, + "y": 1.4455767409633768 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 8.043316855864482, + "x": 8.05, "y": 0.7104198823782273 }, "prevControl": { - "x": 7.043316855864482, + "x": 7.050000000000001, "y": 0.7104198823782273 }, "nextControl": { - "x": 9.04331685586447, + "x": 9.049999999999988, "y": 0.7104198823782273 }, "isLocked": false, @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 0.75, - "y": 4.35 + "x": 0.8, + "y": 4.4 }, "prevControl": { - "x": 1.2709445330007911, - "y": 1.3955767409633757 + "x": 1.320944533000791, + "y": 1.4455767409633768 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/RightNote-Center.path b/src/main/deploy/pathplanner/paths/RightNote-Center.path index 29dd5e3..6a86c3f 100644 --- a/src/main/deploy/pathplanner/paths/RightNote-Center.path +++ b/src/main/deploy/pathplanner/paths/RightNote-Center.path @@ -16,15 +16,15 @@ }, { "anchor": { - "x": 2.0, + "x": 2.15, "y": 4.15 }, "prevControl": { - "x": 1.0004588208546235, + "x": 1.1504588208546234, "y": 4.180289126640771 }, "nextControl": { - "x": 2.325609112756687, + "x": 2.475609112756687, "y": 4.140133057189191 }, "isLocked": false, @@ -32,11 +32,11 @@ }, { "anchor": { - "x": 1.35, + "x": 1.5, "y": 5.52 }, "prevControl": { - "x": 2.333480197729623, + "x": 2.483480197729623, "y": 5.278259901135186 }, "nextControl": null, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7a92f68..bec509c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -111,7 +111,7 @@ public static final class IntakeConstants { public static final double kIntakeSpeed = 0.5; // TODO: Tune distance sensor threshold for detecting note - public static final double kDistanceSensorThreshold = 10; + public static final double kDistanceSensorThreshold = 3; } public static final class ShooterConstants { diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 1049fcf..cbce3fe 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -100,8 +100,15 @@ public void stopIntake() { * Gets distance from Rev 2m sensor */ private double getDistanceSensor() { + + if (!Robot.isReal()) { + return -2; + } + if (m_distanceSensor.getRange() == -1) { m_distanceSensorToggle = false; + } else { + m_distanceSensorToggle = true; } return m_distanceSensor.getRange(); } @@ -117,20 +124,20 @@ public void toggleDistanceSensor() { @Override public void periodic() { - haveNote = getDistanceSensorToggle() ? getDistanceSensor() < IntakeConstants.kDistanceSensorThreshold : false; + haveNote = m_distanceSensorToggle ? getDistanceSensor() < IntakeConstants.kDistanceSensorThreshold : false; // Note: negative because encoder goes from 0 to -193 cuz weird double armMotorSpeed = MathUtil.clamp(m_armPID.calculate(m_armEncoder.getDistance(), m_armSetpoint), -0.3, 0.3); m_armMotor.set(armMotorSpeed); m_intakeMotor.set(m_intakeSpeed); - SmartDashboard.putNumber("intakespeed", m_intakeSpeed); SmartDashboard.putNumber("Arm Angle", m_armEncoder.getDistance()); SmartDashboard.putNumber("Arm Absolute Angle", m_armEncoder.getAbsolutePosition()); SmartDashboard.putBoolean("Have Note?", haveNote); - SmartDashboard.putNumber("distance sensor", m_distanceSensorToggle ? m_distanceSensor.getRange(Rev2mDistanceSensor.Unit.kInches) : -1); - SmartDashboard.putNumber("pid output", armMotorSpeed); - SmartDashboard.putNumber("Get offset", m_armEncoder.getPositionOffset()); + SmartDashboard.putNumber("distance sensor", m_distanceSensorToggle ? m_distanceSensor.getRange() : -1); + SmartDashboard.putBoolean("distance sensor toggle", m_distanceSensorToggle); + // SmartDashboard.putNumber("pid output", armMotorSpeed); + // SmartDashboard.putNumber("Get offset", m_armEncoder.getPositionOffset()); } public boolean haveNote() { From 974a416cc60442c08056c7d8ab3e99f29cf40363 Mon Sep 17 00:00:00 2001 From: ProgrammingSR Date: Thu, 21 Mar 2024 17:36:43 -0700 Subject: [PATCH 04/15] Auto path changes --- .../autos/1+0.5-SourceSide-Leave.auto | 75 +++++++++++++++++++ .../pathplanner/autos/1-AmpSide-Leave.auto | 8 +- .../pathplanner/autos/2-Center-Stays.auto | 17 ++--- .../pathplanner/autos/2-SourceSide-Leave.auto | 12 --- .../pathplanner/paths/5thNote-Back.path | 52 +++++++++++++ .../pathplanner/paths/Middle5thNote.path | 58 ++++++++++++++ .../pathplanner/paths/MoveOut-Left.path | 24 +++--- .../pathplanner/paths/Straight-Back.path | 12 +-- 8 files changed, 218 insertions(+), 40 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/1+0.5-SourceSide-Leave.auto create mode 100644 src/main/deploy/pathplanner/paths/5thNote-Back.path create mode 100644 src/main/deploy/pathplanner/paths/Middle5thNote.path diff --git a/src/main/deploy/pathplanner/autos/1+0.5-SourceSide-Leave.auto b/src/main/deploy/pathplanner/autos/1+0.5-SourceSide-Leave.auto new file mode 100644 index 0000000..0391f17 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/1+0.5-SourceSide-Leave.auto @@ -0,0 +1,75 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.8, + "y": 4.4 + }, + "rotation": -60.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + }, + { + "type": "path", + "data": { + "pathName": "Middle5thNote" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Intake in" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Prep-Speed - 60%" + } + }, + { + "type": "path", + "data": { + "pathName": "5thNote-Back" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Outtake" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/1-AmpSide-Leave.auto b/src/main/deploy/pathplanner/autos/1-AmpSide-Leave.auto index fa76be9..ae003cf 100644 --- a/src/main/deploy/pathplanner/autos/1-AmpSide-Leave.auto +++ b/src/main/deploy/pathplanner/autos/1-AmpSide-Leave.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.7516461186768542, - "y": 4.35 + "x": 0.75, + "y": 6.75 }, - "rotation": -60.0 + "rotation": 60.0 }, "command": { "type": "sequential", @@ -24,7 +24,7 @@ { "type": "path", "data": { - "pathName": "MoveOut-Right" + "pathName": "MoveOut-Left" } } ] diff --git a/src/main/deploy/pathplanner/autos/2-Center-Stays.auto b/src/main/deploy/pathplanner/autos/2-Center-Stays.auto index 407a129..7d73b9a 100644 --- a/src/main/deploy/pathplanner/autos/2-Center-Stays.auto +++ b/src/main/deploy/pathplanner/autos/2-Center-Stays.auto @@ -17,21 +17,20 @@ "name": "Shoot" } }, + { + "type": "named", + "data": { + "name": "Intake out" + } + }, { "type": "deadline", "data": { "commands": [ { - "type": "sequential", + "type": "path", "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "CenterNote-Center" - } - } - ] + "pathName": "CenterNote-Center" } }, { diff --git a/src/main/deploy/pathplanner/autos/2-SourceSide-Leave.auto b/src/main/deploy/pathplanner/autos/2-SourceSide-Leave.auto index 8f294b1..e92c299 100644 --- a/src/main/deploy/pathplanner/autos/2-SourceSide-Leave.auto +++ b/src/main/deploy/pathplanner/autos/2-SourceSide-Leave.auto @@ -32,22 +32,10 @@ "data": { "name": "Intake" } - }, - { - "type": "named", - "data": { - "name": "Reset Distance Sensor" - } } ] } }, - { - "type": "named", - "data": { - "name": "Reset Distance Sensor" - } - }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/paths/5thNote-Back.path b/src/main/deploy/pathplanner/paths/5thNote-Back.path new file mode 100644 index 0000000..a1ace2b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/5thNote-Back.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.0, + "y": 0.75 + }, + "prevControl": null, + "nextControl": { + "x": 7.346710693421388, + "y": 1.0261620523241057 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.0, + "y": 1.25 + }, + "prevControl": { + "x": 5.436099261932909, + "y": 0.6271857083288086 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -30.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Middle5thNote.path b/src/main/deploy/pathplanner/paths/Middle5thNote.path new file mode 100644 index 0000000..3fd72f1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Middle5thNote.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.8, + "y": 4.4 + }, + "prevControl": null, + "nextControl": { + "x": 1.3209445330007905, + "y": 1.4455767409633768 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.0, + "y": 0.75 + }, + "prevControl": { + "x": 7.0, + "y": 0.75 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.85, + "rotationDegrees": 0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -60.067304943288455, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/MoveOut-Left.path b/src/main/deploy/pathplanner/paths/MoveOut-Left.path index e0e41d1..014b5a9 100644 --- a/src/main/deploy/pathplanner/paths/MoveOut-Left.path +++ b/src/main/deploy/pathplanner/paths/MoveOut-Left.path @@ -8,24 +8,24 @@ }, "prevControl": null, "nextControl": { - "x": 1.9850139700279412, - "y": 7.637013735565933 + "x": 1.9683280647280124, + "y": 7.625029557612825 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.589886872086828, - "y": 7.114065766591227 + "x": 3.45, + "y": 7.5 }, "prevControl": { - "x": 3.029411851614082, - "y": 7.114065766591227 + "x": 1.9500000000000002, + "y": 7.5 }, "nextControl": { - "x": 6.150361892559576, - "y": 7.114065766591227 + "x": 4.950000000000003, + "y": 7.5 }, "isLocked": false, "linkedName": null @@ -44,7 +44,13 @@ "linkedName": null } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 0, + "rotateFast": false + } + ], "constraintZones": [], "eventMarkers": [], "globalConstraints": { diff --git a/src/main/deploy/pathplanner/paths/Straight-Back.path b/src/main/deploy/pathplanner/paths/Straight-Back.path index 2eddfae..3f00bbf 100644 --- a/src/main/deploy/pathplanner/paths/Straight-Back.path +++ b/src/main/deploy/pathplanner/paths/Straight-Back.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.55744644822623, - "y": 6.0 + "x": 0.75, + "y": 3.0 }, "prevControl": null, "nextControl": { - "x": 2.557446448226229, - "y": 6.0 + "x": 1.7500000000000013, + "y": 3.0 }, "isLocked": false, "linkedName": null @@ -17,11 +17,11 @@ { "anchor": { "x": 4.0, - "y": 6.0 + "y": 3.0 }, "prevControl": { "x": 3.0, - "y": 6.0 + "y": 3.0 }, "nextControl": null, "isLocked": false, From 520aba004637de7c622efac0b2798760145e8565 Mon Sep 17 00:00:00 2001 From: ProgrammingSR Date: Thu, 21 Mar 2024 17:38:24 -0700 Subject: [PATCH 05/15] chore: updated venderdep and WPILib --- build.gradle | 2 +- vendordeps/PathplannerLib.json | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/build.gradle b/build.gradle index 58026ff..506045e 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.3.1" + id "edu.wpi.first.GradleRIO" version "2024.3.2" } java { diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index b849e92..a019706 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2024.2.5", + "version": "2024.2.7", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2024", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.2.5" + "version": "2024.2.7" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.2.5", + "version": "2024.2.7", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, From ffd36ea9896133f6ef6bf65a4d073d03f0dd8e43 Mon Sep 17 00:00:00 2001 From: ProgrammingSR Date: Thu, 21 Mar 2024 18:17:36 -0700 Subject: [PATCH 06/15] fix?: distance sensor update and can simulate robot now --- src/main/java/frc/robot/Constants.java | 2 +- .../frc/robot/commands/IntakeArmPositionCommand.java | 3 ++- .../java/frc/robot/subsystems/IntakeSubsystem.java | 10 +++++++--- 3 files changed, 10 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index bec509c..7a92f68 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -111,7 +111,7 @@ public static final class IntakeConstants { public static final double kIntakeSpeed = 0.5; // TODO: Tune distance sensor threshold for detecting note - public static final double kDistanceSensorThreshold = 3; + public static final double kDistanceSensorThreshold = 10; } public static final class ShooterConstants { diff --git a/src/main/java/frc/robot/commands/IntakeArmPositionCommand.java b/src/main/java/frc/robot/commands/IntakeArmPositionCommand.java index 7349812..30ee44c 100644 --- a/src/main/java/frc/robot/commands/IntakeArmPositionCommand.java +++ b/src/main/java/frc/robot/commands/IntakeArmPositionCommand.java @@ -5,6 +5,7 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Robot; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.IntakeSubsystem.ArmPosition; @@ -29,6 +30,6 @@ public void initialize() { // Returns true when the command should end. @Override public boolean isFinished() { - return m_intakeSubsystem.armAtSetpoint(); + return m_intakeSubsystem.armAtSetpoint() || !Robot.isReal(); } } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index cbce3fe..daae545 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -30,14 +30,18 @@ public class IntakeSubsystem extends SubsystemBase { /** If true, the distance sensor will be used to determine if we have a note */ private boolean m_distanceSensorToggle = Robot.isReal(); - private Rev2mDistanceSensor m_distanceSensor = m_distanceSensorToggle ? new Rev2mDistanceSensor(Port.kMXP) : null; // NavX I2C access port + private Rev2mDistanceSensor m_distanceSensor = m_distanceSensorToggle ? new Rev2mDistanceSensor(Port.kMXP) : null; private double m_intakeSpeed = 0; private double m_armSetpoint = IntakeConstants.kIntakeRaisedAngle; /** Creates a new IntakeSubsystem */ public IntakeSubsystem() { - if (m_distanceSensorToggle) m_distanceSensor.setAutomaticMode(true); + if (m_distanceSensorToggle) { + m_distanceSensor.setAutomaticMode(true); + m_distanceSensor.setEnabled(true); + m_distanceSensor.setMeasurementPeriod(0.01); + } m_armEncoder.setPositionOffset(IntakeConstants.kArmEncoderOffset); SmartDashboard.putNumber("arm", m_armEncoder.getAbsolutePosition()); @@ -124,7 +128,7 @@ public void toggleDistanceSensor() { @Override public void periodic() { - haveNote = m_distanceSensorToggle ? getDistanceSensor() < IntakeConstants.kDistanceSensorThreshold : false; + haveNote = m_distanceSensorToggle ? (getDistanceSensor() < IntakeConstants.kDistanceSensorThreshold && getDistanceSensor() > 0) : false; // Note: negative because encoder goes from 0 to -193 cuz weird double armMotorSpeed = MathUtil.clamp(m_armPID.calculate(m_armEncoder.getDistance(), m_armSetpoint), -0.3, 0.3); From d30f660cf3d92af0eafd903a4b493883334dff73 Mon Sep 17 00:00:00 2001 From: ProgrammingSR Date: Thu, 21 Mar 2024 19:19:46 -0700 Subject: [PATCH 07/15] FEAT: replaced rev2mdistancesensor with color sensor tested but should look over --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 8 ++-- .../frc/robot/commands/NoteIntakeCommand.java | 6 +-- .../frc/robot/subsystems/IntakeSubsystem.java | 47 +++++-------------- vendordeps/REVLib.json | 10 ++-- 5 files changed, 23 insertions(+), 50 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7a92f68..b5926f4 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -111,7 +111,7 @@ public static final class IntakeConstants { public static final double kIntakeSpeed = 0.5; // TODO: Tune distance sensor threshold for detecting note - public static final double kDistanceSensorThreshold = 10; + public static final int kProximityThreshold = 140; } public static final class ShooterConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7422d9a..e57ea7a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -225,10 +225,10 @@ private void configureBindings() { return m_operatorController.getBButton() && m_operatorController.getRightBumper(); }).whileTrue(new InstantCommand(() -> m_climberSubsystem.reverse())); - // Toggle Distance Sensor, Operator Controller Left Bumper + Start Button - new Trigger(() -> { - return m_operatorController.getLeftBumper() && m_operatorController.getStartButton(); - }).onTrue(new InstantCommand(() -> m_intakeSubsystem.toggleDistanceSensor())); + // // Toggle Distance Sensor, Operator Controller Left Bumper + Start Button + // new Trigger(() -> { + // return m_operatorController.getLeftBumper() && m_operatorController.getStartButton(); + // }).onTrue(new InstantCommand(() -> m_intakeSubsystem.toggleDistanceSensor())); } /** diff --git a/src/main/java/frc/robot/commands/NoteIntakeCommand.java b/src/main/java/frc/robot/commands/NoteIntakeCommand.java index 1778cc7..f8de205 100644 --- a/src/main/java/frc/robot/commands/NoteIntakeCommand.java +++ b/src/main/java/frc/robot/commands/NoteIntakeCommand.java @@ -20,10 +20,6 @@ public NoteIntakeCommand(IntakeSubsystem subsystem) { addRequirements(m_intakeSubsystem); // If the distance sensor is not being used, then we want to use a timer to stop this command - if (!m_intakeSubsystem.getDistanceSensorToggle()) { - m_intakeTimer.reset(); - m_intakeTimer.start(); - } } // Called when the command is initially scheduled. @@ -49,6 +45,6 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return m_intakeSubsystem.getDistanceSensorToggle() ? (m_intakeSubsystem.haveNote() || m_intakeSubsystem.getArmPosition() != IntakeConstants.kIntakeLoweredAngle) : m_intakeTimer.hasElapsed(3); + return (m_intakeSubsystem.haveNote() || m_intakeSubsystem.getArmPosition() != IntakeConstants.kIntakeLoweredAngle); } } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index daae545..9ad4d7f 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -7,9 +7,9 @@ import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkFlex; import com.revrobotics.CANSparkLowLevel.MotorType; -import com.revrobotics.Rev2mDistanceSensor; -import com.revrobotics.Rev2mDistanceSensor.Port; +import com.revrobotics.ColorSensorV3; +import edu.wpi.first.wpilibj.I2C.Port; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj.DutyCycleEncoder; @@ -29,19 +29,14 @@ public class IntakeSubsystem extends SubsystemBase { private DutyCycleEncoder m_armEncoder = new DutyCycleEncoder(IntakeConstants.kArmEncoderChannel); /** If true, the distance sensor will be used to determine if we have a note */ - private boolean m_distanceSensorToggle = Robot.isReal(); - private Rev2mDistanceSensor m_distanceSensor = m_distanceSensorToggle ? new Rev2mDistanceSensor(Port.kMXP) : null; + private boolean m_colorSensorToggle = Robot.isReal(); + private ColorSensorV3 m_colorSensor = new ColorSensorV3(Port.kMXP); private double m_intakeSpeed = 0; private double m_armSetpoint = IntakeConstants.kIntakeRaisedAngle; /** Creates a new IntakeSubsystem */ public IntakeSubsystem() { - if (m_distanceSensorToggle) { - m_distanceSensor.setAutomaticMode(true); - m_distanceSensor.setEnabled(true); - m_distanceSensor.setMeasurementPeriod(0.01); - } m_armEncoder.setPositionOffset(IntakeConstants.kArmEncoderOffset); SmartDashboard.putNumber("arm", m_armEncoder.getAbsolutePosition()); @@ -60,7 +55,7 @@ public void reset() { m_armMotor.set(0); m_intakeSpeed = 0; - m_armSetpoint = getDistanceSensor(); + m_armSetpoint = getArmPosition(); } public void setArmPosition(ArmPosition position) { @@ -101,34 +96,15 @@ public void stopIntake() { } /** - * Gets distance from Rev 2m sensor + * Gets distance from Color sensor */ - private double getDistanceSensor() { - - if (!Robot.isReal()) { - return -2; - } - - if (m_distanceSensor.getRange() == -1) { - m_distanceSensorToggle = false; - } else { - m_distanceSensorToggle = true; - } - return m_distanceSensor.getRange(); - } - - public boolean getDistanceSensorToggle() { - return m_distanceSensorToggle; - } - - /** Toggles whether the distance sensor is used */ - public void toggleDistanceSensor() { - m_distanceSensorToggle = !m_distanceSensorToggle; + public int getProximity() { + return m_colorSensor.getProximity(); } @Override public void periodic() { - haveNote = m_distanceSensorToggle ? (getDistanceSensor() < IntakeConstants.kDistanceSensorThreshold && getDistanceSensor() > 0) : false; + haveNote = getProximity() > IntakeConstants.kProximityThreshold; // Note: negative because encoder goes from 0 to -193 cuz weird double armMotorSpeed = MathUtil.clamp(m_armPID.calculate(m_armEncoder.getDistance(), m_armSetpoint), -0.3, 0.3); @@ -138,10 +114,11 @@ public void periodic() { SmartDashboard.putNumber("Arm Angle", m_armEncoder.getDistance()); SmartDashboard.putNumber("Arm Absolute Angle", m_armEncoder.getAbsolutePosition()); SmartDashboard.putBoolean("Have Note?", haveNote); - SmartDashboard.putNumber("distance sensor", m_distanceSensorToggle ? m_distanceSensor.getRange() : -1); - SmartDashboard.putBoolean("distance sensor toggle", m_distanceSensorToggle); + // SmartDashboard.putNumber("distance sensor", m_distanceSensorToggle ? m_distanceSensor.getRange() : -1); + // SmartDashboard.putBoolean("distance sensor toggle", m_distanceSensorToggle); // SmartDashboard.putNumber("pid output", armMotorSpeed); // SmartDashboard.putNumber("Get offset", m_armEncoder.getPositionOffset()); + SmartDashboard.putNumber("Proximity", getProximity()); } public boolean haveNote() { diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index a829581..f85acd4 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2024.2.1", + "version": "2024.2.4", "frcYear": "2024", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2024.2.1" + "version": "2024.2.4" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.1", + "version": "2024.2.4", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -37,7 +37,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2024.2.1", + "version": "2024.2.4", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -55,7 +55,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.1", + "version": "2024.2.4", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, From 0af36ad23272989d29e6f3fc1f25e57f195fa843 Mon Sep 17 00:00:00 2001 From: WILLIAM WANG Date: Fri, 22 Mar 2024 11:44:26 -0700 Subject: [PATCH 08/15] Broken Path --- .../pathplanner/autos/0-Anywhere-Leave-StraightBack.auto | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/0-Anywhere-Leave-StraightBack.auto b/src/main/deploy/pathplanner/autos/0-Anywhere-Leave-StraightBack.auto index 398d77b..0eeca28 100644 --- a/src/main/deploy/pathplanner/autos/0-Anywhere-Leave-StraightBack.auto +++ b/src/main/deploy/pathplanner/autos/0-Anywhere-Leave-StraightBack.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.55744644822623, - "y": 6.0 + "x": 0.75, + "y": 3.0 }, "rotation": 0 }, From 7bb58efd081fad3b81139e0f6102a1e2f839c1d9 Mon Sep 17 00:00:00 2001 From: WILLIAM WANG Date: Fri, 22 Mar 2024 11:49:33 -0700 Subject: [PATCH 09/15] We want a timer if the distance sensor decides to not work --- src/main/java/frc/robot/commands/NoteIntakeCommand.java | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/commands/NoteIntakeCommand.java b/src/main/java/frc/robot/commands/NoteIntakeCommand.java index 1778cc7..9c30539 100644 --- a/src/main/java/frc/robot/commands/NoteIntakeCommand.java +++ b/src/main/java/frc/robot/commands/NoteIntakeCommand.java @@ -19,11 +19,7 @@ public NoteIntakeCommand(IntakeSubsystem subsystem) { m_intakeSubsystem = subsystem; addRequirements(m_intakeSubsystem); - // If the distance sensor is not being used, then we want to use a timer to stop this command - if (!m_intakeSubsystem.getDistanceSensorToggle()) { - m_intakeTimer.reset(); - m_intakeTimer.start(); - } + m_intakeTimer.start(); } // Called when the command is initially scheduled. From e91401dabfbd8de0568fef8158105f4f8443d053 Mon Sep 17 00:00:00 2001 From: WILLIAM WANG Date: Fri, 22 Mar 2024 12:01:50 -0700 Subject: [PATCH 10/15] Enable on teleop-compressor --- src/main/java/frc/robot/Robot.java | 2 ++ src/main/java/frc/robot/RobotContainer.java | 4 ++++ src/main/java/frc/robot/subsystems/ClimberSubsystem.java | 2 -- 3 files changed, 6 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 3dadca9..6681434 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -96,6 +96,8 @@ public void teleopInit() { } m_robotContainer.resetAllSubsystems(); + + m_robotContainer.compressorInit(); } /** This function is called periodically during operator control. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7422d9a..005bb08 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -240,6 +240,10 @@ public void resetAllSubsystems() { m_robotDrive.reset(); } + public void compressorInit(){ + m_climberSubsystem.toggleCompressor(); + } + /** * Use this to pass the autonomous command to the main {@link Robot} class. * diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java index 8da249a..7a2f791 100644 --- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java @@ -24,13 +24,11 @@ public class ClimberSubsystem extends SubsystemBase { private Value m_state; public ClimberSubsystem() { - m_compressorEnabled = false; m_pHub = new PneumaticHub(2); solenoidOff(); m_compressorEnabled = false; - toggleCompressor(); } // Runs once every tick (~20ms) From 4effaf8205e98ecb600beb5bbf7092b8c5f9fe00 Mon Sep 17 00:00:00 2001 From: WILLIAM WANG Date: Fri, 22 Mar 2024 12:13:18 -0700 Subject: [PATCH 11/15] Toggle compressor start+rightbumper --- src/main/java/frc/robot/RobotContainer.java | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 005bb08..0976a56 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -12,6 +12,7 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.XboxController.Button; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; @@ -229,6 +230,11 @@ private void configureBindings() { new Trigger(() -> { return m_operatorController.getLeftBumper() && m_operatorController.getStartButton(); }).onTrue(new InstantCommand(() -> m_intakeSubsystem.toggleDistanceSensor())); + + // Toggle Compressor, Operator Controller Right Bumper + Menu + new Trigger(() -> { + return m_operatorController.getRightBumper() && m_operatorController.getStartButton(); + }).onTrue(new InstantCommand(() -> m_climberSubsystem.toggleCompressor())); } /** From 8dcdd3f23adab314a6c3e2996d36ba7583017edc Mon Sep 17 00:00:00 2001 From: SR1899 Date: Fri, 22 Mar 2024 18:06:30 -0700 Subject: [PATCH 12/15] yes --- src/main/java/frc/robot/commands/NoteIntakeCommand.java | 3 ++- src/main/java/frc/robot/subsystems/IntakeSubsystem.java | 3 +-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/commands/NoteIntakeCommand.java b/src/main/java/frc/robot/commands/NoteIntakeCommand.java index c5e2a51..f1f0fa3 100644 --- a/src/main/java/frc/robot/commands/NoteIntakeCommand.java +++ b/src/main/java/frc/robot/commands/NoteIntakeCommand.java @@ -20,6 +20,7 @@ public NoteIntakeCommand(IntakeSubsystem subsystem) { addRequirements(m_intakeSubsystem); // If the distance sensor is not being used, then we want to use a timer to stop this command + m_intakeTimer.reset(); m_intakeTimer.start(); } @@ -46,6 +47,6 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return (m_intakeSubsystem.haveNote() || m_intakeSubsystem.getArmPosition() != IntakeConstants.kIntakeLoweredAngle); + return m_intakeSubsystem.m_colorSensorToggle ? (m_intakeSubsystem.haveNote() || m_intakeSubsystem.getArmPosition() != IntakeConstants.kIntakeLoweredAngle): m_intakeTimer.get()>3; } } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 9ad4d7f..7965c50 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -29,7 +29,7 @@ public class IntakeSubsystem extends SubsystemBase { private DutyCycleEncoder m_armEncoder = new DutyCycleEncoder(IntakeConstants.kArmEncoderChannel); /** If true, the distance sensor will be used to determine if we have a note */ - private boolean m_colorSensorToggle = Robot.isReal(); + public boolean m_colorSensorToggle = Robot.isReal(); private ColorSensorV3 m_colorSensor = new ColorSensorV3(Port.kMXP); private double m_intakeSpeed = 0; @@ -37,7 +37,6 @@ public class IntakeSubsystem extends SubsystemBase { /** Creates a new IntakeSubsystem */ public IntakeSubsystem() { - m_armEncoder.setPositionOffset(IntakeConstants.kArmEncoderOffset); SmartDashboard.putNumber("arm", m_armEncoder.getAbsolutePosition()); m_armEncoder.setDistancePerRotation(360); From c3c2c86a7d930610104098fdc81593e8b5448d88 Mon Sep 17 00:00:00 2001 From: SR1899 Date: Fri, 22 Mar 2024 18:16:07 -0700 Subject: [PATCH 13/15] chore: added toggle functionality for the color sensor --- src/main/java/frc/robot/RobotContainer.java | 9 +++++---- .../java/frc/robot/subsystems/IntakeSubsystem.java | 11 ++++++++++- 2 files changed, 15 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0976a56..3bdb256 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -162,7 +162,8 @@ public RobotContainer() { m_robotDrive)); } - /** Scales joystick values so that diagonal driving is faster + /** + * Scales joystick values so that diagonal driving is faster * * @see https://www.desmos.com/calculator/uycqqtkumk * @@ -170,7 +171,7 @@ public RobotContainer() { * @param b The other joystick axis value being scaled */ private double scaleJoysticks(double a, double b) { - return a * Math.min(1/Math.abs(a),1/Math.abs(b))*Math.sqrt(a*a+b*b); + return a * Math.min(1 / Math.abs(a), 1 / Math.abs(b)) * Math.sqrt(a * a + b * b); } /** @@ -229,7 +230,7 @@ private void configureBindings() { // Toggle Distance Sensor, Operator Controller Left Bumper + Start Button new Trigger(() -> { return m_operatorController.getLeftBumper() && m_operatorController.getStartButton(); - }).onTrue(new InstantCommand(() -> m_intakeSubsystem.toggleDistanceSensor())); + }).onTrue(new InstantCommand(() -> m_intakeSubsystem.colorSensorToggle())); // Toggle Compressor, Operator Controller Right Bumper + Menu new Trigger(() -> { @@ -246,7 +247,7 @@ public void resetAllSubsystems() { m_robotDrive.reset(); } - public void compressorInit(){ + public void compressorInit() { m_climberSubsystem.toggleCompressor(); } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 7965c50..408affc 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -113,7 +113,8 @@ public void periodic() { SmartDashboard.putNumber("Arm Angle", m_armEncoder.getDistance()); SmartDashboard.putNumber("Arm Absolute Angle", m_armEncoder.getAbsolutePosition()); SmartDashboard.putBoolean("Have Note?", haveNote); - // SmartDashboard.putNumber("distance sensor", m_distanceSensorToggle ? m_distanceSensor.getRange() : -1); + // SmartDashboard.putNumber("distance sensor", m_distanceSensorToggle ? + // m_distanceSensor.getRange() : -1); // SmartDashboard.putBoolean("distance sensor toggle", m_distanceSensorToggle); // SmartDashboard.putNumber("pid output", armMotorSpeed); // SmartDashboard.putNumber("Get offset", m_armEncoder.getPositionOffset()); @@ -129,4 +130,12 @@ public static enum ArmPosition { Retracted, Amp } + + /** + * Toggles the uage of color sensor + */ + + public void colorSensorToggle() { + m_colorSensorToggle = !m_colorSensorToggle; + } } From 9d7c279e237ddf6714e35ce3b739ca0a14276fe2 Mon Sep 17 00:00:00 2001 From: ProgrammingSR Date: Fri, 22 Mar 2024 19:07:53 -0700 Subject: [PATCH 14/15] fix: use IR value from color sensor instead --- src/main/java/frc/robot/Constants.java | 4 ++-- .../java/frc/robot/subsystems/IntakeSubsystem.java | 14 +++++--------- 2 files changed, 7 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b5926f4..8dfc7e7 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -110,8 +110,8 @@ public static final class IntakeConstants { public static final double kIntakeSpeed = 0.5; - // TODO: Tune distance sensor threshold for detecting note - public static final int kProximityThreshold = 140; + // public static final int kProximityThreshold = 130; + public static final int kIRThreshold = 2; } public static final class ShooterConstants { diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 408affc..237adf2 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -97,15 +97,14 @@ public void stopIntake() { /** * Gets distance from Color sensor */ - public int getProximity() { - return m_colorSensor.getProximity(); + public int getColorIR() { + return m_colorSensor.getIR(); } @Override public void periodic() { - haveNote = getProximity() > IntakeConstants.kProximityThreshold; + haveNote = getColorIR() > IntakeConstants.kIRThreshold; - // Note: negative because encoder goes from 0 to -193 cuz weird double armMotorSpeed = MathUtil.clamp(m_armPID.calculate(m_armEncoder.getDistance(), m_armSetpoint), -0.3, 0.3); m_armMotor.set(armMotorSpeed); m_intakeMotor.set(m_intakeSpeed); @@ -113,12 +112,9 @@ public void periodic() { SmartDashboard.putNumber("Arm Angle", m_armEncoder.getDistance()); SmartDashboard.putNumber("Arm Absolute Angle", m_armEncoder.getAbsolutePosition()); SmartDashboard.putBoolean("Have Note?", haveNote); - // SmartDashboard.putNumber("distance sensor", m_distanceSensorToggle ? - // m_distanceSensor.getRange() : -1); - // SmartDashboard.putBoolean("distance sensor toggle", m_distanceSensorToggle); // SmartDashboard.putNumber("pid output", armMotorSpeed); - // SmartDashboard.putNumber("Get offset", m_armEncoder.getPositionOffset()); - SmartDashboard.putNumber("Proximity", getProximity()); + SmartDashboard.putNumber("Proximity", getColorIR()); + SmartDashboard.putNumber("IR", m_colorSensor.getIR()); } public boolean haveNote() { From 590100af5969ab6e3546691415b0a3bcf28f68a9 Mon Sep 17 00:00:00 2001 From: ProgrammingSR Date: Fri, 22 Mar 2024 19:41:32 -0700 Subject: [PATCH 15/15] Final changes --- src/main/java/frc/robot/Constants.java | 4 ++-- src/main/java/frc/robot/RobotContainer.java | 6 +++--- .../java/frc/robot/subsystems/ClimberSubsystem.java | 2 ++ .../java/frc/robot/subsystems/IntakeSubsystem.java | 10 +++++----- .../java/frc/robot/subsystems/ShooterSubsystem.java | 4 ++-- 5 files changed, 14 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8dfc7e7..39a2bdb 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -110,8 +110,8 @@ public static final class IntakeConstants { public static final double kIntakeSpeed = 0.5; - // public static final int kProximityThreshold = 130; - public static final int kIRThreshold = 2; + public static final int kProximityThreshold = 130; + // public static final int kIRThreshold = 2; } public static final class ShooterConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3bdb256..0c22866 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -227,14 +227,14 @@ private void configureBindings() { return m_operatorController.getBButton() && m_operatorController.getRightBumper(); }).whileTrue(new InstantCommand(() -> m_climberSubsystem.reverse())); - // Toggle Distance Sensor, Operator Controller Left Bumper + Start Button + // Toggle Color Sensor, Operator Controller Left Bumper + Start Button new Trigger(() -> { return m_operatorController.getLeftBumper() && m_operatorController.getStartButton(); }).onTrue(new InstantCommand(() -> m_intakeSubsystem.colorSensorToggle())); - // Toggle Compressor, Operator Controller Right Bumper + Menu + // Toggle Compressor, Operator Controller Right Bumper + Back Button new Trigger(() -> { - return m_operatorController.getRightBumper() && m_operatorController.getStartButton(); + return m_operatorController.getLeftBumper() && m_operatorController.getBackButton(); }).onTrue(new InstantCommand(() -> m_climberSubsystem.toggleCompressor())); } diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java index 7a2f791..3a42743 100644 --- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java @@ -37,6 +37,8 @@ public void periodic() { m_rightSolenoid.set(m_state); SmartDashboard.putString("pneumatics state", m_state.name()); SmartDashboard.putNumber("pressure", m_pHub.getPressure(0)); + SmartDashboard.putBoolean("Compressor Enabled", m_compressorEnabled); + SmartDashboard.putBoolean("Compressor Running", m_pHub.getCompressor()); } /** diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 237adf2..20c50b8 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -38,7 +38,6 @@ public class IntakeSubsystem extends SubsystemBase { /** Creates a new IntakeSubsystem */ public IntakeSubsystem() { m_armEncoder.setPositionOffset(IntakeConstants.kArmEncoderOffset); - SmartDashboard.putNumber("arm", m_armEncoder.getAbsolutePosition()); m_armEncoder.setDistancePerRotation(360); m_intakeMotor.setIdleMode(IdleMode.kCoast); @@ -97,13 +96,13 @@ public void stopIntake() { /** * Gets distance from Color sensor */ - public int getColorIR() { - return m_colorSensor.getIR(); + public int getColorProximity() { + return m_colorSensor.getProximity(); } @Override public void periodic() { - haveNote = getColorIR() > IntakeConstants.kIRThreshold; + haveNote = m_colorSensorToggle ? getColorProximity() > IntakeConstants.kProximityThreshold : false; double armMotorSpeed = MathUtil.clamp(m_armPID.calculate(m_armEncoder.getDistance(), m_armSetpoint), -0.3, 0.3); m_armMotor.set(armMotorSpeed); @@ -113,7 +112,8 @@ public void periodic() { SmartDashboard.putNumber("Arm Absolute Angle", m_armEncoder.getAbsolutePosition()); SmartDashboard.putBoolean("Have Note?", haveNote); // SmartDashboard.putNumber("pid output", armMotorSpeed); - SmartDashboard.putNumber("Proximity", getColorIR()); + SmartDashboard.putNumber("Proximity", m_colorSensor.getProximity()); + SmartDashboard.putBoolean("Color Sensor Toggle", m_colorSensorToggle); SmartDashboard.putNumber("IR", m_colorSensor.getIR()); } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index f54afd1..55c14ea 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -60,8 +60,8 @@ public double returnCurrentSpeed() { @Override public void periodic() { // This method will be called once per scheduler run - SmartDashboard.putNumber("bottom Speed", m_bottomSpeed); - SmartDashboard.putNumber("top Speed", m_topSpeed); + // SmartDashboard.putNumber("bottom Speed", m_bottomSpeed); + // SmartDashboard.putNumber("top Speed", m_topSpeed); m_bottom.set(m_bottomSpeed); m_top.set(m_topSpeed);