Skip to content

Commit

Permalink
Devbot working
Browse files Browse the repository at this point in the history
  • Loading branch information
@suryatho
suryatho committed Feb 6, 2024
1 parent 851c32d commit be7b87f
Show file tree
Hide file tree
Showing 4 changed files with 70 additions and 45 deletions.
49 changes: 22 additions & 27 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import frc.robot.commands.DriveCommands;
import frc.robot.commands.FeedForwardCharacterization;
import frc.robot.subsystems.apriltagvision.AprilTagVision;
import frc.robot.subsystems.apriltagvision.AprilTagVisionConstants;
Expand Down Expand Up @@ -158,19 +159,15 @@ public RobotContainer() {
autoChooser.addOption(
"Left Flywheels FF Characterization",
new FeedForwardCharacterization(
shooter,
shooter::runLeftCharacterizationVolts,
shooter::getLeftCharacterizationVelocity)
.beforeStarting(() -> shooter.setCharacterizing(true))
.finallyDo(() -> shooter.setCharacterizing(false)));
shooter,
shooter::runLeftCharacterizationVolts,
shooter::getLeftCharacterizationVelocity));
autoChooser.addOption(
"Right Flywheels FF Characterization",
new FeedForwardCharacterization(
shooter,
shooter::runRightCharacterizationVolts,
shooter::getRightCharacterizationVelocity)
.beforeStarting(() -> shooter.setCharacterizing(true))
.finallyDo(() -> shooter.setCharacterizing(false)));
shooter,
shooter::runRightCharacterizationVolts,
shooter::getRightCharacterizationVelocity));
autoChooser.addOption(
"Arm Quasistatic Forward", arm.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
autoChooser.addOption("Arm Dynamic Forward", arm.sysIdDynamic(SysIdRoutine.Direction.kForward));
Expand Down Expand Up @@ -204,23 +201,21 @@ public RobotContainer() {
* edu.wpi.first.wpilibj2.command.button.JoystickButton}.
*/
private void configureButtonBindings() {
// drive.setDefaultCommand(
// DriveCommands.joystickDrive(
// drive,
// () -> -controller.getLeftY(),
// () -> -controller.getLeftX(),
// () -> -controller.getRightX()));
// controller.a().onTrue(DriveCommands.toggleCalculateShotWhileMovingRotation(drive));

// arm.setDefaultCommand(arm.runToSetpoint());
// controller
// .a()
// .onTrue(Commands.either(intake.stopCommand(), intake.intakeCommand(),
// intake::running));
// controller
// .x()
// .onTrue(Commands.either(intake.stopCommand(), intake.ejectCommand(),
// intake::running));
drive.setDefaultCommand(
DriveCommands.joystickDrive(
drive,
() -> -controller.getLeftY(),
() -> -controller.getLeftX(),
() -> -controller.getRightX()));
controller
.x()
.onTrue(Commands.runOnce(shooter::setShooting))
.onFalse(Commands.runOnce(shooter::setStop));
controller
.a()
.onTrue(Commands.runOnce(shooter::setIntaking))
.onFalse(Commands.runOnce(shooter::setStop));

controller
.b()
.onTrue(
Expand Down
11 changes: 6 additions & 5 deletions src/main/java/frc/robot/subsystems/drive/Drive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -175,16 +175,17 @@ public void periodic() {
// account for skew
desiredSpeeds = ChassisSpeeds.discretize(desiredSpeeds, 0.02);
// generate feasible next setpoint
currentSetpoint =
setpointGenerator.generateSetpoint(
currentModuleLimits, currentSetpoint, desiredSpeeds, 0.02);
// currentSetpoint =
// setpointGenerator.generateSetpoint(
// currentModuleLimits, currentSetpoint, desiredSpeeds, 0.02);

// run modules
SwerveModuleState[] optimizedSetpointStates = new SwerveModuleState[4];
SwerveModuleState[] optimizedSetpointStates =
DriveConstants.kinematics.toSwerveModuleStates(desiredSpeeds);
for (int i = 0; i < modules.length; i++) {
// Optimize setpoints
optimizedSetpointStates[i] =
SwerveModuleState.optimize(currentSetpoint.moduleStates()[i], modules[i].getAngle());
SwerveModuleState.optimize(optimizedSetpointStates[i], modules[i].getAngle());
modules[i].runSetpoint(optimizedSetpointStates[i]);
}

Expand Down
6 changes: 3 additions & 3 deletions src/main/java/frc/robot/subsystems/superstructure/SuperstructureConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,17 +14,17 @@ public static class ShooterConstants {

public static FlywheelConstants leftFlywheelConstants =
switch (Constants.getRobot()) {
default -> new FlywheelConstants(2, false, 0.0, 0.0, 0.0, 0.33329, 0.00083, 0.0);
default -> new FlywheelConstants(5, false, 0.0, 0.0, 0.0, 0.33329, 0.00083, 0.0);
};

public static FlywheelConstants rightFlywheelConstants =
switch (Constants.getRobot()) {
default -> new FlywheelConstants(1, false, 0.0, 0.0, 0.0, 0.33329, 0.00083, 0.0);
default -> new FlywheelConstants(4, false, 0.0, 0.0, 0.0, 0.33329, 0.00083, 0.0);
};

public static FeederConstants feederConstants =
switch (Constants.getRobot()) {
default -> new FeederConstants(3, false);
default -> new FeederConstants(6, false);
};

public record FlywheelConstants(
Expand Down
49 changes: 39 additions & 10 deletions src/main/java/frc/robot/subsystems/superstructure/shooter/Shooter.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,8 @@ public class Shooter extends SubsystemBase {
new LoggedTunableNumber("Shooter/rightkA", rightFlywheelConstants.kA());
private static final LoggedTunableNumber shooterTolerance =
new LoggedTunableNumber("Shooter/ToleranceRPM", shooterToleranceRPM);
private static final LoggedTunableNumber intakeVolts =
new LoggedTunableNumber("Shooter/IntakeVolts", -5.0);
private final LoggedDashboardNumber leftSpeedRpm =
new LoggedDashboardNumber("Left Speed RPM", 6000);
private final LoggedDashboardNumber rightSpeedRpm =
Expand All @@ -46,7 +48,10 @@ public class Shooter extends SubsystemBase {
private final ShooterIO shooterIO;
private final ShooterIOInputsAutoLogged shooterInputs = new ShooterIOInputsAutoLogged();

private double characterizationVolts = 0.0;
private boolean characterizing = false;
private boolean shooting = false;
private boolean intaking = false;

public Shooter(ShooterIO io) {
System.out.println("[Init] Creating Shooter");
Expand Down Expand Up @@ -92,14 +97,22 @@ public void periodic() {
shooterIO.stop();
} else {
if (!characterizing) {
shooterIO.setRPM(leftSpeedRpm.get(), rightSpeedRpm.get());
double feederSetpointVolts = 0.0;
if (leftSpeedRpm.get() > 0 && rightSpeedRpm.get() > 0 && atSetpoint()) {
feederSetpointVolts = feedVolts.get();
if (shooting) {
shooterIO.setRPM(leftSpeedRpm.get(), rightSpeedRpm.get());
double feederSetpointVolts = 0.0;
if (leftSpeedRpm.get() > 0 && rightSpeedRpm.get() > 0 && atSetpoint()) {
feederSetpointVolts = feedVolts.get();
} else {
feederSetpointVolts = 0;
}
shooterIO.setFeederVoltage(feederSetpointVolts);
} else if (intaking) {
shooterIO.setLeftCharacterizationVoltage(intakeVolts.get());
shooterIO.setRightCharacterizationVoltage(intakeVolts.get());
shooterIO.setFeederVoltage(intakeVolts.get());
} else {
feederSetpointVolts = 0;
shooterIO.stop();
}
shooterIO.setFeederVoltage(feederSetpointVolts);
}
}

Expand All @@ -108,22 +121,38 @@ public void periodic() {
Logger.recordOutput("Shooter/FeederRPM", shooterInputs.feederVelocityRPM);
}

public void setIntaking() {
characterizing = false;
shooting = false;
intaking = true;
}

public void setShooting() {
characterizing = false;
shooting = true;
intaking = false;
}

public void setStop() {
characterizing = false;
shooting = false;
intaking = false;
}

public void runLeftCharacterizationVolts(double volts) {
characterizing = true;
shooterIO.setLeftCharacterizationVoltage(volts);
}

public void runRightCharacterizationVolts(double volts) {
characterizing = true;
shooterIO.setRightCharacterizationVoltage(volts);
}

public double getLeftCharacterizationVelocity() {
return shooterInputs.leftFlywheelVelocityRPM;
}

public void setCharacterizing(boolean characterizing) {
this.characterizing = characterizing;
}

public double getRightCharacterizationVelocity() {
return shooterInputs.rightFlywheelVelocityRPM;
}
Expand Down

0 comments on commit be7b87f

Please sign in to comment.