Skip to content

Commit

Permalink
removed aimbot and updated logic on intakecommand
Browse files Browse the repository at this point in the history
  • Loading branch information
@GearBoxFox
GearBoxFox committed Mar 1, 2024
1 parent f9a16ef commit bf2e2e8
Show file tree
Hide file tree
Showing 4 changed files with 46 additions and 110 deletions.
39 changes: 0 additions & 39 deletions src/main/java/frc/robot/commands/AimBotCommand.java
View file

This file was deleted.

6 changes: 3 additions & 3 deletions src/main/java/frc/robot/commands/DriveCommands.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,9 +39,9 @@ public static Command joystickDrive(
DoubleSupplier omegaSupplier) {
return Commands.run(
() -> {
double xInput = setSensitivity(xSupplier.getAsDouble(), 0.25);
double yInput = setSensitivity(ySupplier.getAsDouble(), 0.25);
double omegaInput = setSensitivity(omegaSupplier.getAsDouble(), 0.15);
double xInput = setSensitivity(xSupplier.getAsDouble(), 0.25);
double yInput = setSensitivity(ySupplier.getAsDouble(), 0.25);
double omegaInput = setSensitivity(omegaSupplier.getAsDouble(), 0.15);

// Apply deadband
double linearMagnitude =
Expand Down
20 changes: 16 additions & 4 deletions src/main/java/frc/robot/subsystems/shooter/ShooterIOKraken.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,6 @@
import lib.properties.phoenix6.Phoenix6PidPropertyBuilder;
import lib.properties.phoenix6.PidPropertyPublic;
import frc.robot.Constants.ShooterConstants;
import org.littletonrobotics.junction.Logger;

public class ShooterIOKraken implements ShooterIO {
private final TalonFX m_leftTalon;
Expand Down Expand Up @@ -148,9 +147,22 @@ public ShooterIOKraken() {
@Override
public void updateInputs(ShooterIOInputsAutoLogged inputs) {
BaseStatusSignal.refreshAll(
m_leftVelSignal,
m_rightVelSignal,
m_leftVoltOutSignal
m_leftVelSignal,
m_rightVelSignal,
m_leftVoltOutSignal,
m_rightVoltOutSignal,
m_kickerVoltOutSignal,
m_intakeVoltOutSignal,
m_indexerVoltOutSignal,
m_leftCurrentDrawSignal,
m_rightCurrentDrawSignal,
m_kickerCurrentDrawSignal,
m_intakeCurrentDrawSignal,
m_indexerCurrentDrawSignal,
m_leftTemperatureSignal,
m_rightTemperatureSignal,
m_intakeTemperatureSignal,
m_indexerTemperatureSignal
);

inputs.tlVelocityRPM = m_leftVelSignal.getValueAsDouble() * 60.0;
Expand Down
91 changes: 27 additions & 64 deletions src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java
View file
Original file line number Diff line number Diff line change
@@ -1,17 +1,11 @@
package frc.robot.subsystems.shooter;

import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation3d;
import com.ctre.phoenix6.SignalLogger;
import edu.wpi.first.units.Measure;
import edu.wpi.first.units.Voltage;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import lib.utils.AllianceFlipUtil;
import lib.utils.FieldConstants;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import org.littletonrobotics.junction.Logger;
import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
Expand All @@ -20,7 +14,8 @@

import static edu.wpi.first.units.BaseUnits.Voltage;
import static edu.wpi.first.units.MutableMeasure.mutable;
import static edu.wpi.first.units.Units.*;
import static edu.wpi.first.units.Units.RotationsPerSecond;
import static edu.wpi.first.units.Units.Volts;

public class ShooterSubsystem extends SubsystemBase {

Expand All @@ -30,53 +25,12 @@ public class ShooterSubsystem extends SubsystemBase {
private final LoggedDashboardNumber m_leftSetpoint;
private final LoggedDashboardNumber m_rightSetpoint;

private final SysIdRoutine m_sysIdLeft;
private final SysIdRoutine m_sysIdRight;

public ShooterSubsystem(ShooterIO io) {
m_io = io;
m_inputs = new ShooterIOInputsAutoLogged();

m_leftSetpoint = new LoggedDashboardNumber("Shooter/Left Flywheel Setpoint RPM");
m_rightSetpoint = new LoggedDashboardNumber("Shooter/Right Flywheel Setpoint RPM");

if (m_io.getClass() == ShooterIOKraken.class || true) {
m_sysIdLeft = new SysIdRoutine(
new SysIdRoutine.Config(null,
Voltage.of(9),
null,
(SysIdRoutineLog.State state) -> SignalLogger.writeString("shooter-left-state", state.toString())),
new SysIdRoutine.Mechanism(
(Measure<Voltage> volt) -> setShooterPowerLeft(volt.in(Volts) / 12.0),
null,
this));
m_sysIdRight = new SysIdRoutine(
new SysIdRoutine.Config(null,
Voltage.of(9),
null,
(SysIdRoutineLog.State state) -> SignalLogger.writeString("shooter-right-state", state.toString())),
new SysIdRoutine.Mechanism(
(Measure<Voltage> volt) -> setShooterPowerRight(volt.in(Volts) / 12.0),
null,
this));
} else {
m_sysIdLeft = new SysIdRoutine(
new SysIdRoutine.Config(null, Voltage.of(9), null),
new SysIdRoutine.Mechanism(
(Measure<Voltage> volt) -> setShooterPowerLeft(volt.in(Volts) / 12.0),
(SysIdRoutineLog log) -> log.motor("shooter-left")
.voltage(mutable(Volts.of(m_inputs.tlAppliedVolts)))
.angularVelocity(mutable(RotationsPerSecond.of(m_inputs.tlVelocityRPM * 60.0))),
this));
m_sysIdRight = new SysIdRoutine(
new SysIdRoutine.Config(null, Voltage.of(9), null),
new SysIdRoutine.Mechanism(
(Measure<Voltage> volt) -> setShooterPowerRight(volt.in(Volts) / 12.0),
(SysIdRoutineLog log) -> log.motor("shooter-right")
.voltage(mutable(Volts.of(m_inputs.trAppliedVolts)))
.angularVelocity(mutable(RotationsPerSecond.of(m_inputs.trVelocityRPM * 60.0))),
this));
}
}

@Override
Expand Down Expand Up @@ -114,6 +68,31 @@ public boolean hasPiece() {
return m_inputs.tofDistanceIn < 60;
}

/** Command Factories */

public Command intakeCommand(double intakePower, double kickerPower, double timeout) {
return (run(() -> {
setIntakePower(intakePower);
setKickerPower(kickerPower);
}).until(this::hasPiece)
.andThen(() -> {
setIntakePower(intakePower * 0.5);
setKickerPower(kickerPower);
})
.withTimeout(timeout)
.andThen(() -> setKickerPower(kickerPower * -0.25))
.withTimeout(timeout * 0.5)
.finallyDo(() -> {
setIntakePower(0.0);
setKickerPower(0.0);
})
)
.handleInterrupt(() -> {
setIntakePower(0.0);
setKickerPower(0.0);
});
}

public Command setShooterPowerFactory(double left, double right, double intake) {
return run(() -> {
setShooterPowerLeft(left);
Expand All @@ -122,21 +101,5 @@ public Command setShooterPowerFactory(double left, double right, double intake)
setIntakePower(intake);
});
}

public Command sysIdQuasistatic(SysIdRoutine.Direction direction, BooleanSupplier isLeft) {
if (isLeft.getAsBoolean()) {
return m_sysIdLeft.quasistatic(direction);
} else {
return m_sysIdRight.quasistatic(direction);
}
}

public Command sysIdDynamic(SysIdRoutine.Direction direction, BooleanSupplier isLeft) {
if (isLeft.getAsBoolean()) {
return m_sysIdLeft.dynamic(direction);
} else {
return m_sysIdRight.dynamic(direction);
}
}
}

0 comments on commit bf2e2e8

Please sign in to comment.