Skip to content

Commit

Permalink
Merge remote-tracking branch 'origin/staging' into elevator
Browse files Browse the repository at this point in the history
  • Loading branch information
gonzalezgonzalezl committed Feb 3, 2024
2 parents 30ab88d + b2b1c7e commit 2a0e687
Show file tree
Hide file tree
Showing 7 changed files with 296 additions and 1 deletion.
19 changes: 18 additions & 1 deletion src/main/java/org/team1540/robot2024/Constants.java
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,7 @@ public final class Constants {
// Whether to pull PID constants from SmartDashboard
public static final boolean tuningMode = true; // TODO: DO NOT SET TO TRUE FOR COMP
private static final Mode simMode = Mode.SIM; // Can also be Mode.REPLAY

public static final Mode currentMode = Robot.isReal() ? Mode.REAL : simMode;

public enum Mode {
Expand Down Expand Up @@ -63,6 +63,23 @@ public static class Drivetrain {
public static final double DRIVE_BASE_RADIUS = Math.hypot(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0);
public static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS;
}
public static class Indexer {
// TODO: fix these constants
public static final int INTAKE_ID = 11;
public static final int FEEDER_ID = 12;
public static final double FEEDER_KP = 0.5;
public static final double FEEDER_KI = 0.1;
public static final double FEEDER_KD = 0.001;
public static final double FEEDER_KS = 0.0;
public static final double FEEDER_KV = 0.0;
public static final double FEEDER_GEAR_RATIO = 1.0;
public static final double INTAKE_GEAR_RATIO = 1.0;
public static final double INTAKE_MOI = 0.025;
public static final double FEEDER_MOI = 0.025;
public static final int BEAM_BREAK_ID = 0;


}


public static class Vision {
Expand Down
23 changes: 23 additions & 0 deletions src/main/java/org/team1540/robot2024/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
import org.team1540.robot2024.commands.SwerveDriveCommand;
import org.team1540.robot2024.commands.elevator.ElevatorManualCommand;
import org.team1540.robot2024.commands.elevator.ElevatorSetpointCommand;
import org.team1540.robot2024.commands.indexer.IntakeCommand;
import org.team1540.robot2024.subsystems.drive.*;
import org.team1540.robot2024.subsystems.elevator.Elevator;
import org.team1540.robot2024.subsystems.elevator.ElevatorIO;
Expand All @@ -30,6 +31,10 @@
import org.team1540.robot2024.subsystems.vision.AprilTagVisionIOLimelight;
import org.team1540.robot2024.subsystems.vision.AprilTagVisionIOSim;
import org.team1540.robot2024.util.PhoenixTimeSyncSignalRefresher;
import org.team1540.robot2024.subsystems.indexer.Indexer;
import org.team1540.robot2024.subsystems.indexer.IndexerIO;
import org.team1540.robot2024.subsystems.indexer.IndexerIOSim;
import org.team1540.robot2024.subsystems.indexer.IndexerIOSparkMax;
import org.team1540.robot2024.util.swerve.SwerveFactory;
import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
import org.team1540.robot2024.util.vision.VisionPoseAcceptor;
Expand All @@ -48,6 +53,7 @@ public class RobotContainer {
public final Tramp tramp;
public final Shooter shooter;
public final Elevator elevator;
public final Indexer indexer;
public final AprilTagVision aprilTagVision;

// Controller
Expand Down Expand Up @@ -80,6 +86,10 @@ public RobotContainer() {
tramp = new Tramp(new TrampIOSparkMax());
shooter = new Shooter(new ShooterPivotIOTalonFX(), new FlywheelsIOTalonFX());
elevator = new Elevator(new ElevatorIOTalonFX());
indexer =
new Indexer(
new IndexerIOSparkMax()
);
aprilTagVision = new AprilTagVision(
new AprilTagVisionIOLimelight(Constants.Vision.FRONT_CAMERA_NAME, Constants.Vision.FRONT_CAMERA_POSE),
new AprilTagVisionIOLimelight(Constants.Vision.REAR_CAMERA_NAME, Constants.Vision.REAR_CAMERA_POSE),
Expand All @@ -106,6 +116,10 @@ public RobotContainer() {
ignored -> {},
() -> 0.0, // TODO: ACTUALLY GET ELEVATOR HEIGHT HERE
new VisionPoseAcceptor(drivetrain::getChassisSpeeds, () -> 0.0)); // TODO: ACTUALLY GET ELEVATOR VELOCITY HERE
indexer =
new Indexer(
new IndexerIOSim()
);
break;
default:
// Replayed robot, disable IO implementations
Expand All @@ -125,7 +139,14 @@ public RobotContainer() {
(ignored) -> {},
() -> 0.0,
new VisionPoseAcceptor(drivetrain::getChassisSpeeds, () -> 0.0));

indexer =
new Indexer(
new IndexerIO() {}
);

tramp = new Tramp(new TrampIO() {});

break;
}

Expand Down Expand Up @@ -168,6 +189,8 @@ private void configureButtonBindings() {

copilot.a().onTrue(shooter.spinUpCommand(leftFlywheelSetpoint.get(), rightFlywheelSetpoint.get()))
.onFalse(Commands.runOnce(shooter::stopFlywheels, shooter));

driver.a().onTrue(new IntakeCommand(indexer));
}

/**
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,30 @@
package org.team1540.robot2024.commands.indexer;

import edu.wpi.first.wpilibj2.command.Command;
import org.team1540.robot2024.subsystems.indexer.Indexer;

public class IntakeCommand extends Command {

private final Indexer indexer;

public IntakeCommand(Indexer indexer) {
this.indexer = indexer;
addRequirements(indexer);
}

@Override
public void initialize() {
indexer.setIntakePercent(0.4);
}


@Override
public boolean isFinished() {
return indexer.isNotePresent();
}

@Override
public void end(boolean interrupted) {
indexer.setIntakePercent(0);
}
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,51 @@
package org.team1540.robot2024.subsystems.indexer;

import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import org.littletonrobotics.junction.Logger;
import org.team1540.robot2024.util.LoggedTunableNumber;
import static org.team1540.robot2024.Constants.Indexer.*;
import static org.team1540.robot2024.Constants.tuningMode;


public class Indexer extends SubsystemBase {
private final IndexerIO indexerIO;
private final IndexerIOInputsAutoLogged indexerInputs = new IndexerIOInputsAutoLogged();
private final LoggedTunableNumber kP = new LoggedTunableNumber("Indexer/kP", FEEDER_KP);
private final LoggedTunableNumber kI = new LoggedTunableNumber("Indexer/kI", FEEDER_KI);
private final LoggedTunableNumber kD = new LoggedTunableNumber("Indexer/kD", FEEDER_KD);


public Indexer(IndexerIO indexerIO) {
this.indexerIO = indexerIO;
}

public void periodic() {
indexerIO.updateInputs(indexerInputs);
Logger.processInputs("Indexer", indexerInputs);
if (tuningMode) {
if (kP.hasChanged(hashCode()) || kI.hasChanged(hashCode()) || kD.hasChanged(hashCode())) {
indexerIO.configureFeederPID(kP.get(), kI.get(), kD.get());
}
}
}

public void setIntakePercent(double percent) {
indexerIO.setIntakeVoltage(percent * 12.0);
}

public boolean isNotePresent() {
return indexerInputs.noteInIntake;
}

public Command feedToAmp() {
return Commands.runOnce(() -> indexerIO.setFeederVelocity(-600), this);
}

public Command feedToShooter() {
return Commands.runOnce(() -> indexerIO.setFeederVelocity(1200), this);
}


}
Original file line number Diff line number Diff line change
@@ -0,0 +1,33 @@
package org.team1540.robot2024.subsystems.indexer;

import org.littletonrobotics.junction.AutoLog;

public interface IndexerIO {

@AutoLog
class IndexerIOInputs {
public double intakeVoltage;
public double intakeCurrentAmps;
public double intakeVelocityRPM;
public double feederVoltage;
public double feederCurrentAmps;
public double feederVelocityRPM;
public boolean noteInIntake = false;
public double setpointRPM;
public double feederVelocityError;
}

/**
* Updates the set of loggable inputs.
*/
default void updateInputs(IndexerIOInputs inputs) {}

default void setIntakeVoltage(double volts) {}

default void setFeederVoltage(double volts) {}

default void setFeederVelocity(double velocity) {}

default void configureFeederPID(double p, double i, double d) {}

}
Original file line number Diff line number Diff line change
@@ -0,0 +1,66 @@
package org.team1540.robot2024.subsystems.indexer;

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.wpilibj.simulation.DCMotorSim;
import edu.wpi.first.wpilibj.simulation.SimDeviceSim;
import org.team1540.robot2024.Constants;

import static org.team1540.robot2024.Constants.Indexer.*;

public class IndexerIOSim implements IndexerIO {


private final DCMotorSim intakeSim = new DCMotorSim(DCMotor.getNEO(1), INTAKE_GEAR_RATIO, INTAKE_MOI);
private final DCMotorSim feederSim = new DCMotorSim(DCMotor.getNEO(1), FEEDER_GEAR_RATIO, FEEDER_MOI);
// private final SimDeviceSim beamBreakSim = new SimDeviceSim("Indexer Beam Break");
private final PIDController feederSimPID = new PIDController(FEEDER_KP, FEEDER_KI,FEEDER_KD);
private boolean isClosedLoop = true;
private double intakeVoltage = 0.0;
private double feederVoltage = 0.0;
private double feederSetpointRPS = 0.0;

@Override
public void updateInputs(IndexerIOInputs inputs) {
if (isClosedLoop) {
feederVoltage = MathUtil.clamp(feederSimPID.calculate(feederSim.getAngularVelocityRPM() / 60, feederSetpointRPS), -12.0, 12.0);
}
intakeSim.setInputVoltage(intakeVoltage);
feederSim.setInputVoltage(feederVoltage);
intakeSim.update(Constants.LOOP_PERIOD_SECS);
feederSim.update(Constants.LOOP_PERIOD_SECS);
inputs.intakeCurrentAmps = intakeSim.getCurrentDrawAmps();
inputs.intakeVoltage = intakeVoltage;
inputs.intakeVelocityRPM = intakeSim.getAngularVelocityRPM();
inputs.feederCurrentAmps = feederSim.getCurrentDrawAmps();
inputs.feederVoltage = feederVoltage;
inputs.feederVelocityRPM = feederSim.getAngularVelocityRPM();
// inputs.noteInIntake = beamBreakSim.getBoolean("Indexer Beam Break").get();
inputs.setpointRPM = feederSimPID.getSetpoint() * 60;
inputs.feederVelocityError = feederSimPID.getPositionError();
}

@Override
public void setIntakeVoltage(double volts) {
intakeVoltage = MathUtil.clamp(volts, -12.0, 12.0);
}

@Override
public void configureFeederPID(double p, double i, double d) {
feederSimPID.setPID(p, i, d);
}

public void setFeederVoltage(double volts) {
isClosedLoop = false;
feederVoltage = MathUtil.clamp(volts, -12.0, 12.0);
}


public void setFeederVelocity(double velocity) {
isClosedLoop = true;
feederSimPID.reset();
feederSetpointRPS = velocity / 60;
}

}
Original file line number Diff line number Diff line change
@@ -0,0 +1,75 @@
package org.team1540.robot2024.subsystems.indexer;

import com.revrobotics.*;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.wpilibj.DigitalInput;

import static org.team1540.robot2024.Constants.Indexer.*;


public class IndexerIOSparkMax implements IndexerIO {
private final CANSparkMax intakeMotor = new CANSparkMax(INTAKE_ID, CANSparkLowLevel.MotorType.kBrushless);
private final CANSparkMax feederMotor = new CANSparkMax(FEEDER_ID, CANSparkLowLevel.MotorType.kBrushless);
private final DigitalInput indexerBeamBreak = new DigitalInput(BEAM_BREAK_ID);
private final SparkPIDController feederPID;
private final SimpleMotorFeedforward feederFF = new SimpleMotorFeedforward(FEEDER_KS, FEEDER_KV);
private double setpointRPM;


public IndexerIOSparkMax() {
intakeMotor.setIdleMode(CANSparkBase.IdleMode.kBrake);
intakeMotor.enableVoltageCompensation(12.0);
intakeMotor.setSmartCurrentLimit(30);

feederMotor.setIdleMode(CANSparkBase.IdleMode.kBrake);
feederMotor.enableVoltageCompensation(12.0);
feederMotor.setSmartCurrentLimit(30);

feederPID = feederMotor.getPIDController();
feederPID.setP(FEEDER_KP, 0);
feederPID.setI(FEEDER_KI, 0);
feederPID.setD(FEEDER_KD, 0);
}

@Override
public void updateInputs(IndexerIOInputs inputs) {
inputs.intakeCurrentAmps = intakeMotor.getOutputCurrent();
inputs.intakeVoltage = intakeMotor.getBusVoltage() * intakeMotor.getAppliedOutput();
inputs.intakeVelocityRPM = intakeMotor.getEncoder().getVelocity();
inputs.feederCurrentAmps = feederMotor.getOutputCurrent();
inputs.feederVoltage = feederMotor.getBusVoltage() * feederMotor.getAppliedOutput();
inputs.feederVelocityRPM = feederMotor.getEncoder().getVelocity();
inputs.noteInIntake = indexerBeamBreak.get();
inputs.setpointRPM = setpointRPM;
inputs.feederVelocityError = setpointRPM - feederMotor.getEncoder().getVelocity();
}

@Override
public void setIntakeVoltage(double volts) {
intakeMotor.setVoltage(volts);
}

@Override
public void setFeederVoltage(double volts) {
feederMotor.setVoltage(volts);
}

@Override
public void setFeederVelocity(double velocity) {
setpointRPM = velocity;
feederPID.setReference(
velocity / FEEDER_GEAR_RATIO, // Should this be multiplied?
CANSparkBase.ControlType.kVelocity,
0,
feederFF.calculate(velocity),
SparkPIDController.ArbFFUnits.kVoltage
);
}

@Override
public void configureFeederPID(double p, double i, double d) {
feederPID.setP(p);
feederPID.setI(i);
feederPID.setD(d);
}
}

0 comments on commit 2a0e687

Please sign in to comment.