Skip to content

Commit

Permalink
New indexer logic just dropped
Browse files Browse the repository at this point in the history
  • Loading branch information
@keckothedragon
keckothedragon committed Oct 14, 2024
1 parent 134347d commit d128ffc
Show file tree
Hide file tree
Showing 13 changed files with 220 additions and 125 deletions.
39 changes: 20 additions & 19 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -132,28 +132,28 @@ public static final class IntakeConstants {
}

public static final class IndexerConstants {
public static final LoggedTunableNumber kIdleVoltage =
new LoggedTunableNumber("Indexer Idle Voltage", 0.0);
public static final LoggedTunableNumber kIntakingVoltage =
new LoggedTunableNumber("Indexer Intake Voltage", -8.0);
public static final LoggedTunableNumber kIndexingVoltage =
new LoggedTunableNumber("Indexer Intake Voltage", -8.0);
public static final LoggedTunableNumber kReversingVoltage =
new LoggedTunableNumber("Indexer Reverse Voltage", 8.0);
public static final LoggedTunableNumber kShootingVoltage =
new LoggedTunableNumber("Indexer Shooting Voltage", -10.0);
public static final LoggedTunableNumber kEjectingVoltage =
new LoggedTunableNumber("Indexer Eject Voltage", 8.0);
public static final LoggedTunableNumber kIntakingVelocity =
new LoggedTunableNumber("Indexer Intake Velocity", -8.0);
public static final LoggedTunableNumber kIndexingVelocity =
new LoggedTunableNumber("Indexer Indexing Velocity", -8.0);
public static final LoggedTunableNumber kReversingVelocity =
new LoggedTunableNumber("Indexer Reverse Velocity", 8.0);
public static final LoggedTunableNumber kShootingVelocity =
new LoggedTunableNumber("Indexer Shooting Velocity", -10.0);
public static final LoggedTunableNumber kEjectingVelocity =
new LoggedTunableNumber("Indexer Eject Velocity", 8.0);

public static final LoggedTunableNumber kShootingTimeout =
new LoggedTunableNumber("Indexer Shooting Timeout", 1.0);
public static final LoggedTunableNumber kIntakeTimeout =
new LoggedTunableNumber("Indexer Shooting Timeout", 1.0);
public static final LoggedTunableNumber kIndexingTimeout =
new LoggedTunableNumber("Indexer Indexing Timeout", 0.1);
public static final LoggedTunableNumber kReverseTimeout =
new LoggedTunableNumber("Indexer Reverse Timeout", 0.5);

public static final LoggedTunableNumber kIndexerP = new LoggedTunableNumber("Indexer P", 0.4);
public static final LoggedTunableNumber kIndexerI = new LoggedTunableNumber("Indexer I", 0.0);
public static final LoggedTunableNumber kIndexerD = new LoggedTunableNumber("Indexer D", 0.0);

// Simulation constants
public static final DCMotor kSimGearbox = DCMotor.getNEO(1);
public static final double kSimGearing = 1.0;
Expand All @@ -167,14 +167,15 @@ public static final class ShooterConstants {
new LoggedTunableNumber("Shooter Idle Voltage", 0.0);
public static final LoggedTunableNumber kEjectingVoltage =
new LoggedTunableNumber("Shooter Ejecting Voltage", 7.0);
public static final LoggedTunableNumber kP = new LoggedTunableNumber("Flywheel P", 0.03);
public static final LoggedTunableNumber kI = new LoggedTunableNumber("Flywheel I", 0.0);
public static final LoggedTunableNumber kD = new LoggedTunableNumber("Flywheel D", 0.0);
public static final LoggedTunableNumber kFlywheelP =
new LoggedTunableNumber("Flywheel P", 0.03);
public static final LoggedTunableNumber kFlywheelI = new LoggedTunableNumber("Flywheel I", 0.0);
public static final LoggedTunableNumber kFlywheelD = new LoggedTunableNumber("Flywheel D", 0.0);

public static final PIDController kTopController =
new PIDController(kP.get(), kI.get(), kD.get());
new PIDController(kFlywheelP.get(), kFlywheelI.get(), kFlywheelD.get());
public static final PIDController kBottomController =
new PIDController(kP.get(), kI.get(), kD.get());
new PIDController(kFlywheelP.get(), kFlywheelI.get(), kFlywheelD.get());

public static final LoggedTunableNumber kTopKS =
new LoggedTunableNumber("Top Flywheel kS", 0.12);
Expand Down
5 changes: 5 additions & 0 deletions src/main/java/frc/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@
package frc.robot;

import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import org.littletonrobotics.junction.LoggedRobot;
Expand Down Expand Up @@ -84,8 +85,12 @@ public void robotPeriodic() {
// finished or interrupted commands, and running subsystem periodic() methods.
// This must be called from the robot's periodic block in order for anything in
// the Command-based framework to work.
var start = Timer.getFPGATimestamp();

CommandScheduler.getInstance().run();

Logger.recordOutput("PeriodicTime/CommandScheduler", Timer.getFPGATimestamp() - start);

robotContainer.updateRobotState();
}

Expand Down
76 changes: 13 additions & 63 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -191,19 +191,27 @@ private void configureButtonBindings() {

m_driverControls
.runIntake()
.whileTrue(
Commands.run(
.or(m_operatorControls.runIntake())
.onTrue(
Commands.runOnce(
() -> {
m_robotState.updateRobotAction(RobotAction.kIntake);
}))
.onFalse(
Commands.runOnce(
() -> {
m_robotState.setDefaultAction();

// advance to indexing if still intaking
// otherwise it's moved on alr so no need to change state
if (m_indexer.getState() == IndexerState.kIntaking) {
m_indexer.updateState(IndexerState.kIndexing);
}
}));

m_driverControls
.runIndexer()
.or(m_operatorControls.runIndexer())
.onTrue(
Commands.runOnce(
() -> {
Expand All @@ -225,6 +233,7 @@ private void configureButtonBindings() {

m_driverControls
.ejectGamePiece()
.or(m_operatorControls.ejectGamePiece())
.onTrue(
Commands.runOnce(
() -> {
Expand All @@ -239,6 +248,7 @@ private void configureButtonBindings() {

m_driverControls
.revShooter()
.or(m_operatorControls.revAndAlign())
.onTrue(
Commands.runOnce(
() -> {
Expand All @@ -265,6 +275,7 @@ private void configureButtonBindings() {

m_driverControls
.amp()
.or(m_operatorControls.amp())
.onTrue(
Commands.runOnce(
() -> {
Expand Down Expand Up @@ -299,54 +310,6 @@ private void configureButtonBindings() {
m_robotState.setDefaultAction();
}));

m_operatorControls
.runIntake()
.whileTrue(
Commands.run(
() -> {
m_robotState.updateRobotAction(RobotAction.kIntake);
}))
.onFalse(
Commands.runOnce(
() -> {
m_robotState.setDefaultAction();
}));

m_operatorControls
.runIndexer()
.whileTrue(Commands.run(() -> m_indexer.updateState(IndexerState.kShooting)));

m_operatorControls
.ejectGamePiece()
.onTrue(
Commands.runOnce(
() -> {
m_robotState.updateRobotAction(RobotAction.kVomitting);
}))
.onFalse(
Commands.runOnce(
() -> {
m_robotState.setDefaultAction();
m_indexer.updateState(IndexerState.kIdle);
}));

m_operatorControls
.revAndAlign()
.onTrue(
Commands.runOnce(
() -> {
m_robotState.updateRobotAction(RobotAction.kRevAndAlign);
}))
.onFalse(
Commands.runOnce(
() -> {
m_robotState.setDefaultAction();
}));

m_operatorControls
.revAndShoot()
.onTrue(Commands.runOnce(() -> m_robotState.updateRobotAction(RobotAction.kAutoShoot)));

m_operatorControls
.revNoAlign()
.onTrue(
Expand Down Expand Up @@ -380,19 +343,6 @@ private void configureButtonBindings() {
m_shooter.updateState(ShooterState.kIdle);
m_drive.updateProfile(DriveProfiles.kDefault);
}));

m_operatorControls
.amp()
.onTrue(
Commands.runOnce(
() -> {
m_ampToggle = !m_ampToggle;
if (m_ampToggle) {
m_robotState.updateRobotAction(RobotAction.kAmpLineup);
} else {
m_robotState.setDefaultAction();
}
}));
}

/**
Expand Down
5 changes: 5 additions & 0 deletions src/main/java/frc/robot/RobotState.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.Commands;
import frc.robot.Constants.IndexerConstants;
import frc.robot.Constants.ShooterConstants;
Expand Down Expand Up @@ -90,6 +91,8 @@ public static RobotState getInstance() {
}

public void updateRobotState() {
var start = Timer.getFPGATimestamp();

m_profiles.getPeriodicFunction().run();

Logger.recordOutput("RobotState/CurrentAction", (RobotAction) m_profiles.getCurrentProfile());
Expand All @@ -98,6 +101,8 @@ public void updateRobotState() {
DriverStation.getAlliance().isPresent()
? DriverStation.getAlliance().get().toString()
: "Unknown");

Logger.recordOutput("PeriodicTime/RobotState", Timer.getFPGATimestamp() - start);
}

public void revAndAlignPeriodic() {
Expand Down
1 change: 0 additions & 1 deletion src/main/java/frc/robot/commands/DriveCommands.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,7 +64,6 @@ public static Command joystickDrive(
boolean isFlipped =
DriverStation.getAlliance().isPresent()
&& DriverStation.getAlliance().get() == Alliance.Red;
System.out.println();
drive.setDesiredChassisSpeeds(
ChassisSpeeds.fromFieldRelativeSpeeds(
linearVelocity.getX() * DriveConstants.kMaxLinearSpeed,
Expand Down
12 changes: 8 additions & 4 deletions src/main/java/frc/robot/subsystems/aprilTagVision/AprilTagVision.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,6 +60,8 @@ public AprilTagVision(AprilTagVisionIO... ios) {

@Override
public void periodic() {
var start = Timer.getFPGATimestamp();

for (int i = 0; i < m_ios.length; i++) {
m_ios[i].updateInputs(m_inputs[i]);

Expand Down Expand Up @@ -215,14 +217,14 @@ public void periodic() {

// Get tag poses and update last detection times
List<Pose3d> tagPoses = new ArrayList<>();
int start;
int startIndex;
// Depending on the tag count the poses start at different indices
if (tagCount == 1) {
start = 10;
startIndex = 10;
} else {
start = 18;
startIndex = 18;
}
for (int i = start; i < values.length; i++) {
for (int i = startIndex; i < values.length; i++) {
int tagId = (int) values[i];
m_lastTagDetectionTimes.put(tagId, Timer.getFPGATimestamp());
Optional<Pose3d> tagPose = FieldConstants.kAprilTagLayout.getTagPose(tagId);
Expand Down Expand Up @@ -308,5 +310,7 @@ public void periodic() {
allVisionObservations.stream()
.sorted(Comparator.comparingDouble(VisionObservation::timestamp))
.forEach(RobotState.getInstance()::addVisionObservation);

Logger.recordOutput("PeriodicTime/AprilTagVision", Timer.getFPGATimestamp() - start);
}
}
5 changes: 5 additions & 0 deletions src/main/java/frc/robot/subsystems/drive/Drive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,7 @@
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
Expand Down Expand Up @@ -148,6 +149,8 @@ public Drive(
}

public void periodic() {
var start = Timer.getFPGATimestamp();

m_odometryLock.lock(); // Prevents odometry updates while reading data
m_gyroIO.updateInputs(m_gyroInputs);
for (var module : m_modules) {
Expand Down Expand Up @@ -207,6 +210,8 @@ public void periodic() {
}

Logger.recordOutput("Drive/Profile", (DriveProfiles) m_profiles.getCurrentProfile());

Logger.recordOutput("PeriodicTime/Drive", Timer.getFPGATimestamp() - start);
}

public void defaultPeriodic() {
Expand Down
Loading

0 comments on commit d128ffc

Please sign in to comment.