Skip to content
New issue

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

#

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

Already on GitHub? # to your account

feat: elevator zeroing #22

Merged
merged 2 commits into from
Feb 8, 2025
Merged

feat: elevator zeroing #22

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
f57008b
[Feat] Ground intake states
eaeaeaosn Feb 7, 2025
ffc9000
feat: elevator zeroing
eaeaeaosn Feb 8, 2025
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
29 changes: 15 additions & 14 deletions src/main/java/frc/robot/RobotConstants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -377,28 +377,28 @@ public static class ElevatorConstants {

public static final double ELEVATOR_SPOOL_DIAMETER = 0.04 + 0.003; //0.04m for spool diameter, 0.003 for rope diameter
public static final double ELEVATOR_GEAR_RATIO = 3.0;
public static final double DEADZONE_DISTANCE = 0.02;
public static final double DEADZONE_DISTANCE = 0.0;

public static final TunableNumber motionAcceleration = new TunableNumber("Elevator/MotionAcceleration",
20.0);
200);
public static final TunableNumber motionCruiseVelocity = new TunableNumber(
"Elevator/MotionCruiseVelocity", 20.0);
"Elevator/MotionCruiseVelocity", 100);
public static final TunableNumber motionJerk = new TunableNumber("Elevator/MotionJerk", 0.0);

public static final TunableNumber MAX_EXTENSION_METERS = new TunableNumber("ELEVATOR SETPOINTS/max",
2);
1.57);
public static final TunableNumber IDLE_EXTENSION_METERS = new TunableNumber(
"ELEVATOR GOALS/idle", 0);
public static final TunableNumber L1_EXTENSION_METERS = new TunableNumber("ELEVATOR SETPOINTS/L1",
0.1);
0.45);
public static final TunableNumber L2_EXTENSION_METERS = new TunableNumber("ELEVATOR SETPOINTS/L2",
0.2);
0.53);
public static final TunableNumber L3_EXTENSION_METERS = new TunableNumber("ELEVATOR SETPOINTS/L3",
0.3);// 0.107853495
0.95);// 0.107853495
public static final TunableNumber L4_EXTENSION_METERS = new TunableNumber("ELEVATOR SETPOINTS/L4",
0.4);
1.55);
public static final TunableNumber ELEVATOR_ZEROING_CURRENT = new TunableNumber("Elevator zeroing current",
15);
20);

}

Expand All @@ -407,13 +407,14 @@ public static class ElevatorConstants {
* Constants for the elevator motor gains.
*/
public static class ElevatorGainsClass {
public static final TunableNumber ELEVATOR_KP = new TunableNumber("ELEVATOR PID/kp", 0.2);
public static final TunableNumber ELEVATOR_KP = new TunableNumber("ELEVATOR PID/kp", 15);
public static final TunableNumber ELEVATOR_KI = new TunableNumber("ELEVATOR PID/ki", 0);
public static final TunableNumber ELEVATOR_KD = new TunableNumber("ELEVATOR PID/kd", 0.001);
public static final TunableNumber ELEVATOR_KD = new TunableNumber("ELEVATOR PID/kd", 0);
public static final TunableNumber ELEVATOR_KA = new TunableNumber("ELEVATOR PID/ka",
0.0037512677);
public static final TunableNumber ELEVATOR_KV = new TunableNumber("ELEVATOR PID/kv", 0.113);// 0.107853495
0);
public static final TunableNumber ELEVATOR_KV = new TunableNumber("ELEVATOR PID/kv", 0);// 0.107853495
public static final TunableNumber ELEVATOR_KS = new TunableNumber("ELEVATOR PID/ks",
0.28475008);
0);
public static final TunableNumber ELEVATOR_KG = new TunableNumber("ELEVATOR PID/kg", 0.3);
}
}
9 changes: 5 additions & 4 deletions src/main/java/frc/robot/RobotContainer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,7 @@

import static edu.wpi.first.units.Units.Seconds;
import static frc.robot.RobotConstants.BeamBreakConstants.*;
import static frc.robot.RobotConstants.ElevatorConstants.*;

import java.io.IOException;
import java.util.function.*;
Expand Down Expand Up @@ -137,10 +138,10 @@ private void configureTesterBindings(CommandXboxController controller) {
new Trigger(controller.start())
.onTrue(superstructure.setWantedSuperStateCommand(Superstructure.WantedSuperState.STOPPED));
//test of elevator heights
controller.a().onTrue(Commands.runOnce(() -> elevatorSubsystem.setPosition(0.5),elevatorSubsystem).until(() ->elevatorSubsystem.isAtSetpoint(0.5)));
controller.b().onTrue(Commands.runOnce(() -> elevatorSubsystem.setPosition(0.6),elevatorSubsystem).until(() ->elevatorSubsystem.isAtSetpoint(0.6)));
controller.x().onTrue(Commands.runOnce(() -> elevatorSubsystem.setPosition(0.7),elevatorSubsystem).until(() ->elevatorSubsystem.isAtSetpoint(0.7)));
controller.y().onTrue(Commands.runOnce(() -> elevatorSubsystem.setPosition(0.47),elevatorSubsystem).until(() ->elevatorSubsystem.isAtSetpoint(0.47)));
controller.a().onTrue(Commands.runOnce(() -> elevatorSubsystem.setPosition(L1_EXTENSION_METERS.get()),elevatorSubsystem).until(() ->elevatorSubsystem.isAtSetpoint(L1_EXTENSION_METERS.get())));
controller.b().onTrue(Commands.runOnce(() -> elevatorSubsystem.setPosition(L2_EXTENSION_METERS.get()),elevatorSubsystem).until(() ->elevatorSubsystem.isAtSetpoint(L2_EXTENSION_METERS.get())));
controller.x().onTrue(Commands.runOnce(() -> elevatorSubsystem.setPosition(L3_EXTENSION_METERS.get()),elevatorSubsystem).until(() ->elevatorSubsystem.isAtSetpoint(L3_EXTENSION_METERS.get())));
controller.y().onTrue(Commands.runOnce(() -> elevatorSubsystem.setPosition(L4_EXTENSION_METERS.get()),elevatorSubsystem).until(() ->elevatorSubsystem.isAtSetpoint(L4_EXTENSION_METERS.get())));
controller.povDown().onTrue(new ElevatorZeroingCommand(elevatorSubsystem));
}

Expand Down
5 changes: 2 additions & 3 deletions src/main/java/frc/robot/commands/ElevatorZeroingCommand.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,17 +21,16 @@ public void initialize() {

@Override
public void execute(){
elevator.setVoltage(-0.5);
elevator.setVoltage(-1);
}

@Override
public boolean isFinished() {
return elevator.getLeaderCurrent() > RobotConstants.ElevatorConstants.ELEVATOR_ZEROING_CURRENT.get();
return Math.abs(elevator.getLeaderCurrent()) > RobotConstants.ElevatorConstants.ELEVATOR_ZEROING_CURRENT.get();
}

@Override
public void end(boolean interrupted) {

elevator.resetPosition();
elevator.setVoltage(0);
}
Expand Down
22 changes: 19 additions & 3 deletions src/main/java/frc/robot/subsystems/Superstructure.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,12 +12,14 @@ public class Superstructure extends SubsystemBase {
public enum WantedSuperState {
STOPPED,
INTAKE_CORAL_FUNNEL,
INTAKE_CORAL_GROUND,
SHOOT_CORAL
}

public enum CurrentSuperState {
STOPPED,
INTAKE_CORAL_FUNNEL,
INTAKE_CORAL_GROUND,
SHOOT_CORAL
}

Expand Down Expand Up @@ -45,6 +47,9 @@ private CurrentSuperState handleStateTransition() {
case INTAKE_CORAL_FUNNEL:
currentSuperState = CurrentSuperState.INTAKE_CORAL_FUNNEL;
break;
case INTAKE_CORAL_GROUND:
currentSuperState = CurrentSuperState.INTAKE_CORAL_GROUND;
break;
case SHOOT_CORAL:
currentSuperState = CurrentSuperState.SHOOT_CORAL;
break;
Expand All @@ -61,6 +66,9 @@ private void applyStates() {
case INTAKE_CORAL_FUNNEL:
intakeCoralFunnel();
break;
case INTAKE_CORAL_GROUND:
intakeCoralGround();
break;
case SHOOT_CORAL:
shootCoral();
break;
Expand All @@ -80,15 +88,23 @@ public Command setWantedSuperStateCommand(WantedSuperState wantedSuperState) {
}

private void intakeCoralFunnel() {
if (endEffector.isFunnelIntakeFinished()) {
endEffector.setWantedState(EndEffectorSubsystem.WantedState.FUNNEL_TRANSFER);
if (endEffector.isIntakeFinished()) {
endEffector.setWantedState(EndEffectorSubsystem.WantedState.TRANSFER);
} else {
endEffector.setWantedState(EndEffectorSubsystem.WantedState.FUNNEL_INTAKE);
}
}

private void intakeCoralGround() {
if (endEffector.isIntakeFinished()) {
endEffector.setWantedState(EndEffectorSubsystem.WantedState.TRANSFER);
} else {
endEffector.setWantedState(EndEffectorSubsystem.WantedState.GROUND_INTAKE);
}
}

private void shootCoral() {
endEffector.setWantedState(EndEffectorSubsystem.WantedState.SHOOT);
if (endEffector.isCoralReady()) {endEffector.setWantedState(EndEffectorSubsystem.WantedState.SHOOT);}
}

private void handleStopped() {
Expand Down
4 changes: 0 additions & 4 deletions src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,10 +27,6 @@ public default void stop() {
setVoltage(0.0);
}

public default double getLeaderCurrent(){
return getLeaderCurrent();
}

public default void resetEncoder(double position) {}

public default void resetEncoder() {
Expand Down
16 changes: 9 additions & 7 deletions src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,6 @@ public class ElevatorIOReal implements ElevatorIO {
private final StatusSignal<Current> supplyRight;
private final StatusSignal<Temperature> tempLeft;
private final StatusSignal<Temperature> tempRight;
private final StatusSignal<Double> closedLoopReferenceSlope;

public ElevatorIOReal() {
this.leader = new TalonFX(LEFT_ELEVATOR_MOTOR_ID, CANIVORE_CAN_BUS_NAME);
Expand Down Expand Up @@ -98,17 +97,21 @@ public ElevatorIOReal() {
statorLeft = leader.getStatorCurrent();
statorRight = follower.getStatorCurrent();
supplyLeft = leader.getSupplyCurrent();
supplyRight = leader.getSupplyCurrent();
supplyRight = follower.getSupplyCurrent();
tempLeft = leader.getDeviceTemp();
tempRight = follower.getDeviceTemp();
closedLoopReferenceSlope = leader.getClosedLoopReferenceSlope();

follower.setControl(new Follower(leader.getDeviceID(), true));
}

@Override
public void updateInputs(ElevatorIOInputs inputs) {
BaseStatusSignal.refreshAll();
BaseStatusSignal.refreshAll(
voltageLeft, voltageRight,
statorLeft, statorRight,
supplyLeft, supplyRight,
tempLeft, tempRight
);

inputs.positionMeters = getHeight();
inputs.velocityMetersPerSec = getVelocity();
Expand All @@ -126,6 +129,7 @@ public void updateInputs(ElevatorIOInputs inputs) {
slot0Configs.kP = ELEVATOR_KP.get();
slot0Configs.kI = ELEVATOR_KI.get();
slot0Configs.kD = ELEVATOR_KD.get();
slot0Configs.kG = ELEVATOR_KG.get();

motionMagicConfigs.MotionMagicAcceleration = motionAcceleration.get();
motionMagicConfigs.MotionMagicCruiseVelocity = motionCruiseVelocity.get();
Expand Down Expand Up @@ -154,9 +158,7 @@ public void resetEncoder(final double position) {
follower.setPosition(position);
}

public double getLeaderCurrent(){
return leader.getStatorCurrent().getValueAsDouble();
}


@Override
public void resetPosition(){
Expand Down
9 changes: 8 additions & 1 deletion src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
package frc.robot.subsystems.elevator;

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.filter.LinearFilter;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import org.littletonrobotics.junction.Logger;

Expand All @@ -10,6 +11,9 @@ public class ElevatorSubsystem extends SubsystemBase {
private final ElevatorIO io;
private final ElevatorIOInputsAutoLogged inputs = new ElevatorIOInputsAutoLogged();

private final LinearFilter currentFilter = LinearFilter.movingAverage(5);
public double currentFilterValue = 0.0;

public ElevatorSubsystem(ElevatorIO io) {
this.io = io;
}
Expand All @@ -18,6 +22,9 @@ public ElevatorSubsystem(ElevatorIO io) {
public void periodic() {
io.updateInputs(inputs);
Logger.processInputs("Elevator", inputs);

currentFilterValue = currentFilter.calculate(inputs.statorCurrentAmps[0]);
Logger.recordOutput("Elevator/CurrentFilter", currentFilterValue);
}

public void setPosition(double heightMeters) {
Expand All @@ -38,7 +45,7 @@ public boolean isAtSetpoint(double setpoint) {
}

public double getLeaderCurrent(){
return io.getLeaderCurrent();
return currentFilterValue;
}

public void resetPosition(){
Expand Down
40 changes: 31 additions & 9 deletions src/main/java/frc/robot/subsystems/endeffector/EndEffectorSubsystem.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,7 @@ public class EndEffectorSubsystem extends RollerSubsystem {
private SystemState systemState = SystemState.IDLING;

private boolean hasTransitionedToTransfer = false;
private boolean hasTransitionedToHold = false;

public double kp = ENDEFFECTOR_KP.get();
public double ki = ENDEFFECTOR_KI.get();
Expand All @@ -42,7 +43,8 @@ public class EndEffectorSubsystem extends RollerSubsystem {
public enum WantedState {
IDLE,
FUNNEL_INTAKE,
FUNNEL_TRANSFER,
GROUND_INTAKE,
TRANSFER,
HOLD,
SHOOT,
OFF
Expand All @@ -51,7 +53,8 @@ public enum WantedState {
public enum SystemState {
IDLING,
FUNNEL_INTAKING,
FUNNEL_TRANSFERRING,
GROUND_INTAKING,
TRANSFERRING,
HOLDING,
SHOOTING,
OFF
Expand Down Expand Up @@ -93,7 +96,10 @@ public void periodic() {
case FUNNEL_INTAKING:
io.setVelocity(intakeRPS);
break;
case FUNNEL_TRANSFERRING:
case GROUND_INTAKING:
io.setVelocity(-intakeRPS);
break;
case TRANSFERRING:
io.setVelocity(transferRPS);
break;
case HOLDING:
Expand Down Expand Up @@ -129,19 +135,31 @@ private SystemState handleStateTransition() {
case FUNNEL_INTAKE -> {
if (middleBBInputs.isBeambreakOn) {
hasTransitionedToTransfer = true;
yield SystemState.FUNNEL_TRANSFERRING;
yield SystemState.TRANSFERRING;
}
yield SystemState.FUNNEL_INTAKING;
}
case FUNNEL_TRANSFER -> {
case GROUND_INTAKE -> {
if (middleBBInputs.isBeambreakOn) {
hasTransitionedToTransfer = true;
yield SystemState.TRANSFERRING;
}
yield SystemState.GROUND_INTAKING;
}
case TRANSFER -> {
if (edgeBBInputs.isBeambreakOn && !middleBBInputs.isBeambreakOn) {
yield SystemState.HOLDING;
}
yield SystemState.FUNNEL_TRANSFERRING;
yield SystemState.TRANSFERRING;
}
case HOLD -> {
hasTransitionedToHold = true;
yield SystemState.HOLDING;
}
case HOLD -> SystemState.HOLDING;
case SHOOT -> {
if (isShootFinished()) {
hasTransitionedToTransfer = false;
hasTransitionedToHold = false;
yield SystemState.IDLING;
}
yield SystemState.SHOOTING;
Expand All @@ -151,11 +169,15 @@ private SystemState handleStateTransition() {
};
}

public boolean isCoralReady () {
return hasTransitionedToHold;
}

public boolean isShootFinished () {
return hasTransitionedToTransfer && !edgeBBInputs.isBeambreakOn;
return hasTransitionedToHold && !edgeBBInputs.isBeambreakOn;
}

public boolean isFunnelIntakeFinished () {
public boolean isIntakeFinished () {
return hasTransitionedToTransfer;
}

Expand Down