Skip to content

Commit

Permalink
Add absolute encoder to arm
Browse files Browse the repository at this point in the history 
Add to DevBotSuperstructure
  • Loading branch information
@suryatho
suryatho committed Feb 9, 2024
1 parent cac4240 commit b6e8025
Show file tree
Hide file tree
Showing 5 changed files with 128 additions and 33 deletions.
14 changes: 8 additions & 6 deletions src/main/java/org/littletonrobotics/frc2024/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -200,10 +200,11 @@ private void configureButtonBindings() {
.leftTrigger()
.onTrue(
Commands.runOnce(
() -> superstructure.setDesiredState(DevBotSuperstructure.State.PREPARE_SHOOT)))
() ->
superstructure.setGoalState(DevBotSuperstructure.SystemState.PREPARE_SHOOT)))
.onFalse(
Commands.runOnce(
() -> superstructure.setDesiredState(DevBotSuperstructure.State.IDLE)));
() -> superstructure.setGoalState(DevBotSuperstructure.SystemState.IDLE)));

Trigger readyToShootTrigger =
new Trigger(() -> drive.isAutoAimGoalCompleted() && superstructure.atShootingSetpoint());
Expand All @@ -219,20 +220,21 @@ private void configureButtonBindings() {
.and(readyToShootTrigger)
.onTrue(
Commands.runOnce(
() -> superstructure.setDesiredState(DevBotSuperstructure.State.SHOOT))
() -> superstructure.setGoalState(DevBotSuperstructure.SystemState.SHOOT))
.andThen(Commands.waitSeconds(0.5))
.andThen(
Commands.runOnce(
() -> superstructure.setDesiredState(DevBotSuperstructure.State.IDLE))));
() ->
superstructure.setGoalState(DevBotSuperstructure.SystemState.IDLE))));

controller
.leftBumper()
.onTrue(
Commands.runOnce(
() -> superstructure.setDesiredState(DevBotSuperstructure.State.INTAKE)))
() -> superstructure.setGoalState(DevBotSuperstructure.SystemState.INTAKE)))
.onFalse(
Commands.runOnce(
() -> superstructure.setDesiredState(DevBotSuperstructure.State.IDLE)));
() -> superstructure.setGoalState(DevBotSuperstructure.SystemState.IDLE)));
}

controller
Expand Down
54 changes: 39 additions & 15 deletions ...in/java/org/littletonrobotics/frc2024/subsystems/superstructure/DevBotSuperstructure.java
View file
Original file line number Diff line number Diff line change
@@ -1,8 +1,10 @@
package org.littletonrobotics.frc2024.subsystems.superstructure;

import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import lombok.Getter;
import lombok.Setter;
import org.littletonrobotics.frc2024.subsystems.superstructure.arm.Arm;
import org.littletonrobotics.frc2024.subsystems.superstructure.flywheels.Flywheels;
import org.littletonrobotics.frc2024.util.LoggedTunableNumber;
Expand All @@ -25,16 +27,17 @@ public class DevBotSuperstructure extends SubsystemBase {
private static LoggedTunableNumber yCompensation =
new LoggedTunableNumber("DevBotSuperstructure/CompensationInches", 6.0);
private static LoggedTunableNumber followThroughTime =
new LoggedTunableNumber("DevBotSuperstructure/FollowthroughTimeSecs", 1.0);
new LoggedTunableNumber("DevBotSuperstructure/FollowthroughTimeSecs", 0.5);

public enum State {
public enum SystemState {
PREPARE_SHOOT,
SHOOT,
INTAKE,
IDLE
}

@Getter private State currentState = State.IDLE;
@Getter private SystemState currentState = SystemState.IDLE;
@Getter @Setter private SystemState goalState = SystemState.IDLE;
private final Arm arm;
private final Flywheels flywheels;

Expand All @@ -48,27 +51,48 @@ public DevBotSuperstructure(Arm arm, Flywheels flywheels) {

@Override
public void periodic() {
switch (goalState) {
case IDLE -> {
if (currentState == SystemState.SHOOT) {
if (followThroughTimer.hasElapsed(followThroughTime.get())) {
currentState = SystemState.IDLE;
followThroughTimer.stop();
followThroughTimer.reset();
} else {
currentState = SystemState.SHOOT;
}
} else {
currentState = SystemState.IDLE;
}
}
case INTAKE -> currentState = SystemState.INTAKE;
case PREPARE_SHOOT -> currentState = SystemState.PREPARE_SHOOT;
case SHOOT -> {
if (currentState != SystemState.PREPARE_SHOOT) {
currentState = SystemState.PREPARE_SHOOT;
} else if (atShootingSetpoint()) {
currentState = SystemState.SHOOT;
followThroughTimer.restart();
}
}
}

switch (currentState) {
case IDLE -> {}
case IDLE -> {
arm.setSetpoint(Rotation2d.fromDegrees(armIdleSetpointDegrees.get()));
flywheels.runVolts(0.0, 0.0);
}
case INTAKE -> {}
case PREPARE_SHOOT -> {}
case SHOOT -> {}
}

Logger.recordOutput("DevBotSuperstructure/currentState", currentState.toString());
Logger.recordOutput("DevBotSuperstructure/GoalState", goalState);
Logger.recordOutput("DevBotSuperstructure/currentState", currentState);
}

@AutoLogOutput(key = "DevBotSuperstructure/ReadyToShoot")
public boolean atShootingSetpoint() {
return (currentState == State.PREPARE_SHOOT || currentState == State.SHOOT)
&& arm.atSetpoint()
&& flywheels.atSetpoint();
}

public void setDesiredState(State desiredState) {
if (desiredState == State.PREPARE_SHOOT && currentState == State.SHOOT) {
return;
}
currentState = desiredState;
return arm.atSetpoint() && flywheels.atSetpoint();
}
}
10 changes: 5 additions & 5 deletions ...main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOKrakenFOC.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,7 @@ public class ArmIOKrakenFOC implements ArmIO {
private final StatusSignal<Double> armPositionRotations;
private final StatusSignal<Double> armAbsolutePositionRotations;
private final StatusSignal<Double> armTrajectorySetpointPositionRotations;
private final StatusSignal<Double> armVelocityRPS;
private final StatusSignal<Double> armVelocityRps;
private final List<StatusSignal<Double>> armAppliedVoltage;
private final List<StatusSignal<Double>> armOutputCurrent;
private final List<StatusSignal<Double>> armTorqueCurrent;
Expand Down Expand Up @@ -85,7 +85,7 @@ public ArmIOKrakenFOC() {
armPositionRotations = leaderMotor.getPosition();
armAbsolutePositionRotations = armEncoder.getPosition();
armTrajectorySetpointPositionRotations = leaderMotor.getClosedLoopReference();
armVelocityRPS = leaderMotor.getVelocity();
armVelocityRps = leaderMotor.getVelocity();
armAppliedVoltage = List.of(leaderMotor.getMotorVoltage(), followerMotor.getMotorVoltage());
armOutputCurrent = List.of(leaderMotor.getSupplyCurrent(), followerMotor.getSupplyCurrent());
armTorqueCurrent = List.of(leaderMotor.getTorqueCurrent(), followerMotor.getTorqueCurrent());
Expand All @@ -96,7 +96,7 @@ public ArmIOKrakenFOC() {
armPositionRotations,
armAbsolutePositionRotations,
armTrajectorySetpointPositionRotations,
armVelocityRPS,
armVelocityRps,
armAppliedVoltage.get(0),
armAppliedVoltage.get(1),
armOutputCurrent.get(0),
Expand All @@ -113,7 +113,7 @@ public void updateInputs(ArmIOInputs inputs) {
BaseStatusSignal.refreshAll(
armPositionRotations,
armTrajectorySetpointPositionRotations,
armVelocityRPS,
armVelocityRps,
armAppliedVoltage.get(0),
armAppliedVoltage.get(1),
armOutputCurrent.get(0),
Expand All @@ -126,7 +126,7 @@ public void updateInputs(ArmIOInputs inputs) {
inputs.armPositionRads = Units.rotationsToRadians(armPositionRotations.getValue());
inputs.armTrajectorySetpointRads =
Units.rotationsToRadians(armTrajectorySetpointPositionRotations.getValue());
inputs.armVelocityRadsPerSec = Units.rotationsToRadians(armVelocityRPS.getValue());
inputs.armVelocityRadsPerSec = Units.rotationsToRadians(armVelocityRps.getValue());
inputs.armAppliedVolts =
armAppliedVoltage.stream().mapToDouble(StatusSignal::getValueAsDouble).toArray();
inputs.armCurrentAmps =
Expand Down
64 changes: 64 additions & 0 deletions ...ava/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/FeederIOKrakenFOC.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,64 @@
package org.littletonrobotics.frc2024.subsystems.superstructure.feeder;

import static org.littletonrobotics.frc2024.subsystems.superstructure.SuperstructureConstants.FeederConstants.*;

import com.ctre.phoenix6.BaseStatusSignal;
import com.ctre.phoenix6.StatusSignal;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.controls.NeutralOut;
import com.ctre.phoenix6.controls.VoltageOut;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.InvertedValue;
import com.ctre.phoenix6.signals.NeutralModeValue;
import edu.wpi.first.math.util.Units;

public class FeederIOKrakenFOC implements FeederIO {
private final TalonFX motor;

private StatusSignal<Double> position;
private StatusSignal<Double> velocity;
private StatusSignal<Double> appliedVoltage;
private StatusSignal<Double> outputCurrent;

public FeederIOKrakenFOC() {
motor = new TalonFX(id, "canivore");

TalonFXConfiguration config = new TalonFXConfiguration();
config.MotorOutput.Inverted =
inverted ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive;
config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
config.CurrentLimits.SupplyCurrentLimit = 30.0;
config.CurrentLimits.SupplyCurrentLimitEnable = true;
config.Voltage.PeakForwardVoltage = 12.0;
config.Voltage.PeakReverseVoltage = -12.0;
motor.getConfigurator().apply(config);

position = motor.getPosition();
velocity = motor.getVelocity();
appliedVoltage = motor.getMotorVoltage();
outputCurrent = motor.getTorqueCurrent();

BaseStatusSignal.setUpdateFrequencyForAll(
50.0, position, velocity, appliedVoltage, outputCurrent);
}

@Override
public void updateInputs(FeederIOInputs inputs) {
BaseStatusSignal.refreshAll(position, velocity, appliedVoltage, outputCurrent);

inputs.positionRads = Units.rotationsToRadians(position.getValueAsDouble()) / reduction;
inputs.velocityRadsPerSec = Units.rotationsToRadians(velocity.getValueAsDouble()) / reduction;
inputs.appliedVoltage = appliedVoltage.getValueAsDouble();
inputs.outputCurrent = outputCurrent.getValueAsDouble();
}

@Override
public void runVolts(double volts) {
motor.setControl(new VoltageOut(volts).withEnableFOC(true));
}

@Override
public void stop() {
motor.setControl(new NeutralOut());
}
}
19 changes: 12 additions & 7 deletions ...main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/FeederIOSim.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,27 +2,32 @@

import static org.littletonrobotics.frc2024.subsystems.superstructure.SuperstructureConstants.FeederConstants.*;

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.wpilibj.simulation.FlywheelSim;

public class FeederIOSim implements FeederIO {
private final FlywheelSim sim = new FlywheelSim(
DCMotor.getKrakenX60Foc(1),
reduction,
0.01);
private final FlywheelSim sim = new FlywheelSim(DCMotor.getKrakenX60Foc(1), reduction, 0.01);

private double appliedVoltage = 0.0;

@Override
public void updateInputs(FeederIOInputs inputs) {
FeederIO.super.updateInputs(inputs);
sim.update(0.02);
inputs.positionRads += sim.getAngularVelocityRadPerSec() * 0.02;
inputs.velocityRadsPerSec = sim.getAngularVelocityRadPerSec();
inputs.appliedVoltage = appliedVoltage;
inputs.outputCurrent = sim.getCurrentDrawAmps();
}

@Override
public void runVolts(double volts) {
FeederIO.super.runVolts(volts);
appliedVoltage = MathUtil.clamp(volts, -12.0, 12.0);
sim.setInputVoltage(appliedVoltage);
}

@Override
public void stop() {
FeederIO.super.stop();
runVolts(0.0);
}
}

0 comments on commit b6e8025

Please sign in to comment.