Merge branch 'testing' of https://github.com/Creekside7688/FRC-2024 i…

…nto testing
Creekside7688 · Feb 28, 2024 · 909cedc · 909cedc
2 parents ccc0257 + 257f363
commit 909cedc
Show file tree

Hide file tree

Showing 17 changed files with 124 additions and 90 deletions.
diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json
@@ -4,9 +4,9 @@
   "holonomicMode": true,
   "pathFolders": [],
   "autoFolders": [],
-  "defaultMaxVel": 3.5,
-  "defaultMaxAccel": 3.0,
-  "defaultMaxAngVel": 540.0,
-  "defaultMaxAngAccel": 720.0,
+  "defaultMaxVel": 3.0,
+  "defaultMaxAccel": 1.5,
+  "defaultMaxAngVel": 180.0,
+  "defaultMaxAngAccel": 180.0,
   "maxModuleSpeed": 4.0
 }
diff --git a/src/main/deploy/pathplanner/autos/Left Amp.auto b/src/main/deploy/pathplanner/autos/Left Amp.auto
@@ -29,24 +29,6 @@
             "pathName": "Left Pickup Amp"
           }
         },
-        {
-          "type": "path",
-          "data": {
-            "pathName": "Left Amp"
-          }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "AmpNote"
-          }
-        },
-        {
-          "type": "path",
-          "data": {
-            "pathName": "Left Pickup 2"
-          }
-        },
         {
           "type": "path",
           "data": {

diff --git a/src/main/deploy/pathplanner/paths/Leave.path b/src/main/deploy/pathplanner/paths/Leave.path
@@ -96,10 +96,10 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.5,
-    "maxAcceleration": 3.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0
+    "maxVelocity": 3.0,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 180.0,
+    "maxAngularAcceleration": 180.0
   },
   "goalEndState": {
     "velocity": 0,

diff --git a/src/main/deploy/pathplanner/paths/Left Amp Return.path b/src/main/deploy/pathplanner/paths/Left Amp Return.path
@@ -3,39 +3,45 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 7.90971311672353,
-        "y": 7.432741351969191
+        "x": 3.3166924515667215,
+        "y": 7.145819382547857
       },
       "prevControl": null,
       "nextControl": {
-        "x": 8.90971311672353,
-        "y": 7.432741351969191
+        "x": 2.2345579236613022,
+        "y": 6.716697069757776
       },
       "isLocked": false,
-      "linkedName": "Left Pickup 2"
+      "linkedName": "Left Pickup 1 Amp"
     },
     {
       "anchor": {
-        "x": 1.8082090218234488,
-        "y": 7.728571853540104
+        "x": 1.82409310273166,
+        "y": 7.938762786616483
       },
       "prevControl": {
-        "x": 2.9083286995402817,
-        "y": 6.5729839567787245
+        "x": 1.740134389359688,
+        "y": 7.528297965686841
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": "Left Amp"
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.25,
+      "rotationDegrees": 90.0,
+      "rotateFast": false
     }
   ],
-  "rotationTargets": [],
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.5,
-    "maxAcceleration": 3.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0
+    "maxVelocity": 1.5,
+    "maxAcceleration": 0.75,
+    "maxAngularVelocity": 180.0,
+    "maxAngularAcceleration": 180.0
   },
   "goalEndState": {
     "velocity": 0,
@@ -48,5 +54,5 @@
     "rotation": 0,
     "velocity": 0
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }
diff --git a/src/main/deploy/pathplanner/paths/Left Amp.path b/src/main/deploy/pathplanner/paths/Left Amp.path
@@ -8,34 +8,40 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 1.1241009869407124,
-        "y": 6.9797508964387305
+        "x": 1.1057796661047867,
+        "y": 6.977901955803912
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 1.8082090218234488,
-        "y": 7.728571853540104
+        "x": 1.8893943242431936,
+        "y": 7.938762786616483
       },
       "prevControl": {
-        "x": 1.6787831773861743,
-        "y": 7.192379069442823
+        "x": 1.8707368323827553,
+        "y": 6.595423372664928
       },
       "nextControl": null,
       "isLocked": false,
       "linkedName": "Left Amp"
     }
   ],
-  "rotationTargets": [],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.5,
+      "rotationDegrees": 90.0,
+      "rotateFast": false
+    }
+  ],
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.5,
-    "maxAcceleration": 3.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0
+    "maxVelocity": 3.0,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 180.0,
+    "maxAngularAcceleration": 180.0
   },
   "goalEndState": {
     "velocity": 0,

diff --git a/src/main/deploy/pathplanner/paths/Left Pickup 2.path b/src/main/deploy/pathplanner/paths/Left Pickup 2.path
@@ -50,10 +50,10 @@
     }
   ],
   "globalConstraints": {
-    "maxVelocity": 3.5,
-    "maxAcceleration": 3.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0
+    "maxVelocity": 3.0,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 180.0,
+    "maxAngularAcceleration": 180.0
   },
   "goalEndState": {
     "velocity": 0,

diff --git a/src/main/deploy/pathplanner/paths/Left Pickup Amp.path b/src/main/deploy/pathplanner/paths/Left Pickup Amp.path
@@ -3,37 +3,48 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 1.8082090218234488,
-        "y": 7.728571853540104
+        "x": 1.8893943242431936,
+        "y": 7.938762786616483
       },
       "prevControl": null,
       "nextControl": {
-        "x": 1.826698428171631,
-        "y": 7.358783726576463
+        "x": 1.9078837305913758,
+        "y": 7.5689746596528416
       },
       "isLocked": false,
       "linkedName": "Left Amp"
     },
     {
       "anchor": {
-        "x": 2.5108064630543674,
-        "y": 6.831835645653273
+        "x": 3.3166924515667215,
+        "y": 7.145819382547857
       },
       "prevControl": {
-        "x": 1.3829526758152613,
-        "y": 6.859569755175546
+        "x": 0.741958574826241,
+        "y": 6.091671092433095
       },
       "nextControl": null,
       "isLocked": false,
       "linkedName": "Left Pickup 1 Amp"
     }
   ],
-  "rotationTargets": [],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.7,
+      "rotationDegrees": 20.0,
+      "rotateFast": false
+    },
+    {
+      "waypointRelativePos": 0.3,
+      "rotationDegrees": 90.0,
+      "rotateFast": false
+    }
+  ],
   "constraintZones": [],
   "eventMarkers": [
     {
       "name": "New Event Marker",
-      "waypointRelativePos": 0.0,
+      "waypointRelativePos": 0.85,
       "command": {
         "type": "sequential",
         "data": {
@@ -50,20 +61,20 @@
     }
   ],
   "globalConstraints": {
-    "maxVelocity": 3.5,
-    "maxAcceleration": 3.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0
+    "maxVelocity": 3.0,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 180.0,
+    "maxAngularAcceleration": 180.0
   },
   "goalEndState": {
     "velocity": 0,
     "rotation": 20.69545073406333,
-    "rotateFast": true
+    "rotateFast": false
   },
   "reversed": false,
   "folder": null,
   "previewStartingState": {
-    "rotation": 0,
+    "rotation": 90.0,
     "velocity": 0
   },
   "useDefaultConstraints": true

diff --git a/src/main/deploy/pathplanner/paths/Right Roundhouse Amp.path b/src/main/deploy/pathplanner/paths/Right Roundhouse Amp.path
@@ -48,10 +48,10 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.5,
-    "maxAcceleration": 3.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0
+    "maxVelocity": 3.0,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 180.0,
+    "maxAngularAcceleration": 180.0
   },
   "goalEndState": {
     "velocity": 0,

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -7,16 +7,21 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import frc.robot.elevator.Elevator;
+import frc.robot.elevator.commands.ElevatorDown;
 
 public class Robot extends TimedRobot {
     private Command autonomousCommand;
 
+    //private Elevator elevator = new Elevator();
     private RobotContainer robotContainer;
     private Alliance alliance = Alliance.Red;
 
     @Override
     public void robotInit() {
         robotContainer = new RobotContainer();
+
         checkDriverStationUpdate();
     }
 
@@ -60,9 +65,11 @@ public void autonomousPeriodic() {
     @Override
     public void teleopInit() {
         checkDriverStationUpdate();
+
         if(autonomousCommand != null) {
             autonomousCommand.cancel();
         }
+        //CommandScheduler.getInstance().schedule(new ElevatorDown(elevator));
     }
 
     @Override

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -82,8 +82,8 @@ public RobotContainer() {
     }
 
     private void configureSubsystemCommands() {
-        controller.getLeftBumper().onTrue(intakeEject);
-        controller.getRightBumper().whileTrue(intakePickup);
+        controller.getLeftBumper().onTrue(intakePickup);
+        controller.getRightBumper().whileTrue(intakeShooterFeed);
 
         controller.getX().onTrue(elevatorUp);
         controller.getA().onTrue(ampScore);
@@ -105,7 +105,7 @@ private void configureSwerveDriveCommands() {
     }
 
     private void configureAutonomous() {
-        NamedCommands.registerCommand("PickupNote", intakePickup);
+        NamedCommands.registerCommand("PickupNote", new IntakePickup(intake));
         NamedCommands.registerCommand("AmpNote", ampScore);
 
         autoSelector.addOption("Right Roundhouse Amp", new PathPlannerAuto("Right Roundhouse Amp"));

diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java
@@ -44,7 +44,7 @@ public class ElevatorConstants {
     public static final double MOTOR_MAX_STEPS = 1.92;
     public static final double MOTOR_MIN_STEPS = 0;
 
-    public static final double MOTOR_TEMP_STEPS = 0.02;
+    public static final double MOTOR_TEMP_STEPS = 6;
     public static final double MOTOR_TEMPDROP_SPEED = -0.1;
     public static final double MOTOR_TEMPDROP_DELAY = 0.5;
 }

diff --git a/src/main/java/frc/robot/constants/IntakeConstants.java b/src/main/java/frc/robot/constants/IntakeConstants.java
@@ -14,7 +14,7 @@ public class IntakeConstants {
     public static final double AMP_SPEED = -1.0;
     public static final double AMP_DURATION = 1;
 
-    public static final double PICKUP_SPEED = 1;
+    public static final double PICKUP_SPEED = 0.8;
 
     public static final double SHOOTER_FEED_SPEED = 1;
     public static final double SHOOTER_FEED_DURATION = 0.75;

diff --git a/src/main/java/frc/robot/elevator/Elevator.java b/src/main/java/frc/robot/elevator/Elevator.java
@@ -17,23 +17,32 @@ public Elevator() {
         motor.restoreFactoryDefaults();
         motor.setIdleMode(ElevatorConstants.IDLE_MODE);
         motor.setSmartCurrentLimit(ElevatorConstants.CURRENT_LIMIT);
-        motor.burnFlash();
+
 
         encoder = motor.getEncoder();
         encoder.setPosition(0);
+        encoder.setPositionConversionFactor(1);
+        motor.burnFlash();
+    }
+
+
+
+    public void getMotorRPM() {
+        SmartDashboard.putNumber("Evt. Motor RPM", encoder.getVelocity());
     }
 
     @Override
+
     public void periodic() {
-        SmartDashboard.putNumber("Elevator Height (Inches)", this.getEncoderPosition() * 210);
+        SmartDashboard.putNumber("Elevator Height (Inches)", this.getEncoderPosition() );
     }
 
     public void run(double speed) {
         motor.set(speed);
     }
 
     public double getEncoderPosition() {
-        return encoder.getPosition() / 210;
+        return encoder.getPosition()/210;
     }
 
     public void resetEncoder() {