Skip to content

Commit

Permalink
ShootWhenReadyAuton and Container work
Browse files Browse the repository at this point in the history
  • Loading branch information
JediScoy committed Mar 31, 2024
1 parent 7f4eedd commit cd49f1d
Show file tree
Hide file tree
Showing 3 changed files with 20 additions and 36 deletions.
9 changes: 2 additions & 7 deletions src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -129,9 +129,8 @@ public RobotContainer() {
NamedCommands.registerCommand("ShootClose", new ShootClose(arm, index, intake, shooter));
NamedCommands.registerCommand("ShootFromStage", shootFromStage);
NamedCommands.registerCommand("Intake", new IntakeIndex(index, intake));
NamedCommands.registerCommand("AutoShootWhenReady", shootWhenReadyAuton); // Autonomous
NamedCommands.registerCommand("AutoShootWhenReady", shootWhenReadyAuton);
NamedCommands.registerCommand("SetFlywheelToIdle", setShooterIdle);
// Constants.ArmConstants.ARM_MID_POSE));

/*
* Auto Chooser
Expand Down Expand Up @@ -204,16 +203,13 @@ private void configureBindings() {
// TODO Test "setShooterVelocity" only, then remove
// m_driverController.rightTrigger().whileTrue(setShooterVelocity);

// TODO Testing "autonShootWhenReady" Remove later
m_driverController.leftTrigger().whileTrue(shootWhenReadyAuton);

// TODO Test MotionMagicVelocityVoltage" only, then remove
// m_driverController.leftTrigger().whileTrue(new InstantCommand(() -> shooter2.setFlywheelVelocityMM(shooter2.FLYWHEEL_VELOCITY)));

// TODO Remove comment out after testing the MotionMagic Velocity Voltage
// m_driverController.leftTrigger().whileTrue(new SetIndex(index, -0.75));

/* TODO These are the rrevious event commands and bindings */
/* TODO These are the previous event commands and bindings */
// m_driverController.rightTrigger().whileTrue(new RevAndShootCommand(index, shooter));
// m_driverController.rightTrigger().whileFalse(new InstantCommand(() -> shooter.SetOutput(0)));

Expand Down Expand Up @@ -243,7 +239,6 @@ public void DebugMethodSingle() {
// #endregion Testing
}


public Command getAutonomousCommand() {
return autoChooser.getSelected();
}
Expand Down
41 changes: 16 additions & 25 deletions src/main/java/frc/robot/commands/ShootWhenReadyAuton.java
Original file line number Diff line number Diff line change
Expand Up @@ -30,53 +30,44 @@ public void initialize() {

@Override
public void execute() {
/*
Turn on the shooter with setTargetFlywheelVelocity
After the flywheel is at target velocity, index the game piece forward
After index.HasCargo() is false, stop the shooter and index
*/


// Turn on the shooter with `setFlywheelVelocity`
shooter2.setFlywheelVelocity(shooter2.FLYWHEEL_VELOCITY);

// Check if the flywheel is at target velocity AND if a game piece is loaded
if (shooter2.isFlywheelNominal3() && notesensor.isNoteLoaded()) {

// If the flywheel is at target velocity AND a game piece is loaded, index the game piece forward
// If the flywheel is at target velocity AND a game piece is loaded, index the game piece forward into the flywheel
index.setPower(Constants.IndexConstants.INDEX_POWER);

/* If a game piece is not loaded,
* Set flywheel to idle speed
* Stop the indexer */
/*
} else if (!notesensor.isNoteLoaded()) {
index.setPower(0);
shooter2.setFlywheelVelocity(shooter2.FLYWHEEL_IDLE_VELOCITY);
*/

// Has to be called a second time
} else {
// Stop the indexer and continue to power the flywheel
index.stopIndexing();
shooter2.setFlywheelVelocity(shooter2.FLYWHEEL_VELOCITY);

} // end of if statement

}

@Override
public void end(boolean interrupted) {
/* After the game piece is launched, the setFlywheelVelocity should be 0 and the index should have power 0 */
shooter2.setFlywheelVelocity(0);
index.stopIndexing();
}

@Override
public boolean isFinished() {
// If a note is not loaded, i.e. it has been fired, then finish the command
if (!notesensor.isNoteLoaded()) {
index.setPower(0);
shooter2.setFlywheelVelocity(shooter2.FLYWHEEL_IDLE_VELOCITY);
return true;
}
// Note is still loaded, keep running the command
return false;
/*
// If a note is not loaded, i.e. it has been fired, then finish the command
if (!notesensor.isNoteLoaded()) {
index.setPower(0);
shooter2.setFlywheelVelocity(shooter2.FLYWHEEL_IDLE_VELOCITY);
return true;
}
// Note is still loaded, keep running the command
*/
return !notesensor.isNoteLoaded();
}

} // End of AutonShootWhenReady
6 changes: 2 additions & 4 deletions src/main/java/frc/robot/subsystems/Gyro.java
Original file line number Diff line number Diff line change
@@ -1,13 +1,11 @@
package frc.robot.subsystems;

import com.ctre.phoenix6.StatusSignal;
import com.ctre.phoenix6.configs.MountPoseConfigs;
import com.ctre.phoenix6.configs.Pigeon2Configuration;
import com.ctre.phoenix6.hardware.Pigeon2;

import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.util.Units;

@SuppressWarnings("unused")

public class Gyro {

Expand Down

0 comments on commit cd49f1d

Please sign in to comment.