Merge pull request #319 from frc-862/MARC

comp code!!!
frc-862 · Aug 25, 2023 · f5c60b4 · f5c60b4
2 parents 8e73212 + b2322bb
commit f5c60b4
Show file tree

Hide file tree

Showing 9 changed files with 33 additions and 202 deletions.
diff --git 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
diff --git 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
@@ -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
@@ -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
@@ -41,31 +41,31 @@ 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();
         } else if (doHoldPower) { // Hold power if no input and last input was inwards
             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
@@ -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
@@ -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