auto testing

FRC-Team-620 · Feb 24, 2024 · df70ae3 · df70ae3
1 parent 95d8d37
commit df70ae3
Show file tree

Hide file tree

Showing 3 changed files with 51 additions and 6 deletions.
diff --git a/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java b/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java
@@ -17,6 +17,7 @@
 import org.jmhsrobotics.frc2024.subsystems.drive.commands.DriveCommand;
 import org.jmhsrobotics.frc2024.subsystems.drive.commands.auto.DriveTimeCommand;
 import org.jmhsrobotics.frc2024.subsystems.intake.IntakeSubsystem;
+import org.jmhsrobotics.frc2024.subsystems.intake.commands.AutoIntakeCommand;
 import org.jmhsrobotics.frc2024.subsystems.intake.commands.DefaultIntakeCommand;
 import org.jmhsrobotics.frc2024.subsystems.intake.commands.ExtakeCommand;
 import org.jmhsrobotics.frc2024.subsystems.intake.commands.IntakeCommand;
@@ -73,7 +74,7 @@ public RobotContainer() {
 
 		// TODO: test these two comamnds individually before test any autos
 		SmartDashboard.putData("AutoIntakeCommand",
-				new IntakeCommand(0.5, this.intakeSubsystem, this.shooterSubsystem));
+				new AutoIntakeCommand(1, this.intakeSubsystem, this.shooterSubsystem));
 		SmartDashboard.putData("AutoShooterCommand", new ShooterAutoCommand(this.shooterSubsystem, 1));
 
 		// TODO: test this combo command after two commands above work as intended(lift
@@ -122,8 +123,8 @@ private void configureBindings() {
 		this.control.presetHigh().onTrue(new ArmSetAmpCommand(this.armSubsystem));
 		this.control.presetMid().onTrue(new ArmSetShootCommand(this.armSubsystem));
 		this.control.presetLow().onTrue(new ArmSetPickupCommand(this.armSubsystem));
-		this.control.intakeInput().whileTrue(new IntakeCommand(0.8, this.intakeSubsystem, this.shooterSubsystem));
-		this.control.extakeInput().whileTrue(new IntakeCommand(-0.8, this.intakeSubsystem, this.shooterSubsystem));
+		this.control.intakeInput().whileTrue(new IntakeCommand(1, this.intakeSubsystem, this.shooterSubsystem));
+		this.control.extakeInput().whileTrue(new IntakeCommand(-1, this.intakeSubsystem, this.shooterSubsystem));
 		this.control.shooterInput().whileTrue(new ShootOpenLoopCommand(80, shooterSubsystem));
 
 		// temp climber controls

diff --git a/src/main/java/org/jmhsrobotics/frc2024/autoCommands/FireCommand.java b/src/main/java/org/jmhsrobotics/frc2024/autoCommands/FireCommand.java
@@ -3,7 +3,7 @@
 import org.jmhsrobotics.frc2024.subsystems.intake.IntakeSubsystem;
 import org.jmhsrobotics.frc2024.subsystems.intake.commands.IntakeCommand;
 import org.jmhsrobotics.frc2024.subsystems.shooter.ShooterSubsystem;
-import org.jmhsrobotics.frc2024.subsystems.shooter.commands.ShooterAutoCommand;
+import org.jmhsrobotics.frc2024.subsystems.shooter.commands.ShootOpenLoopCommand;
 
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 
@@ -14,7 +14,7 @@ public class FireCommand extends SequentialCommandGroup {
 	public FireCommand(IntakeSubsystem intakeSubsystem, ShooterSubsystem shooterSubsystem) {
 		this.intakeSubsystem = intakeSubsystem;
 		this.shooterSubsystem = shooterSubsystem;
-		addCommands(new ShooterAutoCommand(this.shooterSubsystem, 1),
-				new IntakeCommand(1, this.intakeSubsystem, this.shooterSubsystem));
+		addCommands(new ShootOpenLoopCommand(80, this.shooterSubsystem).withTimeout(1),
+				new IntakeCommand(1, this.intakeSubsystem, this.shooterSubsystem).withTimeout(0.2));
 	}
 }
diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/intake/commands/AutoIntakeCommand.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/intake/commands/AutoIntakeCommand.java
@@ -0,0 +1,44 @@
+package org.jmhsrobotics.frc2024.subsystems.intake.commands;
+
+import org.jmhsrobotics.frc2024.subsystems.intake.IntakeSubsystem;
+import org.jmhsrobotics.frc2024.subsystems.shooter.ShooterSubsystem;
+
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class AutoIntakeCommand extends Command {
+
+	private IntakeSubsystem intakeSubsystem;
+	private ShooterSubsystem shooterSubsystem;
+
+	private double speed;
+
+	public AutoIntakeCommand(double speed, IntakeSubsystem intakeSubsystem, ShooterSubsystem shooterSubsystem) {
+		this.speed = speed;
+		this.intakeSubsystem = intakeSubsystem;
+		this.shooterSubsystem = shooterSubsystem;
+
+		addRequirements(this.intakeSubsystem);
+	}
+
+	@Override
+	public void initialize() {
+		this.intakeSubsystem.set(0);
+	}
+
+	@Override
+	public void execute() {
+		this.intakeSubsystem.set(this.speed);
+		this.shooterSubsystem.setSpeed(-.05);
+	}
+
+	@Override
+	public boolean isFinished() {
+		return this.intakeSubsystem.hasNote();
+		// return false;
+	}
+
+	@Override
+	public void end(boolean interrupted) {
+		this.intakeSubsystem.set(0);
+	}
+}