Skip to content

Commit

Permalink
Works fully, ready for outreach
Browse files Browse the repository at this point in the history
  • Loading branch information
@RileyWeezer
RileyWeezer committed May 8, 2024
1 parent e310d18 commit b3b3335
Show file tree
Hide file tree
Showing 3 changed files with 25 additions and 163 deletions.
2 changes: 1 addition & 1 deletion src/main/deploy/resources/intake/intake.json
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
{
"intakeVoltage": 1,
"intakeVoltage": 0.6,
"sensorTrigger": 200,
"motorConfig": {
"port" : 9,
Expand Down
166 changes: 14 additions & 152 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,7 @@
import edu.wpi.first.wpilibj.I2C.Port;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
Expand Down Expand Up @@ -254,173 +255,34 @@ private void registerNamedCommands() {
}

private void configureDefaultCommands() {
mSwerveDrive.setDefaultCommand(
new DriveCommand(mSwerveDrive, dController::getLeftY, dController::getLeftX, dController::getRightX));

mShooter.setDefaultCommand(Commands.run(() -> {mShooter.goToSpecifiedAngle(50);}, mShooter));
// mShooter.setDefaultCommand(Commands.run(() -> mShooter.linearActuatorSetVoltage(mController.getLeftY()*-12), mShooter));

// mShooter.setDefaultCommand(Commands.run(() -> {mShooter.goToSpecifiedAngle(30);}, mShooter));
// mShooter.setDefaultCommand(
// Commands.run(() -> mShooter.linearActuatorSetVoltage(mController.getLeftY() *
// -12), mShooter));
mShooter.setDefaultCommand(Commands.run(() -> mShooter.linearActuatorSetVoltage(mController.getLeftY()*-12), mShooter));

}

private void configureBindings() {
dController.leftBumper().onTrue(Commands.runOnce(() -> mSwerveDrive.setFieldCentric(false), mSwerveDrive));
dController.rightBumper().onTrue(Commands.runOnce(() -> mSwerveDrive.setFieldCentric(true), mSwerveDrive));
dController.start().onTrue(Commands.runOnce(() -> mSwerveDrive.resetHeading(), mSwerveDrive));

dController.leftTrigger().onTrue(
new Uppies(leftClimber, rightClimber)).onFalse(
new ParallelCommandGroup(Commands.runOnce(() -> leftClimber.stopMotor(),
leftClimber),
Commands.runOnce(() -> rightClimber.stopMotor(), rightClimber)));

dController.rightTrigger().onTrue(new Climb(leftClimber, rightClimber));


mController.a().onTrue(new AutoStop(mWrist,
mIntake)).onFalse(Commands.runOnce(() -> {
mWrist.setDesiredPosition(WristPosition.REST);
mIntake.stop();
}, mWrist, mIntake));

mController.b().onTrue(Commands.sequence(
new RotateToAprilTag(mSwerveDrive),
Commands.runOnce(() -> {
mIntake.intake();
mDelivery.toShooter();
}))).onFalse(Commands.runOnce(() -> {
mIntake.stop();
mDelivery.stop();
}));

mController.x().onTrue(new ShooterSequence(mShooter, mDelivery, mIntake, mWrist, mSwerveDrive))
.onFalse(Commands.runOnce(() -> {
mIntake.stop();
mDelivery.stop();
mShooter.linearActuatorSetVoltage(0);
mShooter.stopFlyWheel();
}, mIntake, mDelivery, mShooter));

mController.y().onTrue(Commands.run(() -> {
LimelightHelpers.setPipelineIndex("", LimeLightPipelines.SPEAKER.id);
LimelightHelpers.takeSnapshot("", "Y button " + LimelightHelpers.getTY(""));
mShooter.goToAngle();
mShooter.spinUpFlyWheel();
}, mShooter)).onFalse(Commands.runOnce(() -> {
mShooter.stopFlyWheel();
LimelightHelpers.setPipelineIndex("", LimeLightPipelines.MEGATAG.id);
}, mShooter));

mController.rightStick().onTrue(Commands.waitUntil(mWrist::atSetpoint).andThen(Commands.run(() -> {
mController.a().onTrue(Commands.run(() -> {
mIntake.intake();
mDelivery.toShooter();
}, mDelivery, mIntake).until(mDelivery::shooterHasNote).andThen(Commands.runOnce(() -> {
mIntake.stop();
mDelivery.stop();
}, mIntake, mDelivery)))).onFalse(Commands.runOnce(() -> {
}, mIntake, mDelivery).until(mDelivery::shooterHasNote).andThen(
Commands.runOnce(() -> {
mIntake.stop();
mDelivery.stop();
}, mIntake, mDelivery)
)).onFalse(Commands.runOnce(() -> {
mIntake.stop();
mDelivery.stop();
}, mIntake, mDelivery));

mController.povUp().onTrue(Commands.runOnce(() -> {
mWrist.setDesiredPosition(WristPosition.AMP);
mArm.setDesiredPosition(ArmPosition.AMP);
}, mWrist, mArm));

mController.povDown().onTrue(Commands.sequence(
Commands.runOnce(() -> {
mArm.setDesiredPosition(ArmPosition.REST);
}, mArm),
Commands.waitUntil(mArm::isAtSetpoint).andThen(() -> {
mWrist.setDesiredPosition(WristPosition.REST);
})));

mController.povLeft().onTrue(Commands.sequence(
Commands.run(() -> mShooter.goToSpecifiedAngle(26), mShooter).until(mShooter::isIntakeAble),
Commands.run(() -> {
mDelivery.toIntake();
mIntake.outtake();
}, mDelivery, mIntake).until(mIntake::hasNote),
Commands.runOnce(() -> {
mDelivery.stop();
mIntake.stop();
}, mDelivery, mIntake))).onFalse(Commands.runOnce(() -> {
mDelivery.stop();
mIntake.stop();
}, mDelivery, mIntake));

mController.povRight().onTrue(Commands.run(
() -> mShooter.spinToRpm(2800), mShooter)).onFalse(Commands.runOnce(
() -> mShooter.stopFlyWheel(), mShooter));


mController.start().onTrue(Commands.sequence(
Commands.runOnce(() -> mWrist.setDesiredPosition(WristPosition.AMP), mWrist),
Commands.waitUntil(mWrist::atSetpoint),
Commands.run(() -> mIntake.outtake(), mIntake).withTimeout(0.5),
Commands.runOnce(() -> mIntake.stop(), mIntake)
)).onFalse(Commands.runOnce(() -> {
mWrist.setDesiredPosition(WristPosition.REST);
mIntake.stop();
}, mWrist, mIntake));

mController.leftTrigger().onTrue(Commands.run(() -> {
LimelightHelpers.setPipelineIndex("", LimeLightPipelines.SPEAKER.id);
LimelightHelpers.takeSnapshot("", "Left Trigger button " + LimelightHelpers.getTY(""));
mShooter.goToSpecifiedAngle(60);
mShooter.spinUpFlyWheel();
mController.x().onTrue(Commands.run(() -> {
mShooter.spinToSetRPM();
}, mShooter)).onFalse(Commands.runOnce(() -> {
mShooter.stopFlyWheel();
LimelightHelpers.setPipelineIndex("", LimeLightPipelines.MEGATAG.id);
}, mShooter));

mController.rightTrigger().onTrue(Commands.run(() -> {
LimelightHelpers.setPipelineIndex("", LimeLightPipelines.SPEAKER.id);
LimelightHelpers.takeSnapshot("", "Right Trigger button " + LimelightHelpers.getTY(""));
mShooter.goToSpecifiedAngle(35);
mShooter.spinUpFlyWheel();
}, mShooter)).onFalse(Commands.runOnce(() -> {
mShooter.stopFlyWheel();
LimelightHelpers.setPipelineIndex("", LimeLightPipelines.MEGATAG.id);
}, mShooter));
// mController.leftTrigger().onTrue(Commands.sequence(
// Commands.run(() -> mShooter.goToSpecifiedAngle(44.5)).until(mShooter::isAtAngle),
// Commands.run(() -> mShooter.spinUpFlyWheel(), mShooter).until(mShooter::isAtShootingSpeed),
// Commands.run(() -> {mDelivery.toShooter(); mIntake.intake();}, mDelivery, mIntake).withTimeout(1)
// )).onFalse(Commands.runOnce(() -> {
// mShooter.stopFlyWheel();
// mDelivery.stop();
// mIntake.intake();
// }, mShooter, mDelivery, mIntake));
// mController.rightTrigger().onTrue(new Climb(leftClimber, rightClimber).withTimeout(5));

// mController.back().whileTrue(new ParallelCommandGroup(Commands.run(() -> leftClimber.stopMotor(), leftClimber),
// Commands.run(() -> rightClimber.stopMotor(), rightClimber)));

// mController.leftTrigger().onTrue(
// new Uppies(leftClimber, rightClimber)).onFalse(
// new ParallelCommandGroup(Commands.runOnce(() -> leftClimber.stopMotor(), leftClimber),
// Commands.runOnce(() -> rightClimber.stopMotor(), rightClimber)));

mController.leftBumper().onTrue(Commands.runOnce(() -> {
mDelivery.toShooter();
mIntake.intake();
}, mDelivery)).onFalse(Commands.runOnce(() -> {
mDelivery.stop();
mIntake.stop();
}, mDelivery, mIntake));

mController.rightBumper().onTrue(Commands.runOnce(() -> {
mDelivery.toIntake();
mIntake.outtake();
}, mDelivery)).onFalse(Commands.runOnce(() -> {
mDelivery.stop();
mIntake.stop();
}, mDelivery, mIntake));

mController.rightBumper().onTrue(Commands.runOnce(() -> mDelivery.toShooter(), mDelivery)).onFalse(
Commands.runOnce(() -> mDelivery.stop(), mDelivery));
}

public Command getAutonomousCommand() {
Expand Down
20 changes: 10 additions & 10 deletions src/main/java/frc/robot/subsystems/Shooter.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@
import edu.wpi.first.math.controller.ArmFeedforward;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.networktables.GenericEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
Expand All @@ -34,6 +35,7 @@ public class Shooter extends SubsystemBase {
private double angleCalcA;
private double angleCalcB;
private double angleCalcC;
private GenericEntry manualRPMTarget;

private double rpmCalcA;
private double rpmCalcB;
Expand Down Expand Up @@ -121,6 +123,7 @@ public Shooter(ShooterConfig config, SOTA_MotorController linearActuator, SOTA_A
Shuffleboard.getTab("Shooter").addDouble("Linear Actuator Voltage", linearActuator::getMotorCurrent);
Shuffleboard.getTab("Shooter").addBoolean("At Angle", this::isAtAngle);
Shuffleboard.getTab("Shooter").addBoolean("Intakeable", this::isIntakeAble);
manualRPMTarget = Shuffleboard.getTab("Outreach").add("TargetRPM", 0).getEntry();
}

public double getCorrectedEncoderPosition() {
Expand All @@ -145,16 +148,9 @@ public double calcTargetRpm() {
}

public void linearActuatorSetVoltage(double volts) {
if (getCorrectedEncoderPosition() > maxLinearValue && volts > 0) {
linearActuator.stopMotor();
} else if (getCorrectedEncoderPosition() < minLinearValue && volts < 0) {
linearActuator.stopMotor();
} else if (linearPID.atSetpoint()) {
linearActuator.stopMotor();
} else {
linearActuator.setVoltage(volts);
}
// linearActuator.setVoltage(volts);

linearActuator.setVoltage(volts);

}

public void spinUpFlyWheel() {
Expand All @@ -168,6 +164,10 @@ public void spinToRpm(int rpm) {
rightShooter.setVoltage(rightRPMToVolts(rpm));
}

public void spinToSetRPM() {
spinToRpm((int) manualRPMTarget.getInteger(0));
}

public void stopFlyWheel() {
leftShooter.stopMotor();
rightShooter.stopMotor();
Expand Down

0 comments on commit b3b3335

Please sign in to comment.