Skip to content

Commit

Permalink
Merge pull request #121 from DurhamAcademy/multi-step-autoaim
Browse files Browse the repository at this point in the history 
spinup and hood separate calls
  • Loading branch information
@Ryan-Bauroth
Ryan-Bauroth authored Apr 5, 2024
2 parents 7653f35 + b496c63 commit 15c6d50
Show file tree
Hide file tree
Showing 4 changed files with 72 additions and 19 deletions.
12 changes: 9 additions & 3 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -201,7 +201,7 @@ public String getCameraName() {
);
NamedCommands.registerCommand(
"Ready Shooter",
autoAim(shooter, drive, feeder)
autoAim(shooter, drive, feeder, () -> true)
);
NamedCommands.registerCommand(
"Zero Feeder",
Expand All @@ -213,7 +213,7 @@ public String getCameraName() {
);
NamedCommands.registerCommand(
"Auto Point",
ShooterCommands.autoAim(shooter, drive, feeder)
ShooterCommands.autoAim(shooter, drive, feeder, () -> true)
);
NamedCommands.registerCommand(
"Shoot When Ready",
Expand Down Expand Up @@ -348,13 +348,16 @@ private void configureButtonBindings() {

operatorController
.povLeft()
.and(operatorController.a().negate())
.and(operatorController.y().negate())
.whileTrue(
IntakeCommands.flushIntake(intake)
.alongWith(FeederCommands.flushFeeder(feeder))
);
operatorController
.povDown()
.and(operatorController.a().negate())
.and(operatorController.y().negate())
.whileTrue(
sequence(
parallel(
Expand All @@ -376,20 +379,23 @@ private void configureButtonBindings() {
operatorController
.povUp()
.and(operatorController.a())
.and(operatorController.y().negate())
.whileTrue(
LEDCommands.ledsUp(leds)
);
operatorController
.povDown()
.and(operatorController.a())
.and(operatorController.y().negate())
.whileTrue(
LEDCommands.ledsDown(leds)
);

// ---- SHOOTER COMMANDS ----
operatorController
.y()
.whileTrue(autoAim(shooter, drive, feeder));
.whileTrue(autoAim(shooter, drive, feeder, operatorController.povUp()));

operatorController
.x()
.whileTrue(
Expand Down
11 changes: 8 additions & 3 deletions src/main/java/frc/robot/commands/FeederCommands.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@ public static Command idleFeeder(Feeder feeder) {
zeroToBeamBreak(feeder)
),
feeder::getBeamBroken
)
).beforeStarting(()-> feeder.setState(Feeder.State.none))
);
}
public static Command feedToShooter(Feeder feeder) {
Expand All @@ -29,7 +29,9 @@ public static Command feedToShooter(Feeder feeder) {
run(() -> feeder.runVolts(9), feeder)
.withTimeout(0.5),
runOnce(() -> feeder.runVolts(0.0))
);
)
.beforeStarting(()->feeder.setState(Feeder.State.feedingShooter))
.finallyDo(interrupted -> feeder.setState(Feeder.State.none));
}

public static Command flushFeeder(Feeder feeder) {
Expand Down Expand Up @@ -58,7 +60,10 @@ public static Command feedToBeamBreak(Feeder feeder) {
feeder::getBeamBroken
)
),
feeder::getBeamBroken);
feeder::getBeamBroken)
.beforeStarting(()-> feeder.setState(Feeder.State.zeroingNote))
.finallyDo(interrupted -> feeder.setState(Feeder.State.none));

}

private static Command slowToBeam(Feeder feeder) {
Expand Down
50 changes: 37 additions & 13 deletions src/main/java/frc/robot/commands/ShooterCommands.java
View file
Original file line number Diff line number Diff line change
@@ -1,19 +1,24 @@
package frc.robot.commands;


import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.filter.Debouncer;
import edu.wpi.first.math.geometry.*;
import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
import edu.wpi.first.math.interpolation.InterpolatingTreeMap;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.ProfiledPIDCommand;
import frc.robot.subsystems.drive.Drive;
import frc.robot.subsystems.feeder.Feeder;
import frc.robot.subsystems.shooter.Shooter;
import org.littletonrobotics.junction.Logger;
import org.littletonrobotics.junction.networktables.LoggedDashboardBoolean;
import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;

import java.util.function.BooleanSupplier;

import static edu.wpi.first.units.Units.RadiansPerSecond;
import static edu.wpi.first.wpilibj.DriverStation.Alliance.Blue;
import static edu.wpi.first.wpilibj2.command.Commands.*;
Expand All @@ -31,6 +36,13 @@ public class ShooterCommands {
public static LoggedDashboardBoolean isOverridden = new LoggedDashboardBoolean("Override Hood Angle", false);
public static LoggedDashboardNumber overrideAngle = new LoggedDashboardNumber("Overridden Hood Angle", 0.0);

public static void periodic() {
flywheelSpeed.periodic();
isOverridden.periodic();
overrideAngle.periodic();
retractAfterShot.periodic();
}

public static double inverseInterpolate(Double startValue, Double endValue, Double q) {
double totalRange = endValue - startValue;
if (totalRange <= 0) {
Expand Down Expand Up @@ -69,10 +81,10 @@ public static void construct() {
distanceToAngle.put(1000.0, 0.0);

distanceToRPM.clear();
distanceToRPM.put(0.0, 3500.0);
distanceToRPM.put(0.894, 3500.0);
distanceToRPM.put(3.506, 5000.0);
distanceToRPM.put(0.0, 1250.0);
distanceToRPM.put(0.894, 1500.0);
distanceToRPM.put(2.52, 3750.0); //GOOD VALUES
distanceToRPM.put(3.506, 5000.0);
distanceToRPM.put(4.25, 4000.0);
distanceToRPM.put(1000.0,
4000.0);
Expand All @@ -81,18 +93,19 @@ public static void construct() {
public static LoggedDashboardBoolean retractAfterShot = new LoggedDashboardBoolean("Aim/Retract After Shooting", true);
public static LoggedDashboardNumber flywheelSpeed = new LoggedDashboardNumber("Aim/FlywheelSpeed", 3000);

public static Command autoAim(Shooter shooter, Drive drive, Feeder feeder) {
public static Command autoAim(Shooter shooter, Drive drive, Feeder feeder, BooleanSupplier hoodUp) {
construct();
flywheelSpeed.periodic();
isOverridden.periodic();
overrideAngle.periodic();
retractAfterShot.periodic();
// var a = new ProfiledPIDController(0.0,0.0,0.0,new TrapezoidProfile.Constraints(0,0));
// var b = new TrapezoidProfile(new TrapezoidProfile.Constraints(0,0));
// b.calculate(0,a.getSetpoint(), a.getGoal());
// b.totalTime();
final boolean[] lastFeeder = {feeder.getBeamBroken()};
//the parameter is the robot, idk how to declare it, also this returns the angle
return run(() -> {
if (isOverridden.get()) {
shooter.shooterRunVelocity(flywheelSpeed.get());
shooter.setTargetShooterAngle(Rotation2d.fromRadians(overrideAngle.get()));
shooter.overrideHoodAtSetpoint(true);
shooter.shooterRunVelocity(3000);
} else {
// Calcaulate new linear velocity
// Get the angle to point at the goal
Expand All @@ -110,11 +123,21 @@ public static Command autoAim(Shooter shooter, Drive drive, Feeder feeder) {
Logger.recordOutput("distanceFromGoal", distance);
Logger.recordOutput("Aim/getZ", targetRelativeToShooter.getZ());
Logger.recordOutput("Aim/atan", atan);
shooter.setTargetShooterAngle(Rotation2d.fromRadians(atan + distanceToAngle.get(distance)));
shooter.overrideHoodAtSetpoint(true);
shooter.shooterRunVelocity(distanceToRPM.get(distance));
if(hoodUp.getAsBoolean()) {
shooter.setTargetShooterAngle(Rotation2d.fromRadians(atan + distanceToAngle.get(distance)));
shooter.overrideHoodAtSetpoint(true);
} else {
shooter.setTargetShooterAngle(Rotation2d.fromRadians(IDLE_ANGLE));
shooter.overrideHoodAtSetpoint(false);
}
}
}, shooter)
// .until(()-> {
// var b = (feeder.getBeamBroken() != lastFeeder[0]);
// lastFeeder[0] = feeder.getBeamBroken();
// return feeder.getState() == Feeder.State.feedingShooter && b;
// })
.raceWith(SpecializedCommands.timeoutDuringAutoSim(2))
.withName("Auto Aim");
}
Expand All @@ -125,14 +148,15 @@ public static Command JustShoot(Shooter shooter) {
shooter.setTargetShooterAngle(Rotation2d.fromRadians(.8));
shooter.shooterRunVelocity(3500);
}, shooter)
.raceWith(SpecializedCommands.timeoutDuringAutoSim(2))
.withName("Just Shoot");
}

public static final double IDLE_ANGLE = 1.6;

public static Command shooterIdle(Shooter shooter) {
return run(() -> {
shooter.shooterRunVelocity(0.0);
shooter.setTargetShooterAngle(Rotation2d.fromRadians(1.6));
shooter.setTargetShooterAngle(Rotation2d.fromRadians(IDLE_ANGLE));
}, shooter)
.withName("Shooter Idle");
}
Expand Down
18 changes: 18 additions & 0 deletions src/main/java/frc/robot/subsystems/feeder/Feeder.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,24 @@ public class Feeder extends SubsystemBase {
private final ProfiledPIDController pidController;
double offset = 0.0;

private State state = State.none;

public enum State {
feedingShooter,
intaking,
zeroingNote,
humanPlayerIntake,
none
}

public State getState() {
return state;
}

public void setState(State state) {
this.state = state;
}

Debouncer debouncer = new Debouncer(.05, kBoth);
Debouncer debouncer2 = new Debouncer(.05, kBoth);

Expand Down

0 comments on commit 15c6d50

Please sign in to comment.