diff --git a/src/main/java/frc/robot/commands/GroundIntake.java b/src/main/java/frc/robot/commands/GroundIntake.java index e159da62..d59efaa3 100644 --- a/src/main/java/frc/robot/commands/GroundIntake.java +++ b/src/main/java/frc/robot/commands/GroundIntake.java @@ -34,8 +34,8 @@ public void initialize() {} public void execute() { double robotSpeed = Math.sqrt(Math.pow(driveTrain.getChassisSpeeds().vxMetersPerSecond, 2) + Math.pow(driveTrain.getChassisSpeeds().vyMetersPerSecond, 2)); double rollerSpeed = (IntakeConstants.INTAKE_SPEED - (0.25 * IntakeConstants.INTAKE_SPEED)) * (robotSpeed / DriveConstants.MAX_VELOCITY_METERS_PER_SECOND) + (0.25 * IntakeConstants.INTAKE_SPEED); - double sideSpeed = (IntakeConstants.INTAKE_SIDE_SPEED - (0.25 * IntakeConstants.INTAKE_SIDE_SPEED)) * (robotSpeed / DriveConstants.MAX_VELOCITY_METERS_PER_SECOND) + (0.75 * IntakeConstants.INTAKE_SIDE_SPEED); - double indexerSpeed = (IndexerConstants.INDEXER_INTAKE_SPEED- (0.25 * IndexerConstants.INDEXER_INTAKE_SPEED)) * (robotSpeed / DriveConstants.MAX_VELOCITY_METERS_PER_SECOND) + (0.75 * IndexerConstants.INDEXER_INTAKE_SPEED); + double sideSpeed = (IntakeConstants.INTAKE_SIDE_SPEED - (0.25 * IntakeConstants.INTAKE_SIDE_SPEED)) * (robotSpeed / DriveConstants.MAX_VELOCITY_METERS_PER_SECOND) + (0.25 * IntakeConstants.INTAKE_SIDE_SPEED); + double indexerSpeed = (IndexerConstants.INDEXER_INTAKE_SPEED- (0.25 * IndexerConstants.INDEXER_INTAKE_SPEED)) * (robotSpeed / DriveConstants.MAX_VELOCITY_METERS_PER_SECOND) + (0.25 * IndexerConstants.INDEXER_INTAKE_SPEED); intake.intakeYes(rollerSpeed, sideSpeed); indexer.set(indexerSpeed); }