Team_2648/2024_Robot_Code
5
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

4 note testing with shooting and intaking

Browse Source
This commit is contained in:
Bradley Bickford
2024-03-14 17:29:33 -04:00
parent 97938bfea2
commit d15e9ff14d
26 changed files with 85 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files

42
src/main/java/frc/robot/RobotContainer.java
View File

@@ -29,6 +29,7 @@ import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.PrintCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.WaitCommand;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.robot.Commands.autoIntaking;
@@ -111,6 +112,7 @@ public class RobotContainer {
).withTimeout(2.0)
);
/*
NamedCommands.registerCommand(
"Speaker Note Shot",
Commands.parallel(
@@ -120,10 +122,29 @@ public class RobotContainer {
1.0
),
indexer.shootNote(() -> 1.0)
).withTimeout(2.0)
).withTimeout(1.0)
);
*/
NamedCommands.registerCommand(
"Speaker Note Shot",
shooter.angleSpeedsSetpoints(
() -> ShooterConstants.kShooterLoadAngle,
1.0,
1.0
).raceWith(
new WaitCommand(0.5)
).andThen(
shooter.angleSpeedsSetpoints(
() -> ShooterConstants.kShooterLoadAngle,
1.0,
1.0
).alongWith(indexer.shootNote(() -> 1.0))
.withTimeout(1.0)
)
);
/*
NamedCommands.registerCommand(
"Auto Intake",
Commands.parallel(
@@ -132,7 +153,22 @@ public class RobotContainer {
() -> 1.0
),
indexer.advanceNote()
).withTimeout(3.0)//.until(indexer::getBeamBreak)
).until(() -> !indexer.getBeamBreak())
.withTimeout(1.0)
);*/
NamedCommands.registerCommand(
"Auto Intake",
intake.intakeControl(
() -> IntakeConstants.kDownAngle,
() -> 1.0
).alongWith(
indexer.advanceNote()
).until(() -> !indexer.getBeamBreak())
.andThen(
intake.stopAll()
)
//.withTimeout(1.0)
);
//NamedCommands.registerCommand("Auto Intake", new PrintCommand("Intake Note"));

8
src/main/java/frc/robot/subsystems/Intake.java
View File

@@ -10,6 +10,7 @@ import com.revrobotics.CANSparkLowLevel.MotorType;
import edu.wpi.first.math.controller.ArmFeedforward;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.util.function.BooleanConsumer;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.constants.IntakeConstants;
@@ -154,6 +155,13 @@ public class Intake extends SubsystemBase{
});
}
public Command stopAll() {
return runOnce(() -> {
intakeRoller.set(0);
intakePivot.setVoltage(0);
});
}
public double getIntakeAngle(){
return Units.rotationsToRadians(intakeEncoder.getPosition()/IntakeConstants.kIntakePivotConversionFactor)-armOffset;
}