diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index dc4a1673..d8c32e65 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,7 +11,7 @@ import frc.robot.commands.PowerIntake; import frc.robot.commands.PowerLinkage; import frc.robot.commands.SetIntake; -import frc.robot.commands.SetLinkageTalon; +import frc.robot.commands.SetLinkage; import frc.robot.commands.ShootInSpeaker; import frc.robot.commands.PowerFlywheel; import frc.robot.commands.RobotOrientedDrive; @@ -171,7 +171,7 @@ public RobotContainer() { private final void initializeCommands() { fieldOrientedDrive = new FieldOrientedDrive(drivetrain); robotOrientedDrive = new RobotOrientedDrive(drivetrain); - runExtendIntake = new RunExtendIntake(intake); + runExtendIntake = new RunExtendIntake(intake, linkage); powerIntakeReversed = new PowerIntakeReversed(intake); powerIntake = new PowerIntake(intake); powerFlywheel = new PowerFlywheel(flywheel); @@ -204,6 +204,7 @@ private void configureDefaultCommands() { // flywheel.setDefaultCommand(powerFlywheel); // drivetrain.setDefaultCommand(fieldOrientedDrive); climber.setDefaultCommand(powerClimber); + //intake.setDefaultCommand(powerIntake); } /** @@ -227,6 +228,7 @@ private void configureBindings() { operatorController.a().onTrue(levelClimbers); operatorController.b().onTrue(new InstantCommand(() -> climber.zeroBoth(), climber)); + operatorController.y().toggleOnTrue(runExtendIntake); // driverController.x().whileTrue(new InstantCommand(() -> // drivetrain.zero(),drivetrain)); @@ -241,6 +243,7 @@ public void configureCharacterizationBindings(){ driverController.y().whileTrue(drivetrain.sysIdRoutine.dynamic(SysIdRoutine.Direction.kReverse)); } public void onDisable() { + climber.stop(); flywheel.stop(); intake.stop(); linkage.stop(); diff --git a/src/main/java/frc/robot/commands/PowerIntake.java b/src/main/java/frc/robot/commands/PowerIntake.java index 892cfdd2..76a12bfc 100644 --- a/src/main/java/frc/robot/commands/PowerIntake.java +++ b/src/main/java/frc/robot/commands/PowerIntake.java @@ -28,13 +28,13 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - // if(operatorCont.getRightTriggerAxis() > .75) { - // intake.run(-.5); - // } else { - // intake.run(-.15); - // } + if(operatorCont.getRightTriggerAxis() > .75) { + intake.run(.5); + } else { + intake.run(.15); + } - intake.run(1.0); + // intake.run(1.0); CommandLogger.logCommandRunning(this); } diff --git a/src/main/java/frc/robot/commands/RunExtendIntake.java b/src/main/java/frc/robot/commands/RunExtendIntake.java index 2bfbbf7c..4fb40735 100644 --- a/src/main/java/frc/robot/commands/RunExtendIntake.java +++ b/src/main/java/frc/robot/commands/RunExtendIntake.java @@ -20,24 +20,28 @@ enum IntakeCases {CHECK_ROBOT_EMPTY, EXTEND_INTAKE, WAIT_FOR_SENSOR, UP_TO_SHOOT private static XboxController operatorCont = new XboxController(1); private Timer timer = new Timer(); private Timer sensorTimer = new Timer(); - private double setPoint; + private boolean isAuto; + private boolean spiked = false; private IntakeCases state = IntakeCases.CHECK_ROBOT_EMPTY; /** Creates a new Java. */ - public RunExtendIntake(Intake intake) { + public RunExtendIntake(Intake intake, Linkage linkage) { this.intake = intake; + this.linkage = linkage; // Use addRequirements() here to declare subsystem dependencies. - addRequirements(intake); + addRequirements(intake, linkage); } - // public RunExtendIntake(boolean isAuto) { - // inAuto = isAuto; - // // Use addRequirements() here to declare subsystem dependencies. - // addRequirements(intake); - // } + public RunExtendIntake(Intake intake, Linkage linkage, boolean isAuto) { + this.isAuto = isAuto; + this.intake = intake; + this.linkage = linkage; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(intake, linkage); + } // Called when the command is initially scheduled. @Override @@ -53,12 +57,14 @@ public void execute() { switch(state){ case CHECK_ROBOT_EMPTY: if(intake.getSideSensor()) { - state = IntakeCases.EXTEND_INTAKE; + state = IntakeCases.WAIT_FOR_SENSOR; } break; case EXTEND_INTAKE: - intake.run(.65); // we should extend too but idk how we should implement this - //linkage.setAngle(180); + intake.run(.7); // we should extend too but idk how we should implement this + if(isAuto) { + linkage.setAngle(0.0); // dunno if this value is right + } if(intake.getAmps() > 20 && timer.get() > .25) { sensorTimer.start(); state = IntakeCases.WAIT_FOR_SENSOR; @@ -69,7 +75,7 @@ public void execute() { // if(!intake.getHighSensor()) { // state = IntakeCases.UP_TO_SHOOTER_P1; // } - if(sensorTimer.get() > 1) { + if(sensorTimer.get() > 1.0) { state = IntakeCases.EXTEND_INTAKE; sensorTimer.reset(); } @@ -154,8 +160,11 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { if(state == IntakeCases.RETRACT_STOP) { + // dunno if this value is right System.out.print("COMMAND ENDED"); + if(linkage.isAtSetpoint()) { return true; + } } return false; } diff --git a/src/main/java/frc/robot/commands/SetLinkageTalon.java b/src/main/java/frc/robot/commands/SetLinkage.java similarity index 92% rename from src/main/java/frc/robot/commands/SetLinkageTalon.java rename to src/main/java/frc/robot/commands/SetLinkage.java index 85721d09..b75f4490 100644 --- a/src/main/java/frc/robot/commands/SetLinkageTalon.java +++ b/src/main/java/frc/robot/commands/SetLinkage.java @@ -8,11 +8,11 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Linkage; -public class SetLinkageTalon extends Command { +public class SetLinkage extends Command { final Linkage linkage; /** Creates a new SetLinkageTa\ * lon. */ - public SetLinkageTalon(Linkage linkage) { + public SetLinkage(Linkage linkage) { // Use addRequirements() here to declare subsystem dependencies. this.linkage = linkage; addRequirements(linkage); diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 09381ce9..b57e0e4d 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -64,6 +64,7 @@ public double getAmps() { public void periodic() { // This method will be called once per scheduler run io.updateInputs(inputs); + SmartDashboard.putNumber("Amps", getAmps()); Logger.processInputs("Intake", inputs); } } diff --git a/src/main/java/frc/robot/subsystems/Linkage.java b/src/main/java/frc/robot/subsystems/Linkage.java index dc320910..554c422f 100644 --- a/src/main/java/frc/robot/subsystems/Linkage.java +++ b/src/main/java/frc/robot/subsystems/Linkage.java @@ -43,7 +43,7 @@ public Linkage(LinkageIO io) { } public boolean isAtSetpoint() { - return false; + return Math.abs(this.getAngle()-this.positionSetpoint) < 1.0; } public void run(double speed) { diff --git a/src/main/java/frc/robot/utils/CommandFactory.java b/src/main/java/frc/robot/utils/CommandFactory.java index aa562ee7..f4881dac 100644 --- a/src/main/java/frc/robot/utils/CommandFactory.java +++ b/src/main/java/frc/robot/utils/CommandFactory.java @@ -50,7 +50,7 @@ public PowerLinkage powerLinkage() { // returns type runExtendIntake public RunExtendIntake runExtendIntake() { - return new RunExtendIntake(intake); + return new RunExtendIntake(intake, linkage); } // returns type runLinkage