merge

awtybots · Feb 29, 2024 · dc1a491 · dc1a491
2 parents 70db4fe + 339f823
commit dc1a491
Show file tree

Hide file tree

Showing 8 changed files with 106 additions and 11 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -95,7 +95,7 @@ public static final class IntakeConstants {
     // Intake Feedforward characterization constants
     public static final double ks = 0.1;
     public static final double kv = 5;
-    public static double percentPower = 2;
+    public static double percentPower = 0.3;
   }
 
   public static final class ArmConstants {
@@ -154,7 +154,7 @@ public static final class Presets {
   }
 
   public static final class WristConstants {
-    public static final int kWristMotorId = 60; // ! Change before testing
+    public static final int kWristMotorId = 15; // ! Change before testing
 
     public static final int kCurrentLimit = 30;
 
@@ -199,7 +199,9 @@ public static final class ClimberConstants {
   }
 
   public static final class SticksConstants {
-    public static final int kSticksMotorId = 10000; // ! Change before testing
+    public static final int kLeftStickMotorId = 19; 
+    public static final int kRightStickMotorId = 18; 
+
 
     public static final int kCurrentLimit = 30;
 

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -28,6 +28,9 @@
 import frc.robot.commands.FeedForwardCharacterization;
 import frc.robot.commands.Positions.AmpShot;
 import frc.robot.commands.Positions.FloorPickup;
+import frc.robot.commands.Positions.ShootClose;
+import frc.robot.commands.Positions.ShootFar;
+import frc.robot.commands.Positions.ShootMedium;
 import frc.robot.commands.Positions.SpeakerShot;
 import frc.robot.commands.Positions.StowPosition;
 import frc.robot.commands.Positions.Upwards;
@@ -328,6 +331,10 @@ private void configureButtonBindings() {
                 sClimber::stop,
                 sClimber));
 
+    operatorController.povDown().whileTrue(ShootFar.run(sArm, sArmElevator, sWrist));
+    operatorController.povRight().whileTrue(ShootMedium.run(sArm, sArmElevator, sWrist));
+    operatorController.povUp().whileTrue(ShootClose.run(sArm, sArmElevator, sWrist));
+
     // run straight up position when y is pressed on operator. Using command Upwards
     operatorController.y().whileTrue(Upwards.run(sArm, sArmElevator, sWrist));
 

diff --git a/src/main/java/frc/robot/commands/ControlCommands/DriveCommands.java b/src/main/java/frc/robot/commands/ControlCommands/DriveCommands.java
@@ -27,7 +27,7 @@
 import java.util.function.DoubleSupplier;
 
 public class DriveCommands {
-  private static final double DEADBAND = 0.1;
+  private static final double DEADBAND = 0.17;
 
   private DriveCommands() {}
 
@@ -65,7 +65,7 @@ public static Command joystickDrive(
                 new Rotation2d(
                     MathUtil.applyDeadband(xSupplier.getAsDouble(), DEADBAND),
                     MathUtil.applyDeadband(ySupplier.getAsDouble(), DEADBAND));
-            omega = (MathUtil.applyDeadband(omegaSupplier.getAsDouble(), DEADBAND)) / 2;
+            omega = (MathUtil.applyDeadband(omegaSupplier.getAsDouble(), DEADBAND));
           }
 
           // Square values
@@ -87,7 +87,7 @@ public static Command joystickDrive(
               ChassisSpeeds.fromFieldRelativeSpeeds(
                   linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(),
                   linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(),
-                  omega * drive.getMaxAngularSpeedRadPerSec(),
+                  omega * drive.getMaxAngularSpeedRadPerSec() / 2.3,
                   isFlipped
                       ? drive.getRotation().plus(new Rotation2d(Math.PI))
                       : drive.getRotation()));

diff --git a/src/main/java/frc/robot/commands/ControlCommands/IntakeShooterControls.java b/src/main/java/frc/robot/commands/ControlCommands/IntakeShooterControls.java
@@ -59,14 +59,19 @@ public static Command intakeShooterDrive(
             revSpeed = MathUtil.applyDeadband(revSpeed, DEADBAND);
 
             double stickMagnitude = fwdSpeed - revSpeed;
+            stickMagnitude =
+                stickMagnitude * stickMagnitude * stickMagnitude; // more control over lower speeds
 
             int proximity = intake.getProximity();
-            boolean noteDetected = proximity < 2047 && proximity > 1000;
-            // ! An educated guess, may cause problems. It's at its highest when
-            // ! close and the smallest when far.
+            boolean noteDetected = proximity > 50;
 
             if (noteDetected) {
-              intake.runPercentSpeed(0);
+              if (stickMagnitude > 0.15) {
+                intake.runPercentSpeed(0);
+              } else {
+                intake.runPercentSpeed(
+                    Constants.IntakeConstants.percentPower * stickMagnitude); // allow for extake
+              }
             } else {
               intake.runPercentSpeed(Constants.IntakeConstants.percentPower * stickMagnitude);
             }

diff --git a/src/main/java/frc/robot/commands/Positions/FloorPickup.java b/src/main/java/frc/robot/commands/Positions/FloorPickup.java
@@ -12,7 +12,7 @@ public static Command run(Arm arm, ArmElevator armElevator, Wrist wrist) {
     return Commands.run(
         () -> {
           // Position preset settings
-          double ARMANGLE = 0.18;
+          double ARMANGLE = 0.205;
           double ARMELEVATORPOSITION = 0;
           double WRISTANGLE = 0;
 

diff --git a/src/main/java/frc/robot/commands/Positions/ShootClose.java b/src/main/java/frc/robot/commands/Positions/ShootClose.java
@@ -0,0 +1,27 @@
+package frc.robot.commands.Positions;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.subsystems.arm.Arm;
+import frc.robot.subsystems.armElevator.ArmElevator;
+import frc.robot.subsystems.wrist.Wrist;
+
+public class ShootClose { // bumper pressed against the wall
+
+  public static Command run(Arm arm, ArmElevator armElevator, Wrist wrist) {
+    return Commands.run(
+        () -> {
+          // Position preset settings
+          double ARMANGLE = 0.4342;
+          double ARMELEVATORPOSITION = 0;
+          double WRISTANGLE = 0;
+
+          arm.runTargetAngle(ARMANGLE);
+          armElevator.runTargetPosition(ARMELEVATORPOSITION);
+          wrist.runTargetAngle(WRISTANGLE);
+        },
+        arm,
+        armElevator,
+        wrist);
+  }
+}
diff --git a/src/main/java/frc/robot/commands/Positions/ShootFar.java b/src/main/java/frc/robot/commands/Positions/ShootFar.java
@@ -0,0 +1,27 @@
+package frc.robot.commands.Positions;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.subsystems.arm.Arm;
+import frc.robot.subsystems.armElevator.ArmElevator;
+import frc.robot.subsystems.wrist.Wrist;
+
+public class ShootFar { // two robots away from wall
+
+  public static Command run(Arm arm, ArmElevator armElevator, Wrist wrist) {
+    return Commands.run(
+        () -> {
+          // Position preset settings
+          double ARMANGLE = 0.7917;
+          double ARMELEVATORPOSITION = 0;
+          double WRISTANGLE = 0;
+
+          arm.runTargetAngle(ARMANGLE);
+          armElevator.runTargetPosition(ARMELEVATORPOSITION);
+          wrist.runTargetAngle(WRISTANGLE);
+        },
+        arm,
+        armElevator,
+        wrist);
+  }
+}
diff --git a/src/main/java/frc/robot/commands/Positions/ShootMedium.java b/src/main/java/frc/robot/commands/Positions/ShootMedium.java
@@ -0,0 +1,27 @@
+package frc.robot.commands.Positions;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.subsystems.arm.Arm;
+import frc.robot.subsystems.armElevator.ArmElevator;
+import frc.robot.subsystems.wrist.Wrist;
+
+public class ShootMedium { // one robot length away
+
+  public static Command run(Arm arm, ArmElevator armElevator, Wrist wrist) {
+    return Commands.run(
+        () -> {
+          // Position preset settings
+          double ARMANGLE = 0.6429;
+          double ARMELEVATORPOSITION = 0;
+          double WRISTANGLE = 0;
+
+          arm.runTargetAngle(ARMANGLE);
+          armElevator.runTargetPosition(ARMELEVATORPOSITION);
+          wrist.runTargetAngle(WRISTANGLE);
+        },
+        arm,
+        armElevator,
+        wrist);
+  }
+}