Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Major upgrades #88

Closed
wants to merge 16 commits into from
Closed

Major upgrades #88

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
16 commits
Select commit Hold shift + click to select a range
6818dc1
changed to use only 1 subwoofer supplier, added operatorController, a…
rflood07 Feb 26, 2024
aab4da6
operator controller good
rflood07 Feb 27, 2024
f6aa252
cleaned up button bindings
rflood07 Feb 27, 2024
60ad90a
found values for comp bot equation, changed speaker math a wee
rflood07 Feb 28, 2024
65c684b
put stage angle good
2491NoMythic Feb 28, 2024
fc675e9
tuned some constants and set climber inversions
rflood07 Feb 28, 2024
3a84ebe
tuned initial shot and cleanup
2491NoMythic Feb 28, 2024
2d36bce
tuned some shooting
2491NoMythic Feb 28, 2024
daeca88
tuned values, works <3
2491NoMythic Feb 28, 2024
1261dc0
odometry better
2491NoMythic Feb 28, 2024
d80f84e
amp shot and score 1 autos work
2491NoMythic Feb 28, 2024
873584e
podium shot updated
2491NoMythic Feb 28, 2024
28a4627
Merge branch 'tuneShootingEquationComp' of https://github.com/2491-No…
2491NoMythic Feb 28, 2024
3a825d5
added groundIntake and safe angle when we are on the other side of th…
2491NoMythic Feb 28, 2024
0c4568f
cleanup
2491NoMythic Feb 28, 2024
7aecdbd
changed ground intake control
2491NoMythic Feb 28, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/LimelightHelpers.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -765,7 +765,7 @@ public static LimelightResults getLatestResults(String limelightName) {
try {
results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class);
} catch (JsonProcessingException e) {
System.err.println("lljson error: " + e.getMessage());
// System.err.println("lljson error: " + e.getMessage());
}

long end = System.nanoTime();
Expand Down
82 changes: 59 additions & 23 deletions src/main/java/frc/robot/RobotContainer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,9 @@

import static frc.robot.settings.Constants.PS4Driver.*;
import static frc.robot.settings.Constants.ShooterConstants.LONG_SHOOTING_RPS;

import java.util.function.BooleanSupplier;

import static frc.robot.settings.Constants.DriveConstants.*;

import com.pathplanner.lib.auto.AutoBuilder;
Expand All @@ -19,6 +22,7 @@
import frc.robot.commands.CollectNote;
import frc.robot.commands.Drive;
import frc.robot.commands.DriveTimeCommand;
import frc.robot.commands.GroundIntake;
import frc.robot.commands.ConditionalIndexer;
import frc.robot.settings.Constants.IndexerConstants;
import frc.robot.commands.IndexCommand;
Expand Down Expand Up @@ -87,6 +91,7 @@ public class RobotContainer {
private Climber climber;
private Lights lights;
private PS4Controller driverController;
private PS4Controller operatorController;
//private PS4Controller operatorController;
private Limelight limelight;
private IndexCommand defaulNoteHandlingCommand;
Expand All @@ -96,6 +101,21 @@ public class RobotContainer {
private SendableChooser<Command> autoChooser;
private PowerDistribution PDP;

BooleanSupplier ZeroGyroSup;
BooleanSupplier AimWhileMovingSup;
BooleanSupplier ShootIfReadySup;
BooleanSupplier SubwooferAngleSup;
BooleanSupplier StageAngleSup;
BooleanSupplier HumanPlaySup;
BooleanSupplier AmpAngleSup;
BooleanSupplier SourcePickupSup;
BooleanSupplier ClimberDownSup;
BooleanSupplier ClimberUpSup;
BooleanSupplier ShooterUpManualSup;
BooleanSupplier ManualShootSup;
BooleanSupplier ForceVisionSup;
BooleanSupplier GroundIntakeSup;

// Replace with CommandPS4Controller or CommandJoystick if needed

/** The container for the robot. Contains subsystems, OI devices, and commands. */
Expand All @@ -120,8 +140,23 @@ public RobotContainer() {
// SignalLogger.setPath("/media/sda1/ctre-logs/");
// SignalLogger.start();
driverController = new PS4Controller(DRIVE_CONTROLLER_ID);
//operatorController = new PS4Controller(OPERATOR_CONTROLLER_ID);
operatorController = new PS4Controller(OPERATOR_CONTROLLER_ID);
//operatorController = new PS4Controller(OPERATOs_CONTROLLER_ID);
PDP = new PowerDistribution(1, ModuleType.kRev);

ZeroGyroSup = driverController::getOptionsButton;
AimWhileMovingSup = driverController::getL2Button;
ShootIfReadySup = driverController::getR2Button;
SubwooferAngleSup = driverController::getCrossButton;
StageAngleSup = driverController::getTriangleButton;
HumanPlaySup = driverController::getR1Button;
AmpAngleSup = ()->driverController.getPOV() == 90;
ClimberDownSup = operatorController::getCrossButton;
ClimberUpSup = operatorController::getTriangleButton;
ShooterUpManualSup = ()->driverController.getPOV() == 0;
ManualShootSup = driverController::getL1Button;
ForceVisionSup = driverController::getOptionsButton;
GroundIntakeSup = operatorController::getTouchpad;

// = new PathPlannerPath(null, DEFAUL_PATH_CONSTRAINTS, null, climberExists);
limelightInit();
Expand Down Expand Up @@ -171,7 +206,7 @@ private void shooterInst() {
}
private void angleShooterInst(){
angleShooterSubsystem = new AngleShooterSubsystem();
defaultShooterAngleCommand = new AimShooter(angleShooterSubsystem, driverController::getPOV, driverController::getR1Button, driverController::getR1Button, driverController::getR2Button, driverController::getL2Button);
defaultShooterAngleCommand = new AimShooter(angleShooterSubsystem, driverController::getPOV, HumanPlaySup, SubwooferAngleSup, StageAngleSup, GroundIntakeSup);
angleShooterSubsystem.setDefaultCommand(defaultShooterAngleCommand);
}
private void intakeInst() {
Expand All @@ -184,7 +219,7 @@ private void indexInit() {
indexer = new IndexerSubsystem();
}
private void indexCommandInst() {
defaulNoteHandlingCommand = new IndexCommand(indexer, driverController::getR2Button, driverController::getL2Button, shooter, intake, driveTrain, angleShooterSubsystem, driverController::getR1Button, driverController::getPOV);
defaulNoteHandlingCommand = new IndexCommand(indexer, ShootIfReadySup, AimWhileMovingSup, shooter, intake, driveTrain, angleShooterSubsystem, HumanPlaySup, StageAngleSup, SubwooferAngleSup);
indexer.setDefaultCommand(defaulNoteHandlingCommand);
}

Expand Down Expand Up @@ -213,50 +248,48 @@ private void lightsInst() {
* joysticks}.
*/
private void configureBindings() {

// new Trigger(driverController::getCrossButton).onTrue(new autoAimParallel(driveTrain/*, shooter*/));
new Trigger(driverController::getPSButton).onTrue(new InstantCommand(driveTrain::zeroGyroscope));
new Trigger(driverController::getCircleButton).whileTrue(new GoToAmp(driveTrain));
new Trigger(driverController::getL2Button).whileTrue(new AimRobotMoving(
new Trigger(ZeroGyroSup).onTrue(new InstantCommand(driveTrain::zeroGyroscope));
// new Trigger(driverController::getCircleButton).whileTrue(new GoToAmp(driveTrain)); unused becuase we dont pickup from amp with a path anymore
new Trigger(AimWhileMovingSup).whileTrue(new AimRobotMoving(
driveTrain,
() -> modifyAxis(-driverController.getRawAxis(Z_AXIS), DEADBAND_NORMAL),
() -> modifyAxis(-driverController.getRawAxis(Y_AXIS), DEADBAND_NORMAL),
() -> modifyAxis(-driverController.getRawAxis(X_AXIS), DEADBAND_NORMAL),
driverController::getL2Button,
driverController::getR1Button));
driverController::getCrossButton,
driverController::getTriangleButton));

if(Preferences.getBoolean("Detector Limelight", false)) {
new Trigger(driverController::getR1Button).onTrue(new SequentialCommandGroup(
new Trigger(operatorController::getR1Button).onTrue(new SequentialCommandGroup(
new CollectNote(driveTrain, limelight),
new DriveTimeCommand(-2, 0, 0, 0.5, driveTrain)
));
}
new Trigger(driverController::getOptionsButton).onTrue(new InstantCommand(()->SmartDashboard.putBoolean("force use limelight", true))).onFalse(new InstantCommand(()->SmartDashboard.putBoolean("force use limelight", false)));
new Trigger(driverController::getTouchpadPressed).onTrue(new InstantCommand(driveTrain::stop, driveTrain));
new Trigger(ForceVisionSup).onTrue(new InstantCommand(()->SmartDashboard.putBoolean("force use limelight", true))).onFalse(new InstantCommand(()->SmartDashboard.putBoolean("force use limelight", false)));
SmartDashboard.putData("force update limelight position", new InstantCommand(()->driveTrain.forceUpdateOdometryWithVision(), driveTrain));
if(angleShooterExists) {
new Trigger(()->driverController.getPOV() == 180).whileTrue(new AngleShooter(angleShooterSubsystem, ()->ShooterConstants.PRAC_MAXIMUM_SHOOTER_ANGLE));
new Trigger(ShooterUpManualSup).whileTrue(new AngleShooter(angleShooterSubsystem, ()->ShooterConstants.PRAC_MAXIMUM_SHOOTER_ANGLE));
SmartDashboard.putData("Manual Angle Shooter Up", new AngleShooter(angleShooterSubsystem, ()->ShooterConstants.PRAC_MAXIMUM_SHOOTER_ANGLE));
}
if(indexerExists) {
new Trigger(driverController::getL1Button).whileTrue(new ManualShoot(indexer, driverController::getPOV));
new Trigger(ManualShootSup).whileTrue(new ManualShoot(indexer, driverController::getPOV));
}
if(climberExists) {
// new Trigger(driverController::getCrossButton).whileTrue(new AutoClimb(climber)).onFalse(new InstantCommand(()-> climber.climberStop()));
new Trigger(driverController::getCrossButton).onTrue(new InstantCommand(()-> climber.climberGo(ClimberConstants.CLIMBER_SPEED_DOWN))).onFalse(new InstantCommand(()-> climber.climberGo(0)));
new Trigger(driverController::getTriangleButton).onTrue(new InstantCommand(()-> climber.climberGo(ClimberConstants.CLIMBER_SPEED_UP))).onFalse(new InstantCommand(()-> climber.climberGo(0)));
new Trigger(ClimberDownSup).onTrue(new InstantCommand(()-> climber.climberGo(ClimberConstants.CLIMBER_SPEED_DOWN))).onFalse(new InstantCommand(()-> climber.climberGo(0)));
new Trigger(ClimberUpSup).onTrue(new InstantCommand(()-> climber.climberGo(ClimberConstants.CLIMBER_SPEED_UP))).onFalse(new InstantCommand(()-> climber.climberGo(0)));
// new Trigger(driverController::getSquareButton).whileTrue(new ClimberPullDown(climber));
}
if(shooterExists) {
new Trigger(()->driverController.getPOV() == 90).whileTrue(new InstantCommand(()->shooter.shootRPS(ShooterConstants.AMP_RPS), shooter));
new Trigger(AmpAngleSup).whileTrue(new InstantCommand(()->shooter.shootRPS(ShooterConstants.AMP_RPS), shooter));
}
if(intakeExists) {
new Trigger(driverController::getTouchpad).onTrue(new InstantCommand(()->intake.intakeYes(IntakeConstants.INTAKE_SPEED))).onFalse(new InstantCommand(intake::intakeOff));
new Trigger(GroundIntakeSup).whileTrue(new GroundIntake(intake, indexer));
}
if(indexerExists&&shooterExists&&angleShooterExists) {
// new Trigger(()->driverController.getPOV() == 270).whileTrue(new InstantCommand(()->angleShooterSubsystem.setDesiredShooterAngle(ShooterConstants.HUMAN_PLAYER_ANGLE)));
// new Trigger(()->driverController.getPOV() == 270).whileTrue(new InstantCommand(()->shooter.shootRPS(ShooterConstants.HUMAN_PLAYER_RPS)));
// new Trigger(()->driverController.getPOV() == 270).whileTrue(new InstantCommand(()->indexer.set(IndexerConstants.HUMAN_PLAYER_INDEXER_SPEED)));
new Trigger(()->driverController.getPOV() == 90).whileTrue(new shootAmp(indexer, shooter));
new Trigger(AmpAngleSup).whileTrue(new shootAmp(indexer, shooter));
SmartDashboard.putData("amp shot", new shootAmp(indexer, shooter));
}
InstantCommand setOffsets = new InstantCommand(driveTrain::setEncoderOffsets) {
Expand All @@ -274,11 +307,14 @@ public boolean runsWhenDisabled() {
* D-Pad down: move shooter up manually (hold)
* R1: aim shooter at amp (hold)
* Options button: collect note from human player
* Triangle: move climber up (hold)
* Square: auto-pick up note
* Cross: manually pull down with climber (hold)
* Touchpad: manually turn on Intake (hold) [only works if intake code doesn't exist in IndexCommand]
* L1,L2,R1,R2 held: aim shooter at speaker and set shooter to shooter speed
*
* operator:
* Triangle: climber up (hold)
* Cross: climber down (hold)
* R1: auto pickup note from ground (hold)
*
*/
//FOR TESTING PURPOSES:
Expand Down Expand Up @@ -373,7 +409,7 @@ private void registerNamedCommands() {
NamedCommands.registerCommand("intakeOn", new InstantCommand(()-> intake.intakeYes(1)));
}
if(indexerExists&&shooterExists) {
NamedCommands.registerCommand("initialShot", new initialShot(shooter, indexer, 2.0, 2.25));
NamedCommands.registerCommand("initialShot", new initialShot(shooter, indexer, 2.0, 2.25, angleShooterSubsystem));
NamedCommands.registerCommand("shootNote", new shootNote(indexer, 1));
NamedCommands.registerCommand("shootNote", new shootNote(indexer, 1));
NamedCommands.registerCommand("setFeedTrue", new InstantCommand(()->SmartDashboard.putBoolean("feedMotor", true)));
Expand Down
18 changes: 10 additions & 8 deletions src/main/java/frc/robot/commands/AimRobotMoving.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,12 +27,13 @@ public class AimRobotMoving extends Command {
DoubleSupplier translationXSupplier;
DoubleSupplier translationYSupplier;
BooleanSupplier run;
BooleanSupplier cancel;
BooleanSupplier cancel1;
BooleanSupplier cancel2;
DoubleSupplier rotationSupplier;
double rotationSpeed;
double allianceOffset;

public AimRobotMoving(DrivetrainSubsystem drivetrain, DoubleSupplier rotationSupplier, DoubleSupplier translationXSupplier, DoubleSupplier translationYSupplier, BooleanSupplier run, BooleanSupplier cancel){
public AimRobotMoving(DrivetrainSubsystem drivetrain, DoubleSupplier rotationSupplier, DoubleSupplier translationXSupplier, DoubleSupplier translationYSupplier, BooleanSupplier run, BooleanSupplier cancel1, BooleanSupplier cancel2){
m_drivetrain = drivetrain;
speedController = new PIDController(
AUTO_AIM_ROBOT_kP,
Expand All @@ -43,7 +44,8 @@ public AimRobotMoving(DrivetrainSubsystem drivetrain, DoubleSupplier rotationSup
this.translationXSupplier = translationXSupplier;
this.translationYSupplier = translationYSupplier;
this.rotationSupplier = rotationSupplier;
this.cancel = cancel;
this.cancel1 = cancel1;
this.cancel2 = cancel2;
this.run = run;
addRequirements(drivetrain);
}
Expand All @@ -61,7 +63,7 @@ public void execute() {
desiredRobotAngle = m_drivetrain.calculateSpeakerAngleMoving();
speedController.setSetpoint(desiredRobotAngle);
//move robot to desired angle
this.currentHeading = m_drivetrain.getGyroscopeRotation().getDegrees();
this.currentHeading = m_drivetrain.getPose().getRotation().getDegrees();
if(Math.abs(rotationSupplier.getAsDouble()) > 0.3) {
rotationSpeed = rotationSupplier.getAsDouble() * DriveConstants.MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND;
} else {
Expand All @@ -72,19 +74,19 @@ public void execute() {
} else {
allianceOffset = 0;
}
if(!cancel.getAsBoolean()) {
if(!cancel1.getAsBoolean()) {
m_drivetrain.drive(ChassisSpeeds.fromFieldRelativeSpeeds(
translationXSupplier.getAsDouble() * DriveConstants.MAX_VELOCITY_METERS_PER_SECOND,
translationYSupplier.getAsDouble() * DriveConstants.MAX_VELOCITY_METERS_PER_SECOND,
rotationSpeed,
new Rotation2d(m_drivetrain.getGyroscopeRotation().getRadians()+allianceOffset)));
new Rotation2d(m_drivetrain.getPose().getRotation().getRadians()+allianceOffset)));
}
// m_drivetrain.drive(new ChassisSpeeds(
// translationXSupplier.getAsDouble() * DriveConstants.MAX_VELOCITY_METERS_PER_SECOND,
// translationYSupplier.getAsDouble() * DriveConstants.MAX_VELOCITY_METERS_PER_SECOND,
// speedController.calculate(differenceAngle)));

SmartDashboard.putNumber("current Heading", m_drivetrain.getGyroscopeRotation().getDegrees()%360);
SmartDashboard.putNumber("current Heading", m_drivetrain.getPose().getRotation().getDegrees()%360);
SmartDashboard.putNumber("difference", differenceAngle);
SmartDashboard.putNumber("desired angle", desiredRobotAngle);
SmartDashboard.putNumber("PID calculated output", speedController.calculate(differenceAngle));
Expand All @@ -100,6 +102,6 @@ public void end(boolean interrupted) {
// Returns true when the command should end.
@Override
public boolean isFinished() {
return (!run.getAsBoolean() || cancel.getAsBoolean());
return (!run.getAsBoolean() || cancel1.getAsBoolean() || cancel2.getAsBoolean());
}
}
34 changes: 21 additions & 13 deletions src/main/java/frc/robot/commands/AimShooter.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,18 +14,18 @@ public class AimShooter extends Command {
DoubleSupplier POVSupplier;
BooleanSupplier humanPlayerSupplier;
BooleanSupplier SubwooferSupplier1;
BooleanSupplier SubwooferSupplier2;
BooleanSupplier SubwooferSupplier3;
BooleanSupplier StageAngleSupplier;
BooleanSupplier intakeSupplier;

public AimShooter(AngleShooterSubsystem angleShooterSubsystem, DoubleSupplier POVSupplier, BooleanSupplier humanPlayerSupplier,
BooleanSupplier SubwooferSupplier1, BooleanSupplier SubwooferSupplier2, BooleanSupplier SubwooferSupplier3) {
addRequirements(angleShooterSubsystem);
BooleanSupplier SubwooferSupplier1, BooleanSupplier StageAngleSupplier, BooleanSupplier intakeSup) {
this.angleShooterSubsystem = angleShooterSubsystem;
this.POVSupplier = POVSupplier;
this.humanPlayerSupplier = humanPlayerSupplier;
this.SubwooferSupplier1 = SubwooferSupplier1;
this.SubwooferSupplier2 = SubwooferSupplier2;
this.SubwooferSupplier3 = SubwooferSupplier3;
this.StageAngleSupplier = StageAngleSupplier;
this.intakeSupplier = intakeSup;
addRequirements(angleShooterSubsystem);
}

@Override
Expand All @@ -35,20 +35,28 @@ public void initialize() {

@Override
public void execute() {
if(SubwooferSupplier1.getAsBoolean()&&SubwooferSupplier2.getAsBoolean()&&SubwooferSupplier3.getAsBoolean()) {
if(SubwooferSupplier1.getAsBoolean()) {
angleShooterSubsystem.setDesiredShooterAngle(Field.SUBWOOFER_ANGLE);
} else {
if (POVSupplier.getAsDouble() == 90 || POVSupplier.getAsDouble() == 45 || POVSupplier.getAsDouble() == 135) {
angleShooterSubsystem.setDesiredShooterAngle(SmartDashboard.getNumber("amp angle", Field.AMPLIFIER_ANGLE)/*Field.AMPLIFIER_ANGLE*/);
if (StageAngleSupplier.getAsBoolean()) {
angleShooterSubsystem.setDesiredShooterAngle(Field.PODIUM_ANGLE);
} else {
if(humanPlayerSupplier.getAsBoolean()) {
angleShooterSubsystem.setDesiredShooterAngle(ShooterConstants.HUMAN_PLAYER_ANGLE);
} else {
angleShooterSubsystem.setDesiredShooterAngle(angleShooterSubsystem.calculateSpeakerAngle());
if (POVSupplier.getAsDouble() == 90 || POVSupplier.getAsDouble() == 45 || POVSupplier.getAsDouble() == 135) {
angleShooterSubsystem.setDesiredShooterAngle(SmartDashboard.getNumber("amp angle", Field.AMPLIFIER_ANGLE)/*Field.AMPLIFIER_ANGLE*/);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This SmartDashboard.getNumber call seems dangerous... Even if there is no longer an "amp angle" field on the SmartDashboard, it would put my mind at ease if we just changed the Field.AMPLIFIER_ANGLE to be the value you liked best.

If your testing earlier discovered that there was a particular angle that worked best, we need to make sure that the Field.AMPLIFIER_ANGLE constant gets updated appropriately, instead of leaving that change in the SmartDashboard widget.

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I agree we should change this,but to set ur mind at ease, when the Boolean is initialized on SmartDashboard (whenever robot code starts) it is set to whatever the constant is. So if you never change anything on SmartDashboard, you will just use the constant

} else {
if(humanPlayerSupplier.getAsBoolean()) {
angleShooterSubsystem.setDesiredShooterAngle(ShooterConstants.HUMAN_PLAYER_ANGLE);
} else {
if(intakeSupplier.getAsBoolean()) {
angleShooterSubsystem.setDesiredShooterAngle(ShooterConstants.GROUND_INTAKE_SHOOTER_ANGLE);
} else {
angleShooterSubsystem.setDesiredShooterAngle(angleShooterSubsystem.calculateSpeakerAngle());
}
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

A note on this whole execute function-- There's no reason to have the if/else statements increasingly nested... It just makes the code harder to read. This functionality is equivalent to:

if(SubwooferSupplier1.getAsBoolean()) {
...
}
else if(StageAngleSupplier.getAsBoolean()) {
...
}
else if(POVSupplier.getAsDouble() ...) {
...
}
else if(humanPlayerSupplier.getAsBoolean()) {
...
}
else if(intakeSupplier.getAsBoolean()) {
...
}
else {
...
}

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Your right... the nesting is just how i was taught to make if/else statements nested like that, but it is harder to read

}
}
}
}
}

@Override
public boolean isFinished() {
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/commands/Drive.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -50,7 +50,7 @@ public void execute() {
translationXSupplier.getAsDouble() * DriveConstants.MAX_VELOCITY_METERS_PER_SECOND * invert,
translationYSupplier.getAsDouble() * DriveConstants.MAX_VELOCITY_METERS_PER_SECOND * invert,
rotationSupplier.getAsDouble() * DriveConstants.MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND,
drivetrain.getGyroscopeRotation()
drivetrain.getPose().getRotation()
)
);
}
Expand Down
47 changes: 47 additions & 0 deletions src/main/java/frc/robot/commands/GroundIntake.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,47 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package frc.robot.commands;

import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.settings.Constants.IndexerConstants;
import frc.robot.settings.Constants.IntakeConstants;
import frc.robot.subsystems.IndexerSubsystem;
import frc.robot.subsystems.IntakeSubsystem;

public class GroundIntake extends Command {
/** Creates a new GroundIntake. */
IntakeSubsystem intake;
IndexerSubsystem indexer;
public GroundIntake(IntakeSubsystem intake, IndexerSubsystem indexer) {
this.intake = intake;
this.indexer = indexer;
addRequirements(intake, indexer);
// Use addRequirements() here to declare subsystem dependencies.
}

// Called when the command is initially scheduled.
@Override
public void initialize() {
intake.intakeYes(IntakeConstants.INTAKE_SPEED);
indexer.set(IndexerConstants.INDEXER_INTAKE_SPEED);
}

// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {}

// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
intake.intakeOff();
indexer.off();
}

// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
Loading
Loading