Skip to content

Commit

Permalink
Feature disable motion magic (#11)
Browse files Browse the repository at this point in the history 
* Move Arm commands into superstructure
Add ArmVisualizer into Arm
Make sim work for Arm

* Add sim for all rollers
Change Goal names

* Sim works in teleop

* Disable motion magic in ArmIO
Move profile in to subsystem

* Resolves #11

* Fix Alerts in ArmIOKrakenFOC.java
Visualizer working

* Fix Alerts in ArmIOKrakenFOC.java
Visualizer working

* Add models

* Fix Northstar camera names

* Move robot to subfolder

Compatible with custom user assets folder

* Check arm at goal using profile

* Fix imports

* Check for setpoint and goal equal in arm

---------

Co-authored-by: Jonah <[email protected]>
  • Loading branch information
@suryatho @jwbonner
suryatho and jwbonner authored Feb 16, 2024
1 parent 5a40423 commit 557f9f6
Show file tree
Hide file tree
Showing 25 changed files with 491 additions and 307 deletions.
85 changes: 85 additions & 0 deletions ascope_assets/Robot_MA24B/config.json
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,85 @@
{
"name": "MA24B",
"rotations": [
{
"axis": "x",
"degrees": 90
},
{
"axis": "z",
"degrees": 90
}
],
"position": [
0,
0,
0
],
"cameras": [
{
"name": "Northstar 0",
"rotations": [
{
"axis": "y",
"degrees": -28.125
},
{
"axis": "z",
"degrees": 30.0
}
],
"position": [
0.247269,
0.24729186,
0.2244598
],
"resolution": [
1600,
1200
],
"fov": 75
},
{
"name": "Northstar 1",
"rotations": [
{
"axis": "y",
"degrees": -28.125
},
{
"axis": "z",
"degrees": -30.0
}
],
"position": [
0.247269,
-0.24729186,
0.2244598
],
"resolution": [
1600,
1200
],
"fov": 75
}
],
"components": [
{
"zeroedRotations": [
{
"axis": "x",
"degrees": 90
},
{
"axis": "z",
"degrees": 90
}
],
"zeroedPosition": [
0.238,
0.0,
-0.298
]
}
]
}
Binary file added ascope_assets/Robot_MA24B/model.glb
View file
Binary file not shown.
Binary file added ascope_assets/Robot_MA24B/model_0.glb
View file
Binary file not shown.
53 changes: 33 additions & 20 deletions src/main/java/org/littletonrobotics/frc2024/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,12 +35,15 @@
import org.littletonrobotics.frc2024.subsystems.rollers.feeder.Feeder;
import org.littletonrobotics.frc2024.subsystems.rollers.feeder.FeederIO;
import org.littletonrobotics.frc2024.subsystems.rollers.feeder.FeederIOKrakenFOC;
import org.littletonrobotics.frc2024.subsystems.rollers.feeder.FeederIOSim;
import org.littletonrobotics.frc2024.subsystems.rollers.indexer.Indexer;
import org.littletonrobotics.frc2024.subsystems.rollers.indexer.IndexerIO;
import org.littletonrobotics.frc2024.subsystems.rollers.indexer.IndexerIOSim;
import org.littletonrobotics.frc2024.subsystems.rollers.indexer.IndexerIOSparkFlex;
import org.littletonrobotics.frc2024.subsystems.rollers.intake.Intake;
import org.littletonrobotics.frc2024.subsystems.rollers.intake.IntakeIO;
import org.littletonrobotics.frc2024.subsystems.rollers.intake.IntakeIOKrakenFOC;
import org.littletonrobotics.frc2024.subsystems.rollers.intake.IntakeIOSim;
import org.littletonrobotics.frc2024.subsystems.superstructure.Superstructure;
import org.littletonrobotics.frc2024.subsystems.superstructure.arm.Arm;
import org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmIO;
Expand Down Expand Up @@ -121,11 +124,18 @@ public RobotContainer() {
new ModuleIOSim(DriveConstants.moduleConfigs[2]),
new ModuleIOSim(DriveConstants.moduleConfigs[3]));
flywheels = new Flywheels(new FlywheelsIOSim());

feeder = new Feeder(new FeederIOSim());
indexer = new Indexer(new IndexerIOSim());
intake = new Intake(new IntakeIOSim());
rollers = new Rollers(feeder, indexer, intake, new RollersSensorsIO() {});

arm = new Arm(new ArmIOSim());
}
}
}

// No-op implementation for replay
if (drive == null) {
drive =
new Drive(
Expand Down Expand Up @@ -200,20 +210,20 @@ private void configureButtonBindings() {
() ->
drive.setTeleopDriveGoal(
-controller.getLeftY(), -controller.getLeftX(), -controller.getRightX()))
.withName("DriveTeleop"));
.withName("Drive Teleop Input"));

// Aim and Rev Flywheels
controller
.a()
.whileTrue(
Commands.startEnd(drive::setAutoAimGoal, drive::clearAutoAimGoal)
.alongWith(superstructure.aimCommand(), flywheels.shootCommand()));

Trigger readyToShootTrigger =
.alongWith(superstructure.aim(), flywheels.shoot())
.withName("Aim"));
// Shoot
Trigger readyToShoot =
new Trigger(
() ->
drive.isAutoAimGoalCompleted()
&& flywheels.atSetpoint()
&& superstructure.atArmSetpoint());
readyToShootTrigger
() -> drive.isAutoAimGoalCompleted() && superstructure.atGoal() && flywheels.atGoal());
readyToShoot
.whileTrue(
Commands.run(
() -> controller.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 1.0)))
Expand All @@ -222,26 +232,29 @@ private void configureButtonBindings() {
() -> controller.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 0.0)));
controller
.rightTrigger()
.and(readyToShootTrigger)
.and(readyToShoot)
.onTrue(
rollers
.feedShooterCommand()
.withTimeout(1.0)
// Take over superstructure and flywheels, cancelling the main aiming command
.deadlineWith(superstructure.aimCommand(), flywheels.shootCommand()));

Commands.parallel(
Commands.waitSeconds(0.5),
Commands.waitUntil(controller.rightTrigger().negate()))
.deadlineWith(rollers.feedShooter(), superstructure.aim(), flywheels.shoot()));
// Intake Floor
controller
.leftTrigger()
.whileTrue(
superstructure
.floorIntakeCommand()
.alongWith(Commands.waitSeconds(0.25).andThen(rollers.floorIntakeCommand())));
.intake()
.alongWith(
Commands.waitUntil(superstructure::atGoal).andThen(rollers.floorIntake()))
.withName("Floor Intake"));
// Eject Floor
controller
.leftBumper()
.whileTrue(
superstructure
.floorIntakeCommand()
.alongWith(Commands.waitSeconds(0.25).andThen(rollers.ejectFloorCommand())));
.intake()
.alongWith(Commands.waitUntil(superstructure::atGoal).andThen(rollers.ejectFloor()))
.withName("Eject To Floor"));

controller
.y()
Expand Down
1 change: 0 additions & 1 deletion src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSim.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,6 @@
import edu.wpi.first.wpilibj.simulation.DCMotorSim;

public class ModuleIOSim implements ModuleIO {

private final DCMotorSim driveSim =
new DCMotorSim(DCMotor.getKrakenX60Foc(1), moduleConstants.driveReduction(), 0.025);
private final DCMotorSim turnSim =
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,7 @@ public class FlywheelConstants {
case SIMBOT -> new Gains(0.0, 0.0, 0.0, 0.09078, 0.00103, 0.0);
};

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

public record Gains(double kP, double kI, double kD, double kS, double kV, double kA) {}
}
80 changes: 42 additions & 38 deletions src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,31 +26,30 @@ public class Flywheels extends SubsystemBase {
private static final LoggedTunableNumber kS = new LoggedTunableNumber("Flywheels/kS", gains.kS());
private static final LoggedTunableNumber kV = new LoggedTunableNumber("Flywheels/kV", gains.kV());
private static final LoggedTunableNumber kA = new LoggedTunableNumber("Flywheels/kA", gains.kA());
private static LoggedTunableNumber shootingLeftRPM =
new LoggedTunableNumber("Superstructure/ShootingLeftRPM", 6000.0);
private static LoggedTunableNumber shootingRightRPM =
new LoggedTunableNumber("Superstructure/ShootingRightRPM", 4000.0);
private static LoggedTunableNumber idleLeftRPM =
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 shootingLeftRpm =
new LoggedTunableNumber("Superstructure/ShootingLeftRpm", 6000.0);
private static final LoggedTunableNumber shootingRightRpm =
new LoggedTunableNumber("Superstructure/ShootingRightRpm", 4000.0);
private static final LoggedTunableNumber idleLeftRpm =
new LoggedTunableNumber("Superstructure/IdleLeftRpm", 1500.0);
private static final LoggedTunableNumber idleRightRpm =
new LoggedTunableNumber("Superstructure/IdleRightRpm", 1000.0);
private static final LoggedTunableNumber intakingRpm =
new LoggedTunableNumber("Superstructure/IntakingRpm", -2000.0);
private static final LoggedTunableNumber ejectingRpm =
new LoggedTunableNumber("Superstructure/EjectingRpm", 2000.0);
private static final LoggedTunableNumber shooterTolerance =
new LoggedTunableNumber("Flywheels/ToleranceRPM", config.toleranceRPM());
new LoggedTunableNumber("Flywheels/ToleranceRpm", config.toleranceRpm());

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

@RequiredArgsConstructor
public enum Goal {
STOP(() -> 0.0, () -> 0.0),
IDLE(idleLeftRPM, idleRightRPM),
SHOOTING(shootingLeftRPM, shootingRightRPM),
INTAKING(intakingLeftRPM, intakingRightRPM),
IDLE(idleLeftRpm, idleRightRpm),
SHOOT(shootingLeftRpm, shootingRightRpm),
INTAKE(intakingRpm, intakingRpm),
EJECT(ejectingRpm, ejectingRpm),
CHARACTERIZING(() -> 0.0, () -> 0.0);

private final DoubleSupplier leftSetpoint;
Expand All @@ -69,7 +68,8 @@ private double getRightSetpoint() {

public Flywheels(FlywheelsIO io) {
this.io = io;
setDefaultCommand(runOnce(() -> goal = Goal.IDLE).withName("FlywheelsIdle"));

setDefaultCommand(runOnce(this::goIdle).withName("Flywheels Idle"));
}

@Override
Expand All @@ -83,20 +83,18 @@ public void periodic() {
hashCode(), kSVA -> io.setFF(kSVA[0], kSVA[1], kSVA[2]), kS, kV, kA);

if (DriverStation.isDisabled()) {
goal = Goal.STOP;
io.stop();
}

switch (goal) {
case STOP -> io.stop();
case CHARACTERIZING -> {} // Handled by runCharacterizationVolts
default -> io.runVelocity(goal.getLeftSetpoint(), goal.getRightSetpoint());
if (goal != Goal.CHARACTERIZING) {
io.runVelocity(goal.getLeftSetpoint(), goal.getRightSetpoint());
}

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/LeftSetpointRpm", goal.getLeftSetpoint());
Logger.recordOutput("Flywheels/RightSetpointRpm", goal.getRightSetpoint());
Logger.recordOutput("Flywheels/LeftRpm", inputs.leftVelocityRpm);
Logger.recordOutput("Flywheels/RightRpm", inputs.rightVelocityRpm);
}

public void runCharacterizationVolts(double volts) {
Expand All @@ -109,19 +107,25 @@ 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())
<= shooterTolerance.get();
@AutoLogOutput(key = "Flywheels/AtGoal")
public boolean atGoal() {
return Math.abs(inputs.leftVelocityRpm - goal.getLeftSetpoint()) <= shooterTolerance.get()
&& Math.abs(inputs.rightVelocityRpm - goal.getRightSetpoint()) <= shooterTolerance.get();
}

private void goIdle() {
goal = Goal.IDLE;
}

public Command shoot() {
return startEnd(() -> goal = Goal.SHOOT, this::goIdle).withName("Flywheels Shooting");
}

public Command shootCommand() {
return startEnd(() -> goal = Goal.SHOOTING, () -> goal = Goal.IDLE).withName("FlywheelsShoot");
public Command intake() {
return startEnd(() -> goal = Goal.INTAKE, this::goIdle).withName("Flywheels Intaking");
}

public Command intakeCommand() {
return startEnd(() -> goal = Goal.INTAKING, () -> goal = Goal.IDLE).withName("FlywheelsIntake");
public Command eject() {
return startEnd(() -> goal = Goal.EJECT, this::goIdle).withName("Flywheels Ejecting");
}
}
2 changes: 1 addition & 1 deletion src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,6 @@ public void periodic() {
Logger.processInputs(name, inputs);

io.runVolts(getGoal().getVoltageSupplier().getAsDouble());
Logger.recordOutput(name + "/Goal", getGoal().toString());
Logger.recordOutput("Rollers/" + name + "Goal", getGoal().toString());
}
}
1 change: 1 addition & 0 deletions src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSystemIO.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@ abstract class GenericRollerSystemIOInputs {
public double velocityRadsPerSec = 0.0;
public double appliedVoltage = 0.0;
public double outputCurrent = 0.0;
public double tempCelsius = 0.0;
}

default void updateInputs(GenericRollerSystemIOInputs inputs) {}
Expand Down
5 changes: 4 additions & 1 deletion ...java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSystemIOKrakenFOC.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,7 @@ public abstract class GenericRollerSystemIOKrakenFOC implements GenericRollerSys
private final StatusSignal<Double> velocity;
private final StatusSignal<Double> appliedVoltage;
private final StatusSignal<Double> outputCurrent;
private final StatusSignal<Double> tempCelsius;

// Single shot for voltage mode, robot loop will call continuously
private final VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true).withUpdateFreqHz(0);
Expand All @@ -49,9 +50,10 @@ public GenericRollerSystemIOKrakenFOC(
velocity = motor.getVelocity();
appliedVoltage = motor.getMotorVoltage();
outputCurrent = motor.getTorqueCurrent();
tempCelsius = motor.getDeviceTemp();

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

@Override
Expand All @@ -62,6 +64,7 @@ public void updateInputs(GenericRollerSystemIOInputs inputs) {
inputs.velocityRadsPerSec = Units.rotationsToRadians(velocity.getValueAsDouble()) / reduction;
inputs.appliedVoltage = appliedVoltage.getValueAsDouble();
inputs.outputCurrent = outputCurrent.getValueAsDouble();
inputs.tempCelsius = tempCelsius.getValueAsDouble();
}

@Override
Expand Down
Loading

0 comments on commit 557f9f6

Please sign in to comment.