Skip to content

Commit

Permalink
complete refactoring of handoff to be up to date with branches
Browse files Browse the repository at this point in the history
The branches are as follows:
Shooting Calc Comand #33
Shooter Subystem #31
Trigger Subsystem #30
Claw Subsystem #29
Elevator Command #28
Elevator Subsystem #26
Pivot Subsystem #25
  • Loading branch information
Jacob1010-h committed Jan 23, 2024
1 parent 5fb4f3f commit 542fa93
Show file tree
Hide file tree
Showing 2 changed files with 33 additions and 13 deletions.
39 changes: 26 additions & 13 deletions src/main/java/frc/robot/commands/handoff/Handoff.java
Original file line number Diff line number Diff line change
@@ -1,59 +1,72 @@
package frc.robot.commands.handoff;

import java.util.function.BooleanSupplier;

import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.robot.subsystems.Elevator;
import frc.robot.subsystems.Intake;
import frc.robot.subsystems.TriggerWheel;
import frc.robot.subsystems.ElevatorSubs.Claw;
import frc.robot.subsystems.ElevatorSubs.Elevator;
import frc.robot.subsystems.shooter.Pivot;
import frc.robot.subsystems.shooter.Shooter;

public class Handoff {
private final Command emptyCommand = Commands.runOnce(() -> System.out.println(""));
private NotePosition notePosition;
private NotePosition currentNotePos;

private Intake intake;
private TriggerWheel trigger;

private Elevator elevator;
private Intake intake;
private Claw claw;

private Pivot pivot;
private Shooter shooter;


public Handoff() {
intake = new Intake();
trigger = new TriggerWheel();

elevator = new Elevator();
claw = new Claw();

pivot = new Pivot();
shooter = new Shooter();

this.notePosition = trigger.getHasPiece().getAsBoolean()
this.currentNotePos = trigger.hasPiece().getAsBoolean()
? NotePosition.TRIGGER
: NotePosition.NONE;
}

/**
* @return
*/
public NotePosition getNotePosition(){
return this.notePosition;
public NotePosition getCurrentNotePos(){
return this.currentNotePos;
}

/**
* @param notePosition
*/
public void setNotePosition(NotePosition notePosition) {
this.notePosition = notePosition;
public void setCurrentNotePos(NotePosition notePosition) {
this.currentNotePos = notePosition;
}

public void triggerReady() {
trigger.setIsReady(intake.hasGamePieceTrigger());
}

public Trigger readyToShoot() {
return new Trigger(trigger.getHasPiece());
return new Trigger(trigger.hasPiece());
}

public Command triggerToShooter() {
return Commands.either(trigger.shootToPivot(), emptyCommand, this.readyToShoot());
return Commands.either(trigger.toShooter(), emptyCommand, this.readyToShoot());
}

public Command triggerToElevator() {
return Commands.either(trigger.shootToElevator(), emptyCommand, this.readyToShoot());
return Commands.either(trigger.toTrap(), emptyCommand, this.readyToShoot());
}

}
7 changes: 7 additions & 0 deletions src/main/java/frc/robot/subsystems/TriggerWheel.java
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,8 @@
import com.revrobotics.CANSparkLowLevel.PeriodicFrame;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpilibj2.command.button.Trigger;

import java.util.function.BooleanSupplier;
import frc.robot.util.Constants.IntakeConstants;
import frc.robot.util.Neo;
Expand Down Expand Up @@ -57,4 +59,9 @@ public BooleanSupplier hasPiece() {
public void setHasPiece(boolean hasPiece) {
this.hasPiece = () -> hasPiece;
}

public void setIsReady(Trigger hasGamePieceTrigger) {
// TODO Auto-generated method stub
throw new UnsupportedOperationException("Unimplemented method 'setIsReady'");
}
}

0 comments on commit 542fa93

Please sign in to comment.