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

Intake control changes and automated shooter/indexer firing

Browse Source
This commit is contained in:
Bradley Bickford 2024-03-10 15:43:27 -04:00
parent 51f2ad4c30
commit 1119ce0e74
3 changed files with 83 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -18,6 +18,7 @@ import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.GenericHID.RumbleType;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
@ -26,8 +27,10 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.robot.constants.IntakeConstants;
import frc.robot.constants.OIConstants;
import frc.robot.constants.PhotonConstants;
import frc.robot.constants.ShooterConstants;
@ -55,6 +58,7 @@ public class RobotContainer {
private Trigger isTeleopTrigger;
private Trigger isEnabledTrigger;
private Trigger indexerBeamBreakWatcher;
private SendableChooser<Command> autoChooser;
@ -67,6 +71,7 @@ public class RobotContainer {
private int speakerTag;
private boolean setAmpAngle;
private boolean intakeDown;
private boolean invertXYDrive;
public RobotContainer() {
@ -111,8 +116,10 @@ public class RobotContainer {
isTeleopTrigger = new Trigger(DriverStation::isTeleopEnabled);
isEnabledTrigger = new Trigger(DriverStation::isEnabled);
indexerBeamBreakWatcher = new Trigger(indexer::getBeamBreak);
setAmpAngle = false;
intakeDown = false;
configureBindings();
shuffleboardSetup();
@ -157,7 +164,35 @@ public class RobotContainer {
);
//intake.setDefaultCommand(intake.intakeUpCommand());
intake.setDefaultCommand(intake.manualPivot(() -> -operator.getLeftY(), operator::getRightY));
//intake.setDefaultCommand(intake.manualPivot(() -> -operator.getLeftY(), operator::getRightY));
intake.setDefaultCommand(
intake.intakeControl(
() -> {
if (intakeDown) {
return IntakeConstants.kDownAngle;
} else {
return IntakeConstants.kUpAngle;
}
},
operator::getRightY
)
);
operator.povUp().onTrue(
new InstantCommand(
() -> {
intakeDown = false;
}
)
);
operator.povDown().onTrue(
new InstantCommand(
() -> {
intakeDown = true;
}
)
);
shooter.setDefaultCommand(
shooter.angleSpeedsSetpoints(
@ -179,6 +214,20 @@ public class RobotContainer {
);
//shooter.setDefaultCommand(shooter.manualPivot(operator::getRightY, operator::getLeftTriggerAxis));
driver.leftBumper().onTrue(
shooter.angleSpeedsSetpoints(
() -> ShooterConstants.kShooterLoadAngle,
() -> 1.0
).withTimeout(.5).andThen(
shooter.angleSpeedsSetpoints(
() -> ShooterConstants.kShooterLoadAngle,
() -> 1.0
).alongWith(
indexer.shootNote(() -> 1.0)
).withTimeout(2)
)
);
climber.setDefaultCommand(climber.stop());
indexer.setDefaultCommand(indexer.shootNote(
@ -191,6 +240,16 @@ public class RobotContainer {
}
));
indexerBeamBreakWatcher.onFalse(
new RunCommand(() -> {
driver.getHID().setRumble(RumbleType.kBothRumble, 1);
}).withTimeout(.5).andThen(
new InstantCommand(() -> {
driver.getHID().setRumble(RumbleType.kBothRumble, 0);
})
)
);
/*
driver.povCenter().onFalse(
drivetrain.driveCardinal(
@ -284,8 +343,8 @@ public class RobotContainer {
operator.start().onTrue(shooter.zeroEncoder());
operator.povDown().whileTrue(intake.intakeDownCommand());
operator.povUp().whileTrue(intake.intakeUpCommand());
//operator.povDown().whileTrue(intake.intakeDownCommand());
//operator.povUp().whileTrue(intake.intakeUpCommand());
driver.leftTrigger().whileTrue(Commands.parallel( intake.manualPivot(() -> 0.0, () -> 1.0), indexer.advanceNote()));
@ -312,7 +371,7 @@ public class RobotContainer {
teleopTab.addBoolean("shooter beam break", shooter::getBeamBreak);
teleopTab.addDouble("shooter angle", shooter::getShooterAngle);
teleopTab.addDouble("intake angle", intake::getIntakeDegrees);
teleopTab.addDouble("intake pid", intake::getIntakePID);
//teleopTab.addDouble("intake pid", intake::getIntakePID);
teleopTab.addDouble("heading swerve", drivetrain::getHeading);
teleopTab.addDouble("steering encoder", drivetrain::getEncoderSteering);
}

8
src/main/java/frc/robot/constants/IntakeConstants.java
View File

@ -8,13 +8,13 @@ public class IntakeConstants {
public static final int kPivotCurrentLimit = 40;
public static final double kPIntake = 2.5;
public static final double kPIntake = 3;//4; //2.5;
public static final double kIIntake = 0;
public static final double kDIntake = 0;
public static final double kDIntake = 0.01;
public static final double kSFeedForward = 0;
public static final double kGFeedForward = 0;//1.11;
public static final double kVFeedForward = 0;//0.73;
public static final double kGFeedForward = 1;//1.11;
public static final double kVFeedForward = .5;//0.73;
public static final double kStartingAngle = Math.toRadians(105.0);
public static final double kUpAngle = Math.toRadians(90.0);

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

@ -61,6 +61,22 @@ public class Intake extends SubsystemBase{
armOffset = getIntakeAngle()-IntakeConstants.kStartingAngle;
}
public Command intakeControl(DoubleSupplier pivotAngle, DoubleSupplier intakeSpeed) {
return run(() -> {
intakeRoller.set(intakeSpeed.getAsDouble());
intakePivot.setVoltage(
intakeAnglePID.calculate(
getIntakeAngle(),
pivotAngle.getAsDouble()
) + intakeFeedForward.calculate(
pivotAngle.getAsDouble(),
0.0
)
);
});
}
public Command intakeDownCommand() {
return run(() -> {
intakeRoller.set(0.0);