Skip to content

Commit

Permalink
Meg remote-tracking branch origin/staging to tramp
Browse files Browse the repository at this point in the history
  • Loading branch information
NateLydem committed Feb 3, 2024
2 parents a5115c6 + 48618e7 commit 862ef8d
Show file tree
Hide file tree
Showing 15 changed files with 860 additions and 14 deletions.
58 changes: 57 additions & 1 deletion src/main/java/org/team1540/robot2024/Constants.java
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
package org.team1540.robot2024;

import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.util.Units;

/**
Expand All @@ -12,9 +13,10 @@
*/
public final class Constants {
public static final boolean IS_COMPETITION_ROBOT = true;
// Whether to pull PID constants from SmartDashboard
public static final boolean tuningMode = true; // TODO: DO NOT SET TO TRUE FOR COMP
private static final Mode simMode = Mode.SIM; // Can also be Mode.REPLAY


public static final Mode currentMode = Robot.isReal() ? Mode.REAL : simMode;

public enum Mode {
Expand Down Expand Up @@ -43,6 +45,7 @@ public static class SwerveConfig {
public static final int BACK_LEFT = IS_COMPETITION_ROBOT ? 7 : 0;
public static final int BACK_RIGHT = IS_COMPETITION_ROBOT ? 1 : 0;
}

public static class Drivetrain {
public static final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0);
public static final double TURN_GEAR_RATIO = 150.0 / 7.0;
Expand All @@ -56,6 +59,59 @@ public static class Drivetrain {
public static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS;
}

public static class Shooter {
public static class Flywheels {
// TODO: determine ids
public static final int LEFT_ID = 0;
public static final int RIGHT_ID = 0;

public static final double GEAR_RATIO = 24.0 / 36.0;
public static final double SIM_MOI = 4.08232288e-4;

// TODO: if it's tuned in simulation, it's tuned in real life
public static final double KP = 0.4;
public static final double KI = 0.0;
public static final double KD = 0.0;
public static final double KS = 0.01146;
public static final double KV = 0.07485; // TODO: this is what recalc says, may have to tune

public static final double ERROR_TOLERANCE_RPM = 50;
}

public static class Pivot {
// TODO: determine ids
public static final int MOTOR_ID = 0;
public static final int CANCODER_ID = 0;

// TODO: figure this out
public static final double CANCODER_OFFSET_ROTS = 0;
// TODO: determine ratios
public static final double CANCODER_TO_PIVOT = 60.0 / 20.0;
public static final double MOTOR_TO_CANCODER = 33.0;
public static final double TOTAL_GEAR_RATIO = MOTOR_TO_CANCODER * CANCODER_TO_PIVOT;
public static final double SIM_LENGTH_METERS = Units.inchesToMeters(12.910);
// TODO: find the moi
public static final double SIM_MOI = 0.22552744227754662;

public static final Rotation2d MAX_ANGLE = Rotation2d.fromDegrees(60.0);
public static final Rotation2d MIN_ANGLE = Rotation2d.fromDegrees(8.0);

// TODO: tune pid
public static final double KP = 0.1;
public static final double KI = 0.0;
public static final double KD = 0.0;
public static final double KS = 0.0;
public static final double KG = 0.1;
public static final double KV = 0.1;

public static final double CRUISE_VELOCITY_RPS = 4.0;
public static final double MAX_ACCEL_RPS2 = 40.0;
public static final double JERK_RPS3 = 2000;

public static final Rotation2d ERROR_TOLERANCE = Rotation2d.fromDegrees(0.2);
}
}

public static class Elevator {
public static final double CHAIN_HEIGHT_METERS = Units.inchesToMeters(28.25);
public static final double ELEVATOR_MAX_HEIGHT = Units.inchesToMeters(48.0);
Expand Down
4 changes: 4 additions & 0 deletions src/main/java/org/team1540/robot2024/Robot.java
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@
import org.littletonrobotics.junction.networktables.NT4Publisher;
import org.littletonrobotics.junction.wpilog.WPILOGReader;
import org.littletonrobotics.junction.wpilog.WPILOGWriter;
import org.team1540.robot2024.util.MechanismVisualiser;

/**
* The VM is configured to automatically run this class, and to call the functions corresponding to
Expand Down Expand Up @@ -89,6 +90,9 @@ public void robotPeriodic() {
// This must be called from the robot's periodic block in order for anything in
// the Command-based framework to work.
CommandScheduler.getInstance().run();

// Update mechanism visualiser in sim
if (Robot.isSimulation()) MechanismVisualiser.periodic();
}

/**
Expand Down
33 changes: 22 additions & 11 deletions src/main/java/org/team1540/robot2024/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -9,14 +9,15 @@
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;


import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
import org.team1540.robot2024.commands.FeedForwardCharacterization;
import org.team1540.robot2024.commands.SwerveDriveCommand;
import org.team1540.robot2024.subsystems.drive.*;
import org.team1540.robot2024.subsystems.tramp.Tramp;
import org.team1540.robot2024.subsystems.tramp.TrampIO;
import org.team1540.robot2024.subsystems.tramp.TrampIOSim;
import org.team1540.robot2024.subsystems.tramp.TrampIOSparkMax;
import org.team1540.robot2024.subsystems.shooter.*;
import org.team1540.robot2024.util.swerve.SwerveFactory;
import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;

Expand All @@ -32,6 +33,7 @@ public class RobotContainer {
// Subsystems
public final Drivetrain drivetrain;
public final Tramp tramp;
public final Shooter shooter;

// Controller
public final CommandXboxController driver = new CommandXboxController(0);
Expand All @@ -40,6 +42,10 @@ public class RobotContainer {
// Dashboard inputs
public final LoggedDashboardChooser<Command> autoChooser;

// TODO: testing dashboard inputs, remove for comp
public final LoggedDashboardNumber leftFlywheelSetpoint = new LoggedDashboardNumber("Shooter/Flywheels/leftSetpoint", 6000);
public final LoggedDashboardNumber rightFlywheelSetpoint = new LoggedDashboardNumber("Shooter/Flywheels/rightSetpoint", 6000);

/**
* The container for the robot. Contains subsystems, OI devices, and commands.
*/
Expand All @@ -55,6 +61,7 @@ public RobotContainer() {
new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_LEFT, SwerveFactory.SwerveCorner.BACK_LEFT)),
new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_RIGHT, SwerveFactory.SwerveCorner.BACK_RIGHT)));
tramp = new Tramp(new TrampIOSparkMax());
shooter = new Shooter(new ShooterPivotIOTalonFX(), new FlywheelsIOTalonFX());
break;
case SIM:
// Sim robot, instantiate physics sim IO implementations
Expand All @@ -66,22 +73,19 @@ public RobotContainer() {
new ModuleIOSim(),
new ModuleIOSim());
tramp = new Tramp(new TrampIOSim());
shooter = new Shooter(new ShooterPivotIOSim(), new FlywheelsIOSim());
break;

default:
// Replayed robot, disable IO implementations
drivetrain =
new Drivetrain(
new GyroIO() {
},
new ModuleIO() {
},
new ModuleIO() {
},
new ModuleIO() {
},
new ModuleIO() {
});
new GyroIO() {},
new ModuleIO() {},
new ModuleIO() {},
new ModuleIO() {},
new ModuleIO() {});
shooter = new Shooter(new ShooterPivotIO() {}, new FlywheelsIO() {});
tramp = new Tramp(new TrampIO() {});
break;
}
Expand All @@ -95,6 +99,10 @@ public RobotContainer() {
"Drive FF Characterization",
new FeedForwardCharacterization(
drivetrain, drivetrain::runCharacterizationVolts, drivetrain::getCharacterizationVelocity));
autoChooser.addOption(
"Flywheels FF Characterization",
new FeedForwardCharacterization(
shooter, volts -> shooter.setFlywheelVolts(volts, volts), () -> shooter.getLeftFlywheelSpeed() / 60));

// Configure the button bindings
configureButtonBindings();
Expand All @@ -115,6 +123,9 @@ private void configureButtonBindings() {
drivetrain
).ignoringDisable(true)
);

copilot.a().onTrue(shooter.spinUpCommand(leftFlywheelSetpoint.get(), rightFlywheelSetpoint.get()))
.onFalse(Commands.runOnce(shooter::stopFlywheels, shooter));
}

/**
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Subsystem;
import org.team1540.robot2024.util.PolynomialRegression;
import org.team1540.robot2024.util.math.PolynomialRegression;

import java.util.LinkedList;
import java.util.List;
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,37 @@
package org.team1540.robot2024.subsystems.shooter;

import org.littletonrobotics.junction.AutoLog;

public interface FlywheelsIO {
@AutoLog
class FlywheelsIOInputs {
public double leftAppliedVolts = 0.0;
public double leftCurrentAmps = 0.0;
public double leftVelocityRPM = 0.0;

public double rightAppliedVolts = 0.0;
public double rightCurrentAmps = 0.0;
public double rightVelocityRPM = 0.0;
}

/**
* Updates the set of loggable inputs
*/
default void updateInputs(FlywheelsIOInputs inputs) {}

/**
* Runs open loop at the specified voltages
*/
default void setVoltage(double leftVolts, double rightVolts) {}


/**
* Runs closed loop at the specified RPMs
*/
default void setSpeeds(double leftRPM, double rightRPM) {}

/**
* Configures the PID controller
*/
default void configPID(double kP, double kI, double kD) {}
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,74 @@
package org.team1540.robot2024.subsystems.shooter;

import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.wpilibj.simulation.FlywheelSim;
import org.team1540.robot2024.Constants;

import static org.team1540.robot2024.Constants.Shooter.Flywheels.*;

public class FlywheelsIOSim implements FlywheelsIO{
private final FlywheelSim leftSim =
new FlywheelSim(DCMotor.getFalcon500(1), GEAR_RATIO, SIM_MOI);
private final FlywheelSim rightSim =
new FlywheelSim(DCMotor.getFalcon500(1), GEAR_RATIO, SIM_MOI);

private final PIDController rightController = new PIDController(KP, KI, KD);
private final PIDController leftController = new PIDController(KP, KI, KD);
private final SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(KS, KV);

private boolean isClosedLoop;
private double leftSetpointRPS;
private double rightSetpointRPS;

private double leftVolts = 0.0;
private double rightVolts = 0.0;

@Override
public void updateInputs(FlywheelsIOInputs inputs) {
if (isClosedLoop) {
leftVolts =
leftController.calculate(leftSim.getAngularVelocityRPM() / 60, leftSetpointRPS)
+ feedforward.calculate(leftSetpointRPS);
rightVolts =
rightController.calculate(rightSim.getAngularVelocityRPM() / 60, rightSetpointRPS)
+ feedforward.calculate(rightSetpointRPS);
}

leftSim.setInputVoltage(leftVolts);
rightSim.setInputVoltage(rightVolts);
leftSim.update(Constants.LOOP_PERIOD_SECS);
rightSim.update(Constants.LOOP_PERIOD_SECS);

inputs.leftVelocityRPM = leftSim.getAngularVelocityRPM();
inputs.leftAppliedVolts = leftVolts;
inputs.leftCurrentAmps = leftSim.getCurrentDrawAmps();

inputs.rightVelocityRPM = rightSim.getAngularVelocityRPM();
inputs.rightAppliedVolts = rightVolts;
inputs.rightCurrentAmps = rightSim.getCurrentDrawAmps();
}

@Override
public void setVoltage(double leftVolts, double rightVolts) {
isClosedLoop = false;
this.leftVolts = leftVolts;
this.rightVolts = rightVolts;
}

@Override
public void setSpeeds(double leftRPM, double rightRPM) {
isClosedLoop = true;
leftController.reset();
rightController.reset();
leftSetpointRPS = leftRPM / 60;
rightSetpointRPS = rightRPM / 60;
}

@Override
public void configPID(double kP, double kI, double kD) {
leftController.setPID(kP, kI, kD);
rightController.setPID(kP, kI, kD);
}
}
Loading

0 comments on commit 862ef8d

Please sign in to comment.