Skip to content

Commit

Permalink
Arm uses commands
Browse files Browse the repository at this point in the history 
Disable superstructure
  • Loading branch information
@suryatho
suryatho committed Feb 13, 2024
1 parent ba35a76 commit bc9936d
Show file tree
Hide file tree
Showing 8 changed files with 200 additions and 228 deletions.
13 changes: 5 additions & 8 deletions src/main/java/org/littletonrobotics/frc2024/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,10 @@
import org.littletonrobotics.frc2024.subsystems.apriltagvision.AprilTagVisionIO;
import org.littletonrobotics.frc2024.subsystems.apriltagvision.AprilTagVisionIONorthstar;
import org.littletonrobotics.frc2024.subsystems.drive.*;
import org.littletonrobotics.frc2024.subsystems.flywheels.Flywheels;
import org.littletonrobotics.frc2024.subsystems.flywheels.FlywheelsIO;
import org.littletonrobotics.frc2024.subsystems.flywheels.FlywheelsIOSim;
import org.littletonrobotics.frc2024.subsystems.flywheels.FlywheelsIOSparkFlex;
import org.littletonrobotics.frc2024.subsystems.rollers.Rollers;
import org.littletonrobotics.frc2024.subsystems.rollers.RollersSensorsIO;
import org.littletonrobotics.frc2024.subsystems.rollers.RollersSensorsIOReal;
Expand All @@ -48,10 +52,6 @@
import org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmIO;
import org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmIOKrakenFOC;
import org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmIOSim;
import org.littletonrobotics.frc2024.subsystems.flywheels.Flywheels;
import org.littletonrobotics.frc2024.subsystems.flywheels.FlywheelsIO;
import org.littletonrobotics.frc2024.subsystems.flywheels.FlywheelsIOSim;
import org.littletonrobotics.frc2024.subsystems.flywheels.FlywheelsIOSparkFlex;
import org.littletonrobotics.frc2024.util.AllianceFlipUtil;
import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;

Expand Down Expand Up @@ -102,9 +102,8 @@ public RobotContainer() {
intake = new Intake(new IntakeIOKrakenFOC());
rollers = new Rollers(feeder, indexer, intake, new RollersSensorsIOReal());

arm = new Arm(new ArmIOKrakenFOC());
flywheels = new Flywheels(new FlywheelsIOSparkFlex());
superstructure = new Superstructure(arm, flywheels);
arm = new Arm(new ArmIOKrakenFOC());

aprilTagVision =
new AprilTagVision(
Expand All @@ -125,8 +124,6 @@ public RobotContainer() {
new ModuleIOSim(DriveConstants.moduleConfigs[3]));
arm = new Arm(new ArmIOSim());
flywheels = new Flywheels(new FlywheelsIOSim());
// intake = new Intake(new IntakeIOSim());
// feeder = new Feeder(new FeederIOSim());
superstructure = new Superstructure(arm, flywheels);
}
case COMPBOT -> {
Expand Down
1 change: 0 additions & 1 deletion src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,6 @@
import lombok.Getter;
import lombok.RequiredArgsConstructor;
import lombok.Setter;
import org.littletonrobotics.frc2024.subsystems.superstructure.flywheels.FlywheelsIOInputsAutoLogged;
import org.littletonrobotics.frc2024.util.LoggedTunableNumber;
import org.littletonrobotics.junction.AutoLogOutput;
import org.littletonrobotics.junction.Logger;
Expand Down
41 changes: 1 addition & 40 deletions src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java
View file
Original file line number Diff line number Diff line change
@@ -1,28 +1,17 @@
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.RequiredArgsConstructor;
import lombok.Setter;
import org.littletonrobotics.frc2024.RobotState;
import org.littletonrobotics.frc2024.subsystems.superstructure.arm.Arm;
import org.littletonrobotics.frc2024.subsystems.flywheels.Flywheels;
import org.littletonrobotics.frc2024.util.LoggedTunableNumber;
import org.littletonrobotics.frc2024.subsystems.superstructure.arm.Arm;
import org.littletonrobotics.junction.AutoLogOutput;
import org.littletonrobotics.junction.Logger;

@RequiredArgsConstructor
public class Superstructure extends SubsystemBase {
private static LoggedTunableNumber armIdleSetpointDegrees =
new LoggedTunableNumber("Superstructure/ArmIdleSetpointDegrees", 20.0);
private static LoggedTunableNumber armStationIntakeSetpointDegrees =
new LoggedTunableNumber("Superstructure/ArmStationIntakeSetpointDegrees", 45.0);
private static LoggedTunableNumber armIntakeSetpointDegrees =
new LoggedTunableNumber("Superstructure/ArmIntakeDegrees", 40.0);
private static LoggedTunableNumber followThroughTime =
new LoggedTunableNumber("Superstructure/FollowthroughTimeSecs", 0.5);

public enum SystemState {
PREPARE_SHOOT,
Expand Down Expand Up @@ -62,34 +51,6 @@ public void periodic() {
case SHOOT -> currentState = SystemState.SHOOT;
}

switch (currentState) {
case IDLE -> {
arm.setSetpoint(Rotation2d.fromDegrees(armIdleSetpointDegrees.get()));
flywheels.setGoal(Flywheels.Goal.IDLE);
}
case INTAKE -> {
arm.setSetpoint(Rotation2d.fromDegrees(armIntakeSetpointDegrees.get()));
flywheels.setGoal(Flywheels.Goal.IDLE);
}
case STATION_INTAKE -> {
arm.setSetpoint(Rotation2d.fromDegrees(armStationIntakeSetpointDegrees.get()));
flywheels.setGoal(Flywheels.Goal.INTAKING);
}
case REVERSE_INTAKE -> {
arm.setSetpoint(Rotation2d.fromDegrees(armIntakeSetpointDegrees.get()));
}
case PREPARE_SHOOT -> {
var aimingParams = RobotState.getInstance().getAimingParameters();
arm.setSetpoint(aimingParams.armAngle());
flywheels.setGoal(Flywheels.Goal.SHOOTING);
}
case SHOOT -> {
var aimingParams = RobotState.getInstance().getAimingParameters();
flywheels.setGoal(Flywheels.Goal.SHOOTING);
arm.setSetpoint(aimingParams.armAngle());
}
}

Logger.recordOutput("Superstructure/GoalState", goalState);
Logger.recordOutput("Superstructure/CurrentState", currentState);
}
Expand Down
103 changes: 59 additions & 44 deletions src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,14 +7,14 @@
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
import edu.wpi.first.wpilibj.util.Color;
import edu.wpi.first.wpilibj.util.Color8Bit;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import java.util.function.DoubleSupplier;
import lombok.Getter;
import lombok.RequiredArgsConstructor;
import lombok.Setter;
import org.littletonrobotics.frc2024.RobotState;
import org.littletonrobotics.frc2024.util.LoggedTunableNumber;
import org.littletonrobotics.junction.AutoLogOutput;
import org.littletonrobotics.junction.Logger;
Expand All @@ -28,44 +28,49 @@ public class Arm extends SubsystemBase {
private static final LoggedTunableNumber kA = new LoggedTunableNumber("Arm/kA", gains.ffkA());
private static final LoggedTunableNumber kG = new LoggedTunableNumber("Arm/kG", gains.ffkG());
private static final LoggedTunableNumber armVelocity =
new LoggedTunableNumber("Arm/Velocity", profileConstraints.cruiseVelocityRadPerSec());
new LoggedTunableNumber("Arm/Velocity", profileConstraints.maxVelocity);
private static final LoggedTunableNumber armAcceleration =
new LoggedTunableNumber("Arm/Acceleration", profileConstraints.accelerationRadPerSec2());
new LoggedTunableNumber("Arm/Acceleration", profileConstraints.maxAcceleration);
private static final LoggedTunableNumber armToleranceDegreees =
new LoggedTunableNumber("Arm/ToleranceDegrees", positionTolerance.getDegrees());
private static final LoggedTunableNumber armLowerLimit =
new LoggedTunableNumber("Arm/LowerLimitDegrees", 15.0);
private static final LoggedTunableNumber armUpperLimit =
new LoggedTunableNumber("Arm/UpperLimitDegrees", 90.0);
private static LoggedTunableNumber armStowDegrees =
new LoggedTunableNumber("Superstructure/ArmStowDegrees", 20.0);
private static LoggedTunableNumber armStationIntakeDegrees =
new LoggedTunableNumber("Superstructure/ArmStationIntakeDegrees", 45.0);
private static LoggedTunableNumber armIntakeDegrees =
new LoggedTunableNumber("Superstructure/ArmIntakeDegrees", 40.0);

@RequiredArgsConstructor
public enum Goal {
STOW(() -> Units.degreesToRadians(armStowDegrees.get())),
FLOOR_INTAKE(() -> Units.degreesToRadians(armIntakeDegrees.get())),
STATION_INTAKE(() -> Units.degreesToRadians(armStowDegrees.get())),
AIM(() -> RobotState.getInstance().getAimingParameters().armAngle().getRadians());

private final DoubleSupplier armSetpointSupplier;

private double getArmSetpointRads() {
return armSetpointSupplier.getAsDouble();
}
}

@Getter @Setter Goal goal;

private final ArmIO io;
private final ArmIOInputsAutoLogged inputs = new ArmIOInputsAutoLogged();

private final Mechanism2d armMechanism;
private final MechanismRoot2d armRoot;
private final MechanismLigament2d armMeasured;

private boolean homed = false;
@Setter private Rotation2d setpoint = null;

// private final MechanismLigament2d armSetpoint;

public Arm(ArmIO io) {
System.out.println("[Init] Creating Arm");
this.io = io;
io.setBrakeMode(true);

// Create a mechanism
armMechanism = new Mechanism2d(2, 3, new Color8Bit(Color.kAntiqueWhite));
armRoot = armMechanism.getRoot("Arm Joint", armOrigin.getX(), armOrigin.getY());
armMeasured =
new MechanismLigament2d(
"Arm Measured",
armLength,
Units.radiansToDegrees(inputs.armPositionRads),
2.0,
new Color8Bit(Color.kBlack));
armRoot.append(armMeasured);
setDefaultCommand(stowCommand());
}

@Override
Expand All @@ -85,26 +90,17 @@ public void periodic() {
armVelocity,
armAcceleration);

if (setpoint != null) {
io.setSetpoint(
MathUtil.clamp(
setpoint.getRadians(),
Units.degreesToRadians(armLowerLimit.get()),
Units.degreesToRadians(armUpperLimit.get())));
}
io.runSetpoint(
MathUtil.clamp(
goal.getArmSetpointRads(),
Units.degreesToRadians(armLowerLimit.get()),
Units.degreesToRadians(armUpperLimit.get())));

if (DriverStation.isDisabled()) {
io.stop();
}

// Logs
armMeasured.setAngle(Units.radiansToDegrees(inputs.armPositionRads));
Logger.recordOutput("Arm/Mechanism", armMechanism);
}

public void runVolts(double volts) {
setpoint = null;
io.runVolts(volts);
Logger.recordOutput("Arm/Goal", goal);
}

public void stop() {
Expand All @@ -117,7 +113,7 @@ public Rotation2d getAngle() {

@AutoLogOutput(key = "Arm/SetpointAngle")
public Rotation2d getSetpoint() {
return setpoint != null ? setpoint : new Rotation2d();
return Rotation2d.fromRadians(goal.getArmSetpointRads());
}

@AutoLogOutput(key = "Arm/Homed")
Expand All @@ -127,9 +123,28 @@ public boolean homed() {

@AutoLogOutput(key = "Arm/AtSetpoint")
public boolean atSetpoint() {
return setpoint != null
&& Math.abs(Rotation2d.fromRadians(inputs.armPositionRads).minus(setpoint).getDegrees())
<= armToleranceDegreees.get();
return Math.abs(inputs.armPositionRads - goal.getArmSetpointRads())
<= Units.degreesToRadians(armToleranceDegreees.get());
}

public Command stowCommand() {
return runOnce(() -> setGoal(Goal.STOW)).andThen(Commands.idle()).withName("Arm Stow");
}

public Command intakeCommand() {
return runOnce(() -> setGoal(Goal.FLOOR_INTAKE))
.andThen(Commands.idle())
.withName("Arm Intake");
}

public Command stationIntakeCommand() {
return runOnce(() -> setGoal(Goal.STATION_INTAKE))
.andThen(Commands.idle())
.withName("Arm Station Intake");
}

public Command aimCommand() {
return runOnce(() -> setGoal(Goal.AIM)).andThen(Commands.idle()).withName("Arm Aim");
}

public Command getStaticCurrent() {
Expand Down
81 changes: 38 additions & 43 deletions src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,51 +2,46 @@

import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.math.util.Units;
import org.littletonrobotics.frc2024.Constants;

public class ArmConstants {
// reduction is 12:62 18:60 12:65
public static double reduction = (62.0 / 12.0) * (60.0 / 18.0) * (65.0 / 12.0);
public static Rotation2d positionTolerance = Rotation2d.fromDegrees(3.0);
public static Translation2d armOrigin =
new Translation2d(-Units.inchesToMeters(9.11), Units.inchesToMeters(11.75));
public static Rotation2d minAngle = Rotation2d.fromDegrees(10.0);
public static Rotation2d maxAngle = Rotation2d.fromDegrees(110.0);

public static int leaderID = 25;
public static int followerID = 26;
public static int armEncoderID = 42;

public static boolean leaderInverted = false;
public static boolean followerInverted = false;

/**
* The offset of the arm encoder in rotations.
*/
public static double armEncoderOffsetRotations =
Units.radiansToRotations(1.2747380347329678 + Math.PI / 2.0);

public static double armLength =
switch (Constants.getRobot()) {
case DEVBOT -> Units.inchesToMeters(24.8);
default -> Units.inchesToMeters(25.866);
};

public static Gains gains =
switch (Constants.getRobot()) {
case SIMBOT -> new Gains(0.0, 0.0, 0.0, 0.02, 1.0, 0.0, 0.01);
case DEVBOT -> new Gains(1200, 0.0, 120, 6.22, 0.0, 0.0, 8.12);
case COMPBOT -> new Gains(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
};

public static ProfileConstraints profileConstraints = new ProfileConstraints(2 * Math.PI, 10);

public record ProfileConstraints(
double cruiseVelocityRadPerSec, double accelerationRadPerSec2) {
}

public record Gains(
double kP, double kI, double kD, double ffkS, double ffkV, double ffkA, double ffkG) {
}
// reduction is 12:62 18:60 12:65
public static double reduction = (62.0 / 12.0) * (60.0 / 18.0) * (65.0 / 12.0);
public static Rotation2d positionTolerance = Rotation2d.fromDegrees(3.0);
public static Translation2d armOrigin =
new Translation2d(-Units.inchesToMeters(9.11), Units.inchesToMeters(11.75));
public static Rotation2d minAngle = Rotation2d.fromDegrees(10.0);
public static Rotation2d maxAngle = Rotation2d.fromDegrees(110.0);

public static int leaderID = 25;
public static int followerID = 26;
public static int armEncoderID = 42;

public static boolean leaderInverted = false;
public static boolean followerInverted = false;

/** The offset of the arm encoder in rotations. */
public static double armEncoderOffsetRotations =
Units.radiansToRotations(1.2747380347329678 + Math.PI / 2.0);

public static double armLength =
switch (Constants.getRobot()) {
case DEVBOT -> Units.inchesToMeters(24.8);
default -> Units.inchesToMeters(25.866);
};

public static Gains gains =
switch (Constants.getRobot()) {
case SIMBOT -> new Gains(0.0, 0.0, 0.0, 0.02, 1.0, 0.0, 0.01);
case DEVBOT -> new Gains(1200, 0.0, 120, 6.22, 0.0, 0.0, 8.12);
case COMPBOT -> new Gains(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
};

public static TrapezoidProfile.Constraints profileConstraints =
new TrapezoidProfile.Constraints(2 * Math.PI, 10);

public record Gains(
double kP, double kI, double kD, double ffkS, double ffkV, double ffkA, double ffkG) {}
}
9 changes: 3 additions & 6 deletions src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIO.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,25 +5,22 @@
public interface ArmIO {
@AutoLog
class ArmIOInputs {
public boolean hasFoc = false;
public boolean hasAbsoluteSensor = false;
public double armPositionRads = 0.0;

public double armEncoderPositionRads = 0.0;

public double armEncoderAbsolutePositionRads = 0.0;
public double armAbsoluteEncoderPositionRads = 0.0;
public double armTrajectorySetpointRads = 0.0;
public double armVelocityRadsPerSec = 0.0;
public double[] armAppliedVolts = new double[] {};
public double[] armCurrentAmps = new double[] {};
public double[] armTorqueCurrentAmps = new double[] {};
public double[] armTempCelcius = new double[] {};
public boolean absoluteEncoderConnected = true;
}

default void updateInputs(ArmIOInputs inputs) {}

/** Run to setpoint angle in radians */
default void setSetpoint(double setpointRads) {}
default void runSetpoint(double setpointRads) {}

/** Run motors at volts */
default void runVolts(double volts) {}
Expand Down
Loading

0 comments on commit bc9936d

Please sign in to comment.