Good Intake Code

2491-NoMythic · Mar 30, 2024 · d042c92 · d042c92
1 parent ad3665f
commit d042c92
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 7 deletions.
diff --git a/src/main/java/frc/robot/commands/GroundIntake.java b/src/main/java/frc/robot/commands/GroundIntake.java
@@ -32,13 +32,15 @@ public void initialize() {}
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    double mult = 1.0;
+    double mult = 0.43;
     double robotSpeed = Math.sqrt(Math.pow(driveTrain.getChassisSpeeds().vxMetersPerSecond, 2) + Math.pow(driveTrain.getChassisSpeeds().vyMetersPerSecond, 2));
-    double rollerSpeed = (IntakeConstants.INTAKE_SPEED - (mult * IntakeConstants.INTAKE_SPEED)) * (robotSpeed / DriveConstants.MAX_VELOCITY_METERS_PER_SECOND) + (mult * IntakeConstants.INTAKE_SPEED);
-    double sideSpeed =  (IntakeConstants.INTAKE_SIDE_SPEED - (mult * IntakeConstants.INTAKE_SIDE_SPEED)) * (robotSpeed / DriveConstants.MAX_VELOCITY_METERS_PER_SECOND) + (mult * IntakeConstants.INTAKE_SIDE_SPEED);
-    double indexerSpeed = (IndexerConstants.INDEXER_INTAKE_SPEED- (mult * IndexerConstants.INDEXER_INTAKE_SPEED)) * (robotSpeed / DriveConstants.MAX_VELOCITY_METERS_PER_SECOND) + (mult * IndexerConstants.INDEXER_INTAKE_SPEED);
+    double rollerSpeed = (IntakeConstants.INTAKE_SPEED - (mult * IntakeConstants.INTAKE_SPEED)) * (robotSpeed / /*DriveConstants.MAX_VELOCITY_METERS_PER_SECOND*/2.4) + (mult * IntakeConstants.INTAKE_SPEED);
+    double sideSpeed =  (IntakeConstants.INTAKE_SIDE_SPEED - (mult * IntakeConstants.INTAKE_SIDE_SPEED)) * (robotSpeed / /*DriveConstants.MAX_VELOCITY_METERS_PER_SECOND*/2.4) + (mult * IntakeConstants.INTAKE_SIDE_SPEED);
+    double indexerSpeed = (IndexerConstants.INDEXER_INTAKE_SPEED- (mult * IndexerConstants.INDEXER_INTAKE_SPEED)) * (robotSpeed / /*DriveConstants.MAX_VELOCITY_METERS_PER_SECOND*/2.4) + (mult * IndexerConstants.INDEXER_INTAKE_SPEED);
     intake.intakeYes(rollerSpeed, sideSpeed);
     indexer.set(indexerSpeed);
+    // intake.intakeYes(0.6, 0.6);
+    // indexer.set(0.9);
   }
 
   // Called once the command ends or is interrupted.

diff --git a/src/main/java/frc/robot/settings/Constants.java b/src/main/java/frc/robot/settings/Constants.java
@@ -330,7 +330,7 @@ public static final class ClimberConstants{
 public static final class IndexerConstants{
   public static final int INDEXER_MOTOR = 11;
   public static final int CURRENT_LIMIT = 50;
-  public static final double INDEXER_INTAKE_SPEED = 0.4516 ;//ground intake
+  public static final double INDEXER_INTAKE_SPEED = 1;//0.903 ;//speed to pick up at 10 ft/s
   public static final double HUMAN_PLAYER_INDEXER_SPEED = -0.5;//should be 0.5 TODO change to positive
   public static final double INDEXER_SHOOTING_RPS = 90;
   public static final double INDEXER_SHOOTING_POWER = 1;
@@ -362,8 +362,8 @@ public static final class IntakeConstants{
   public static final int BRUSH_2_MOTOR = 2491;
   public static final int BRUSH_3_MOTOR = 2491;
     /** the desired speed for the intake when doing ground intake. In Percent-of-full-power, from -1 to 1 */
-  public static final double INTAKE_SPEED = 0.29167 ;
-  public static final double INTAKE_SIDE_SPEED = 0.29167;
+  public static final double INTAKE_SPEED = 0.65;//0.5834 ; // ground intakes speed. 0.4167 picks up at 10 ft/s
+  public static final double INTAKE_SIDE_SPEED = 0.65;
 }
 public static final class CTREConfigs {
   public TalonFXConfiguration driveMotorConfig;