Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Use LinearProfile for flywheels #17

Merged
merged 7 commits into from
Feb 17, 2024
Merged
Show file tree
Hide file tree
Changes from 2 commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -211,7 +211,7 @@ private void configureButtonBindings() {
new Trigger(
() ->
drive.isAutoAimGoalCompleted()
&& flywheels.atSetpoint()
&& flywheels.atGoal()
&& superstructure.atArmSetpoint());
readyToShootTrigger
.whileTrue(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -10,21 +10,21 @@
import org.littletonrobotics.frc2024.Constants;

public class FlywheelConstants {
public static Config config =
public static FlywheelConfig flywheelConfig =
switch (Constants.getRobot()) {
case COMPBOT -> null;
case DEVBOT -> new Config(5, 4, (1.0 / 2.0), 100.0);
case SIMBOT -> new Config(0, 0, (1.0 / 2.0), 100.0);
case DEVBOT -> new FlywheelConfig(5, 4, (1.0 / 2.0), 6000.0, 100.0);
case SIMBOT -> new FlywheelConfig(0, 0, (1.0 / 2.0), 6000.0, 50.0);
};

public static Gains gains =
switch (Constants.getRobot()) {
case COMPBOT -> null;
case DEVBOT -> new Gains(0.0006, 0.0, 0.05, 0.33329, 0.00083, 0.0);
case SIMBOT -> new Gains(0.0, 0.0, 0.0, 0.09078, 0.00103, 0.0);
case SIMBOT -> new Gains(0.05, 0.0, 0.0, 0.01, 0.00103, 0.0);
};

public record Config(int leftID, int rightID, double reduction, double toleranceRPM) {}
public record FlywheelConfig(int leftID, int rightID, double reduction, double maxAcclerationRpmPerSec, double toleranceRPM) {}

public record Gains(double kP, double kI, double kD, double kS, double kV, double kA) {}
}
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
import java.util.function.DoubleSupplier;
import lombok.Getter;
import lombok.RequiredArgsConstructor;
import org.littletonrobotics.frc2024.util.LinearProfile;
import org.littletonrobotics.frc2024.util.LoggedTunableNumber;
import org.littletonrobotics.junction.AutoLogOutput;
import org.littletonrobotics.junction.Logger;
Expand All @@ -34,17 +35,23 @@ public class Flywheels extends SubsystemBase {
new LoggedTunableNumber("Superstructure/IdleLeftRPM", 200.0);
private static LoggedTunableNumber idleRightRPM =
new LoggedTunableNumber("Superstructure/IdleRightRPM", 200.0);

private static LoggedTunableNumber intakingLeftRPM =
new LoggedTunableNumber("Superstructure/IntakingLeftRPM", -2000.0);
private static LoggedTunableNumber intakingRightRPM =
new LoggedTunableNumber("Superstructure/IntakingRightRPM", -2000.0);
private static final LoggedTunableNumber shooterTolerance =
new LoggedTunableNumber("Flywheels/ToleranceRPM", config.toleranceRPM());
new LoggedTunableNumber("Flywheels/ToleranceRPM", flywheelConfig.toleranceRPM());
private static final LoggedTunableNumber maxAcceleration =
new LoggedTunableNumber("Flywheels/MaxAccelerationRpmPerSec", flywheelConfig.maxAcclerationRpmPerSec());

private final FlywheelsIO io;
private final FlywheelsIOInputsAutoLogged inputs = new FlywheelsIOInputsAutoLogged();

private final LinearProfile leftProfile;
private final LinearProfile rightProfile;
private boolean wasClosedLoop = false;
private boolean closedLoop = false;

@RequiredArgsConstructor
public enum Goal {
STOP(() -> 0.0, () -> 0.0),
Expand All @@ -53,22 +60,26 @@ public enum Goal {
INTAKING(intakingLeftRPM, intakingRightRPM),
CHARACTERIZING(() -> 0.0, () -> 0.0);

private final DoubleSupplier leftSetpoint;
private final DoubleSupplier rightSetpoint;
private final DoubleSupplier leftGoal;
private final DoubleSupplier rightGoal;

private double getLeftSetpoint() {
return leftSetpoint.getAsDouble();
private double getLeftGoal() {
return leftGoal.getAsDouble();
}

private double getRightSetpoint() {
return rightSetpoint.getAsDouble();
private double getRightGoal() {
return rightGoal.getAsDouble();
}
}

@Getter private Goal goal = Goal.IDLE;

public Flywheels(FlywheelsIO io) {
this.io = io;

leftProfile = new LinearProfile(maxAcceleration.get(), 0.02);
jwbonner marked this conversation as resolved.
Show resolved Hide resolved
rightProfile = new LinearProfile(maxAcceleration.get(), 0.02);

setDefaultCommand(runOnce(() -> goal = Goal.IDLE).withName("FlywheelsIdle"));
}

Expand All @@ -81,26 +92,56 @@ public void periodic() {
LoggedTunableNumber.ifChanged(hashCode(), pid -> io.setPID(pid[0], pid[1], pid[2]), kP, kI, kD);
LoggedTunableNumber.ifChanged(
hashCode(), kSVA -> io.setFF(kSVA[0], kSVA[1], kSVA[2]), kS, kV, kA);

LoggedTunableNumber.ifChanged(
hashCode(),
() -> {
leftProfile.setMaxAcceleration(maxAcceleration.get());
rightProfile.setMaxAcceleration(maxAcceleration.get());
},
maxAcceleration);

// Stop when disabled
if (DriverStation.isDisabled()) {
goal = Goal.STOP;
setGoal(Goal.STOP);
}

switch (goal) {
case STOP -> io.stop();
case CHARACTERIZING -> {} // Handled by runCharacterizationVolts
default -> io.runVelocity(goal.getLeftSetpoint(), goal.getRightSetpoint());
// Check if profile needs to be reset
if (!closedLoop && wasClosedLoop) {
leftProfile.reset();
rightProfile.reset();
wasClosedLoop = false;
}

// Run to setpoint
if (closedLoop) {
double leftSetpoint = leftProfile.calculateSetpoint();
double rightSetpoint = rightProfile.calculateSetpoint();
io.runVelocity(leftSetpoint, rightSetpoint);

Logger.recordOutput("Flywheels/SetpointLeftRpm", leftSetpoint);
Logger.recordOutput("Flywheels/SetpointRightRpm", rightSetpoint);
}

Logger.recordOutput("Flywheels/Goal", goal);
Logger.recordOutput("Flywheels/LeftSetpointRPM", goal.getLeftSetpoint());
Logger.recordOutput("Flywheels/RightSetpointRPM", goal.getRightSetpoint());
Logger.recordOutput("Flywheels/LeftRPM", inputs.leftVelocityRpm);
Logger.recordOutput("Flywheels/RightRPM", inputs.rightVelocityRpm);
Logger.recordOutput("Flywheels/GoalLeftRpm", goal.getLeftGoal());
Logger.recordOutput("Flywheels/GoalRightRpm", goal.getRightGoal());
}

private void setGoal(Goal goal) {
if (goal == Goal.CHARACTERIZING || goal == Goal.STOP) {
wasClosedLoop = closedLoop;
closedLoop = false;
return; // Don't set a goal
}
// Enable close loop
leftProfile.setGoal(goal.getLeftGoal(), inputs.leftVelocityRpm);
jwbonner marked this conversation as resolved.
Show resolved Hide resolved
rightProfile.setGoal(goal.getRightGoal(), inputs.rightVelocityRpm);
closedLoop = true;
this.goal = goal;
}

public void runCharacterizationVolts(double volts) {
goal = Goal.CHARACTERIZING;
setGoal(Goal.CHARACTERIZING);
io.runCharacterizationLeftVolts(volts);
io.runCharacterizationRightVolts(volts);
}
Expand All @@ -109,19 +150,20 @@ public double getCharacterizationVelocity() {
return (inputs.leftVelocityRpm + inputs.rightVelocityRpm) / 2.0;
}

@AutoLogOutput(key = "Shooter/AtSetpoint")
public boolean atSetpoint() {
return Math.abs(inputs.leftVelocityRpm - goal.leftSetpoint.getAsDouble())
<= shooterTolerance.get()
&& Math.abs(inputs.rightVelocityRpm - goal.rightSetpoint.getAsDouble())
@AutoLogOutput(key = "Shooter/AtGoal")
public boolean atGoal() {
jwbonner marked this conversation as resolved.
Show resolved Hide resolved
return Math.abs(leftProfile.getCurrentSetpoint() - goal.getLeftGoal()) <= shooterTolerance.get()
&& Math.abs(rightProfile.getCurrentSetpoint() - goal.getRightGoal())
<= shooterTolerance.get();
}

public Command shootCommand() {
return startEnd(() -> goal = Goal.SHOOTING, () -> goal = Goal.IDLE).withName("FlywheelsShoot");
return startEnd(() -> setGoal(Goal.SHOOTING), () -> setGoal(Goal.IDLE))
.withName("FlywheelsShoot");
}

public Command intakeCommand() {
return startEnd(() -> goal = Goal.INTAKING, () -> goal = Goal.IDLE).withName("FlywheelsIntake");
return startEnd(() -> setGoal(Goal.INTAKING), () -> setGoal(Goal.IDLE))
.withName("FlywheelsIntake");
}
}
Original file line number Diff line number Diff line change
Expand Up @@ -18,9 +18,9 @@

public class FlywheelsIOSim implements FlywheelsIO {
private final FlywheelSim leftSim =
new FlywheelSim(DCMotor.getKrakenX60Foc(1), config.reduction(), 0.00363458292);
new FlywheelSim(DCMotor.getKrakenX60Foc(1), flywheelConfig.reduction(), 0.00363458292);
private final FlywheelSim rightSim =
new FlywheelSim(DCMotor.getKrakenX60Foc(1), config.reduction(), 0.00363458292);
new FlywheelSim(DCMotor.getKrakenX60Foc(1), flywheelConfig.reduction(), 0.00363458292);

private final PIDController leftController =
new PIDController(gains.kP(), gains.kI(), gains.kD());
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -31,8 +31,8 @@ public class FlywheelsIOSparkFlex implements FlywheelsIO {

public FlywheelsIOSparkFlex() {
// Init Hardware
leftMotor = new CANSparkFlex(config.leftID(), CANSparkFlex.MotorType.kBrushless);
rightMotor = new CANSparkFlex(config.rightID(), CANSparkFlex.MotorType.kBrushless);
leftMotor = new CANSparkFlex(flywheelConfig.leftID(), CANSparkFlex.MotorType.kBrushless);
rightMotor = new CANSparkFlex(flywheelConfig.rightID(), CANSparkFlex.MotorType.kBrushless);
leftEncoder = leftMotor.getEncoder();
rightEncoder = rightMotor.getEncoder();

Expand Down Expand Up @@ -72,15 +72,15 @@ public FlywheelsIOSparkFlex() {
@Override
public void updateInputs(FlywheelsIOInputs inputs) {
inputs.leftPositionRads =
Units.rotationsToRadians(leftEncoder.getPosition()) / config.reduction();
inputs.leftVelocityRpm = leftEncoder.getVelocity() / config.reduction();
Units.rotationsToRadians(leftEncoder.getPosition()) / flywheelConfig.reduction();
inputs.leftVelocityRpm = leftEncoder.getVelocity() / flywheelConfig.reduction();
inputs.leftAppliedVolts = leftMotor.getAppliedOutput();
inputs.leftOutputCurrent = leftMotor.getOutputCurrent();
inputs.leftTempCelsius = leftMotor.getMotorTemperature();

inputs.rightPositionRads =
Units.rotationsToRadians(rightEncoder.getPosition()) / config.reduction();
inputs.rightVelocityRpm = rightEncoder.getVelocity() / config.reduction();
Units.rotationsToRadians(rightEncoder.getPosition()) / flywheelConfig.reduction();
inputs.rightVelocityRpm = rightEncoder.getVelocity() / flywheelConfig.reduction();
inputs.rightAppliedVolts = rightMotor.getAppliedOutput();
inputs.rightOutputCurrent = rightMotor.getOutputCurrent();
inputs.rightTempCelsius = rightMotor.getMotorTemperature();
Expand All @@ -95,13 +95,13 @@ public void runVolts(double leftVolts, double rightVolts) {
@Override
public void runVelocity(double leftRpm, double rightRpm) {
leftController.setReference(
leftRpm * config.reduction(),
leftRpm * flywheelConfig.reduction(),
CANSparkBase.ControlType.kVelocity,
0,
ff.calculate(leftRpm),
SparkPIDController.ArbFFUnits.kVoltage);
rightController.setReference(
rightRpm * config.reduction(),
rightRpm * flywheelConfig.reduction(),
CANSparkBase.ControlType.kVelocity,
0,
ff.calculate(rightRpm),
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,72 @@
// Copyright (c) 2024 FRC 6328
// http://github.com/Mechanical-Advantage
//
// Use of this source code is governed by an MIT-style
// license that can be found in the LICENSE file at
// the root directory of this project.

package org.littletonrobotics.frc2024.util;

import lombok.Getter;
import lombok.Setter;

/** Ramps up and down to setpoint for velocity closed loop control */
public class LinearProfile {
private double dv;
@Getter private final double period;
@Getter private double currentSetpoint = 0;
@Getter @Setter private double goal = 0;

/**
* Creates a new LinearProfiler
*
* @param maxAcceleration The max ramp rate in velocity in rpm/sec
* @param period Period of control loop (0.02)
*/
public LinearProfile(double maxAcceleration, double period) {
this.period = period;
setMaxAcceleration(maxAcceleration);
}

/** Set the max acceleration constraint in rpm/sec */
public void setMaxAcceleration(double maxAcceleration) {
dv = maxAcceleration * period;
}

/**
* Sets the target setpoint, starting from the current speed
*
* @param goal Target setpoint
* @param currentSpeed Current speed, to be used as the starting setpoint
*/
public void setGoal(double goal, double currentSpeed) {
this.goal = goal;
currentSetpoint = currentSpeed;
}

/** Resets target setpoint and current setpoint */
public void reset() {
currentSetpoint = 0;
goal = 0;
}

/**
* Returns the current setpoint to send to motors
*
* @return Setpoint to send to motors
*/
public double calculateSetpoint() {
if (goal > currentSetpoint) {
currentSetpoint += dv;
if (currentSetpoint > goal) {
currentSetpoint = goal;
}
} else if (goal < currentSetpoint) {
currentSetpoint -= dv;
if (currentSetpoint < goal) {
currentSetpoint = goal;
}
}
return currentSetpoint;
}
}
Loading