From b2322bbffa5a5510a9abb0d9e8a775d5656187d0 Mon Sep 17 00:00:00 2001 From: Jeffthedoor Date: Wed, 23 Aug 2023 16:42:14 -0400 Subject: [PATCH] comp code!!! --- .../deploy/pathplanner/A2[3]-M-BACK-BLUE.path | 22 +-- .../deploy/pathplanner/A2[3]-M-BACK-RED.path | 168 ------------------ .../deploy/pathplanner/B2[1]-M-C-HIGH.path | 2 +- src/main/java/frc/robot/Constants.java | 12 +- src/main/java/frc/robot/RobotContainer.java | 2 - .../java/frc/robot/commands/HoldPower.java | 24 +-- .../frc/robot/commands/Lift/StateTable.java | 2 +- .../java/frc/robot/subsystems/Drivetrain.java | 1 + src/main/java/frc/thunder | 2 +- 9 files changed, 33 insertions(+), 202 deletions(-) delete mode 100644 src/main/deploy/pathplanner/A2[3]-M-BACK-RED.path diff --git a/src/main/deploy/pathplanner/A2[3]-M-BACK-BLUE.path b/src/main/deploy/pathplanner/A2[3]-M-BACK-BLUE.path index 4cc171b..ce406a7 100644 --- a/src/main/deploy/pathplanner/A2[3]-M-BACK-BLUE.path +++ b/src/main/deploy/pathplanner/A2[3]-M-BACK-BLUE.path @@ -29,15 +29,15 @@ { "anchorPoint": { "x": 6.86467565917588, - "y": 4.58 + "y": 4.48 }, "prevControl": { - "x": 5.949839022084632, - "y": 4.854450991127372 + "x": 4.4525175015262315, + "y": 5.195821468835921 }, "nextControl": { - "x": 5.949839022084632, - "y": 4.854450991127372 + "x": 4.4525175015262315, + "y": 5.195821468835921 }, "holonomicAngle": 0, "isReversal": true, @@ -85,12 +85,12 @@ "y": 3.173847751204275 }, "prevControl": { - "x": 6.5042405345716165, - "y": 5.14074652095046 + "x": 5.522572624092617, + "y": 6.039419096382052 }, "nextControl": { - "x": 6.5042405345716165, - "y": 5.14074652095046 + "x": 5.522572624092617, + "y": 6.039419096382052 }, "holonomicAngle": -60.0, "isReversal": true, @@ -110,11 +110,11 @@ { "anchorPoint": { "x": 1.6, - "y": 4.05 + "y": 4.12 }, "prevControl": { "x": 1.3255490088726252, - "y": 4.2382967363652275 + "y": 4.308296736365228 }, "nextControl": null, "holonomicAngle": 0, diff --git a/src/main/deploy/pathplanner/A2[3]-M-BACK-RED.path b/src/main/deploy/pathplanner/A2[3]-M-BACK-RED.path deleted file mode 100644 index cf0ffe5..0000000 --- a/src/main/deploy/pathplanner/A2[3]-M-BACK-RED.path +++ /dev/null @@ -1,168 +0,0 @@ -{ - "waypoints": [ - { - "anchorPoint": { - "x": 1.8, - "y": 4.35 - }, - "prevControl": null, - "nextControl": { - "x": 2.79267217592923, - "y": 4.695715054910851 - }, - "holonomicAngle": 0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [ - "Turn-Off-Vision", - "Score-Piece-Servo", - "Ground-Collect-Cube" - ], - "executionBehavior": "parallel", - "waitBehavior": "deadline", - "waitTime": 0.01 - } - }, - { - "anchorPoint": { - "x": 6.86467565917588, - "y": 4.48 - }, - "prevControl": { - "x": 5.949839022084632, - "y": 4.7544509911273725 - }, - "nextControl": { - "x": 5.949839022084632, - "y": 4.7544509911273725 - }, - "holonomicAngle": 0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": true, - "stopEvent": { - "names": [ - "Hold-Power", - "High-Score-Cube-Back" - ], - "executionBehavior": "parallel", - "waitBehavior": "deadline", - "waitTime": 0.01 - } - }, - { - "anchorPoint": { - "x": 1.5, - "y": 4.2 - }, - "prevControl": { - "x": 1.524748891049163, - "y": 4.192127883459711 - }, - "nextControl": { - "x": 1.524748891049163, - "y": 4.192127883459711 - }, - "holonomicAngle": 0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 7.018836142935444, - "y": 3.173847751204275 - }, - "prevControl": { - "x": 6.5042405345716165, - "y": 5.14074652095046 - }, - "nextControl": { - "x": 6.5042405345716165, - "y": 5.14074652095046 - }, - "holonomicAngle": -60.0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": true, - "stopEvent": { - "names": [ - "Hold-Power", - "Mid-Score-Cube-Back" - ], - "executionBehavior": "parallel", - "waitBehavior": "deadline", - "waitTime": 0.01 - } - }, - { - "anchorPoint": { - "x": 1.6, - "y": 4.05 - }, - "prevControl": { - "x": 1.3255490088726252, - "y": 4.2382967363652275 - }, - "nextControl": null, - "holonomicAngle": 0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [ - "Score", - "Stow" - ], - "executionBehavior": "sequential", - "waitBehavior": "none", - "waitTime": 0 - } - } - ], - "markers": [ - { - "position": 0.45, - "names": [ - "Collect" - ] - }, - { - "position": 2.2254545454543546, - "names": [ - "Stop-Collect" - ] - }, - { - "position": 1.83, - "names": [ - "Score" - ] - }, - { - "position": 2.0654545454545765, - "names": [ - "Ground-Collect-Cube" - ] - }, - { - "position": 2.35, - "names": [ - "Collect" - ] - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/B2[1]-M-C-HIGH.path b/src/main/deploy/pathplanner/B2[1]-M-C-HIGH.path index 9a23c95..329c71f 100644 --- a/src/main/deploy/pathplanner/B2[1]-M-C-HIGH.path +++ b/src/main/deploy/pathplanner/B2[1]-M-C-HIGH.path @@ -78,7 +78,7 @@ ], "markers": [ { - "position": 0.058181818181818994, + "position": 0.890909090909091, "names": [ "Stop-Collect" ] diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 66cc776..4a9d008 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -89,7 +89,7 @@ public static final class DrivetrainConstants { public static final double MAX_ANGULAR_ACCELERATION_RADIANS_PER_SECOND = +MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND * 2 * Math.PI / 5; // Module configuration constants - public static final int DRIVE_CURRENT_LIMIT = 40; + public static final int DRIVE_CURRENT_LIMIT = 35; public static final int STEER_CURRENT_LIMIT = 30; public static final double NOMINAL_VOLTAGE = 12d; @@ -262,13 +262,13 @@ public static final class ArmConstants { public static final class CollectorConstants { public static final boolean MOTOR_INVERT = false; - public static final int CURRENT_LIMIT = 30; + public static final int CURRENT_LIMIT = 50; public static final double HOLD_POWER_CUBE = 0.25; - public static final double HOLD_POWER_CONE = 0.03; + public static final double HOLD_POWER_CONE = 0.35; public static final MotorType MOTOR_TYPE = MotorType.kBrushless; public static final IdleMode NEUTRAL_MODE = IdleMode.kBrake; - public static final double STALL_POWER = 35d; // Used to detect wether or not the collector is stalling meaning it has a game piece + public static final double STALL_POWER = 40d; // Used to detect wether or not the collector is stalling meaning it has a game piece public static final double LOG_PERIOD = 0.22; @@ -313,9 +313,9 @@ public static final class WristConstants { public static final double LOG_PERIOD = 0.24; // Offsets in degrees - public static final double ENCODER_OFFSET_GRIDLOCK = 20.8; //-161.5d; + public static final double ENCODER_OFFSET_GRIDLOCK = -105.3; //-161.5d; - public static final double ENCODER_OFFSET_BLACKOUT = -22; //TODO: check this + public static final double ENCODER_OFFSET_BLACKOUT = -65.7; //TODO: check this // Conversion factor for our wrist, multiply this by the navite units to get degrees public static final double POSITION_CONVERSION_FACTOR = 360; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0add4fb..91c8582 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -160,8 +160,6 @@ protected void configureAutonomousCommands() { // new PathConstraints(AutonomousConstants.MAX_VELOCITY, AutonomousConstants.MAX_ACCELERATION)); //A paths OPEN - autoFactory.makeTrajectory("A2[3]-M-BACK-RED", Maps.getPathMap(drivetrain, servoturn, lift, collector, leds, arm), - new PathConstraints(3.5, 2)); autoFactory.makeTrajectory("A2[3]-M-BACK-BLUE", Maps.getPathMap(drivetrain, servoturn, lift, collector, leds, arm), new PathConstraints(3.5, 2)); //B paths MIDDLE diff --git a/src/main/java/frc/robot/commands/HoldPower.java b/src/main/java/frc/robot/commands/HoldPower.java index 20f4550..9d61a59 100644 --- a/src/main/java/frc/robot/commands/HoldPower.java +++ b/src/main/java/frc/robot/commands/HoldPower.java @@ -41,7 +41,7 @@ public HoldPower(Collector collector, DoubleSupplier input, XboxController drive public void execute() { if (input.getAsDouble() > 0) { // Collector collects doHoldPower = true; - power = input.getAsDouble() * 0.6; + power = input.getAsDouble(); } else if (input.getAsDouble() < 0) { // Collector spits doHoldPower = false; power = input.getAsDouble(); @@ -49,23 +49,23 @@ public void execute() { if(collector.getGamePiece() == GamePiece.CUBE){ // If the collector is holding a cube, hold at a lower power power = CollectorConstants.HOLD_POWER_CUBE; } else{ - if(lift.getGoalState() == LiftState.stowed){ - power = .35; - } else { + // if(lift.getGoalState() == LiftState.stowed){ + // power = .35; + // } else { power = CollectorConstants.HOLD_POWER_CONE; - } + // } } } else { power = 0; } - if (input.getAsDouble() < 0) { - collector.setCurrentLimit(60); - } else if (input.getAsDouble() > 0 && collector.getGamePiece() == GamePiece.CONE) { - collector.setCurrentLimit(50); - } else { - collector.setCurrentLimit(CollectorConstants.CURRENT_LIMIT); - } + // if (input.getAsDouble() < 0) { + // collector.setCurrentLimit(60); + // } else if (input.getAsDouble() > 0 && collector.getGamePiece() == GamePiece.CONE) { + // collector.setCurrentLimit(50); + // } else { + // collector.setCurrentLimit(CollectorConstants.CURRENT_LIMIT); + // } if(DriverStation.isTeleop()) { if(collector.getGamePiece() == GamePiece.CONE){ diff --git a/src/main/java/frc/robot/commands/Lift/StateTable.java b/src/main/java/frc/robot/commands/Lift/StateTable.java index ffc9722..f007362 100644 --- a/src/main/java/frc/robot/commands/Lift/StateTable.java +++ b/src/main/java/frc/robot/commands/Lift/StateTable.java @@ -54,7 +54,7 @@ public class StateTable { private static final double WRIST_GROUND_CUBE_ANGLE = 92; private static final LiftPlan GROUND_CUBE_PLAN = LiftPlan.parallel; - private static final double ELEVATOR_MID_CUBE_POS = 17.5; + private static final double ELEVATOR_MID_CUBE_POS = 14.2; private static final double ARM_MID_CUBE_ANGLE = -68; private static final double WRIST_MID_CUBE_ANGLE = 152d; private static final LiftPlan MID_CUBE_PLAN = LiftPlan.parallel; diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index c560ffe..99c3eb2 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -307,6 +307,7 @@ public void flipFL() { public void flipFR() { flipFR = !flipFR; + } public void flipBL() { diff --git a/src/main/java/frc/thunder b/src/main/java/frc/thunder index 810265e..ce7fdcb 160000 --- a/src/main/java/frc/thunder +++ b/src/main/java/frc/thunder @@ -1 +1 @@ -Subproject commit 810265ef3240b6f2066fbbd728c91a2ad827b35c +Subproject commit ce7fdcb781eaaad95fb497dc666b1e9b934252e8