Skip to content

Commit 862ef8d

Browse files
NateLydemNateLydem
committed
Meg remote-tracking branch origin/staging to tramp
2 parents a5115c6 + 48618e7 commit 862ef8d

15 files changed

+860
-14
lines changed

‎src/main/java/org/team1540/robot2024/Constants.java

+57-1
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,6 @@
11
package org.team1540.robot2024;
22

3+
import edu.wpi.first.math.geometry.Rotation2d;
34
import edu.wpi.first.math.util.Units;
45

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

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

2022
public enum Mode {
@@ -43,6 +45,7 @@ public static class SwerveConfig {
4345
public static final int BACK_LEFT = IS_COMPETITION_ROBOT ? 7 : 0;
4446
public static final int BACK_RIGHT = IS_COMPETITION_ROBOT ? 1 : 0;
4547
}
48+
4649
public static class Drivetrain {
4750
public static final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0);
4851
public static final double TURN_GEAR_RATIO = 150.0 / 7.0;
@@ -56,6 +59,59 @@ public static class Drivetrain {
5659
public static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS;
5760
}
5861

62+
public static class Shooter {
63+
public static class Flywheels {
64+
// TODO: determine ids
65+
public static final int LEFT_ID = 0;
66+
public static final int RIGHT_ID = 0;
67+
68+
public static final double GEAR_RATIO = 24.0 / 36.0;
69+
public static final double SIM_MOI = 4.08232288e-4;
70+
71+
// TODO: if it's tuned in simulation, it's tuned in real life
72+
public static final double KP = 0.4;
73+
public static final double KI = 0.0;
74+
public static final double KD = 0.0;
75+
public static final double KS = 0.01146;
76+
public static final double KV = 0.07485; // TODO: this is what recalc says, may have to tune
77+
78+
public static final double ERROR_TOLERANCE_RPM = 50;
79+
}
80+
81+
public static class Pivot {
82+
// TODO: determine ids
83+
public static final int MOTOR_ID = 0;
84+
public static final int CANCODER_ID = 0;
85+
86+
// TODO: figure this out
87+
public static final double CANCODER_OFFSET_ROTS = 0;
88+
// TODO: determine ratios
89+
public static final double CANCODER_TO_PIVOT = 60.0 / 20.0;
90+
public static final double MOTOR_TO_CANCODER = 33.0;
91+
public static final double TOTAL_GEAR_RATIO = MOTOR_TO_CANCODER * CANCODER_TO_PIVOT;
92+
public static final double SIM_LENGTH_METERS = Units.inchesToMeters(12.910);
93+
// TODO: find the moi
94+
public static final double SIM_MOI = 0.22552744227754662;
95+
96+
public static final Rotation2d MAX_ANGLE = Rotation2d.fromDegrees(60.0);
97+
public static final Rotation2d MIN_ANGLE = Rotation2d.fromDegrees(8.0);
98+
99+
// TODO: tune pid
100+
public static final double KP = 0.1;
101+
public static final double KI = 0.0;
102+
public static final double KD = 0.0;
103+
public static final double KS = 0.0;
104+
public static final double KG = 0.1;
105+
public static final double KV = 0.1;
106+
107+
public static final double CRUISE_VELOCITY_RPS = 4.0;
108+
public static final double MAX_ACCEL_RPS2 = 40.0;
109+
public static final double JERK_RPS3 = 2000;
110+
111+
public static final Rotation2d ERROR_TOLERANCE = Rotation2d.fromDegrees(0.2);
112+
}
113+
}
114+
59115
public static class Elevator {
60116
public static final double CHAIN_HEIGHT_METERS = Units.inchesToMeters(28.25);
61117
public static final double ELEVATOR_MAX_HEIGHT = Units.inchesToMeters(48.0);

‎src/main/java/org/team1540/robot2024/Robot.java

+4
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,7 @@
99
import org.littletonrobotics.junction.networktables.NT4Publisher;
1010
import org.littletonrobotics.junction.wpilog.WPILOGReader;
1111
import org.littletonrobotics.junction.wpilog.WPILOGWriter;
12+
import org.team1540.robot2024.util.MechanismVisualiser;
1213

1314
/**
1415
* The VM is configured to automatically run this class, and to call the functions corresponding to
@@ -89,6 +90,9 @@ public void robotPeriodic() {
8990
// This must be called from the robot's periodic block in order for anything in
9091
// the Command-based framework to work.
9192
CommandScheduler.getInstance().run();
93+
94+
// Update mechanism visualiser in sim
95+
if (Robot.isSimulation()) MechanismVisualiser.periodic();
9296
}
9397

9498
/**

‎src/main/java/org/team1540/robot2024/RobotContainer.java

+22-11
Original file line numberDiff line numberDiff line change
@@ -9,14 +9,15 @@
99
import edu.wpi.first.wpilibj2.command.Commands;
1010
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
1111

12-
12+
import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
1313
import org.team1540.robot2024.commands.FeedForwardCharacterization;
1414
import org.team1540.robot2024.commands.SwerveDriveCommand;
1515
import org.team1540.robot2024.subsystems.drive.*;
1616
import org.team1540.robot2024.subsystems.tramp.Tramp;
1717
import org.team1540.robot2024.subsystems.tramp.TrampIO;
1818
import org.team1540.robot2024.subsystems.tramp.TrampIOSim;
1919
import org.team1540.robot2024.subsystems.tramp.TrampIOSparkMax;
20+
import org.team1540.robot2024.subsystems.shooter.*;
2021
import org.team1540.robot2024.util.swerve.SwerveFactory;
2122
import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
2223

@@ -32,6 +33,7 @@ public class RobotContainer {
3233
// Subsystems
3334
public final Drivetrain drivetrain;
3435
public final Tramp tramp;
36+
public final Shooter shooter;
3537

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

45+
// TODO: testing dashboard inputs, remove for comp
46+
public final LoggedDashboardNumber leftFlywheelSetpoint = new LoggedDashboardNumber("Shooter/Flywheels/leftSetpoint", 6000);
47+
public final LoggedDashboardNumber rightFlywheelSetpoint = new LoggedDashboardNumber("Shooter/Flywheels/rightSetpoint", 6000);
48+
4349
/**
4450
* The container for the robot. Contains subsystems, OI devices, and commands.
4551
*/
@@ -55,6 +61,7 @@ public RobotContainer() {
5561
new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_LEFT, SwerveFactory.SwerveCorner.BACK_LEFT)),
5662
new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_RIGHT, SwerveFactory.SwerveCorner.BACK_RIGHT)));
5763
tramp = new Tramp(new TrampIOSparkMax());
64+
shooter = new Shooter(new ShooterPivotIOTalonFX(), new FlywheelsIOTalonFX());
5865
break;
5966
case SIM:
6067
// Sim robot, instantiate physics sim IO implementations
@@ -66,22 +73,19 @@ public RobotContainer() {
6673
new ModuleIOSim(),
6774
new ModuleIOSim());
6875
tramp = new Tramp(new TrampIOSim());
76+
shooter = new Shooter(new ShooterPivotIOSim(), new FlywheelsIOSim());
6977
break;
7078

7179
default:
7280
// Replayed robot, disable IO implementations
7381
drivetrain =
7482
new Drivetrain(
75-
new GyroIO() {
76-
},
77-
new ModuleIO() {
78-
},
79-
new ModuleIO() {
80-
},
81-
new ModuleIO() {
82-
},
83-
new ModuleIO() {
84-
});
83+
new GyroIO() {},
84+
new ModuleIO() {},
85+
new ModuleIO() {},
86+
new ModuleIO() {},
87+
new ModuleIO() {});
88+
shooter = new Shooter(new ShooterPivotIO() {}, new FlywheelsIO() {});
8589
tramp = new Tramp(new TrampIO() {});
8690
break;
8791
}
@@ -95,6 +99,10 @@ public RobotContainer() {
9599
"Drive FF Characterization",
96100
new FeedForwardCharacterization(
97101
drivetrain, drivetrain::runCharacterizationVolts, drivetrain::getCharacterizationVelocity));
102+
autoChooser.addOption(
103+
"Flywheels FF Characterization",
104+
new FeedForwardCharacterization(
105+
shooter, volts -> shooter.setFlywheelVolts(volts, volts), () -> shooter.getLeftFlywheelSpeed() / 60));
98106

99107
// Configure the button bindings
100108
configureButtonBindings();
@@ -115,6 +123,9 @@ private void configureButtonBindings() {
115123
drivetrain
116124
).ignoringDisable(true)
117125
);
126+
127+
copilot.a().onTrue(shooter.spinUpCommand(leftFlywheelSetpoint.get(), rightFlywheelSetpoint.get()))
128+
.onFalse(Commands.runOnce(shooter::stopFlywheels, shooter));
118129
}
119130

120131
/**

‎src/main/java/org/team1540/robot2024/commands/FeedForwardCharacterization.java

+1-1
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,7 @@
33
import edu.wpi.first.wpilibj.Timer;
44
import edu.wpi.first.wpilibj2.command.Command;
55
import edu.wpi.first.wpilibj2.command.Subsystem;
6-
import org.team1540.robot2024.util.PolynomialRegression;
6+
import org.team1540.robot2024.util.math.PolynomialRegression;
77

88
import java.util.LinkedList;
99
import java.util.List;

‎src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIO.java

+37
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,37 @@
1+
package org.team1540.robot2024.subsystems.shooter;
2+
3+
import org.littletonrobotics.junction.AutoLog;
4+
5+
public interface FlywheelsIO {
6+
@AutoLog
7+
class FlywheelsIOInputs {
8+
public double leftAppliedVolts = 0.0;
9+
public double leftCurrentAmps = 0.0;
10+
public double leftVelocityRPM = 0.0;
11+
12+
public double rightAppliedVolts = 0.0;
13+
public double rightCurrentAmps = 0.0;
14+
public double rightVelocityRPM = 0.0;
15+
}
16+
17+
/**
18+
* Updates the set of loggable inputs
19+
*/
20+
default void updateInputs(FlywheelsIOInputs inputs) {}
21+
22+
/**
23+
* Runs open loop at the specified voltages
24+
*/
25+
default void setVoltage(double leftVolts, double rightVolts) {}
26+
27+
28+
/**
29+
* Runs closed loop at the specified RPMs
30+
*/
31+
default void setSpeeds(double leftRPM, double rightRPM) {}
32+
33+
/**
34+
* Configures the PID controller
35+
*/
36+
default void configPID(double kP, double kI, double kD) {}
37+
}

‎src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIOSim.java

+74
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,74 @@
1+
package org.team1540.robot2024.subsystems.shooter;
2+
3+
import edu.wpi.first.math.controller.PIDController;
4+
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
5+
import edu.wpi.first.math.system.plant.DCMotor;
6+
import edu.wpi.first.wpilibj.simulation.FlywheelSim;
7+
import org.team1540.robot2024.Constants;
8+
9+
import static org.team1540.robot2024.Constants.Shooter.Flywheels.*;
10+
11+
public class FlywheelsIOSim implements FlywheelsIO{
12+
private final FlywheelSim leftSim =
13+
new FlywheelSim(DCMotor.getFalcon500(1), GEAR_RATIO, SIM_MOI);
14+
private final FlywheelSim rightSim =
15+
new FlywheelSim(DCMotor.getFalcon500(1), GEAR_RATIO, SIM_MOI);
16+
17+
private final PIDController rightController = new PIDController(KP, KI, KD);
18+
private final PIDController leftController = new PIDController(KP, KI, KD);
19+
private final SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(KS, KV);
20+
21+
private boolean isClosedLoop;
22+
private double leftSetpointRPS;
23+
private double rightSetpointRPS;
24+
25+
private double leftVolts = 0.0;
26+
private double rightVolts = 0.0;
27+
28+
@Override
29+
public void updateInputs(FlywheelsIOInputs inputs) {
30+
if (isClosedLoop) {
31+
leftVolts =
32+
leftController.calculate(leftSim.getAngularVelocityRPM() / 60, leftSetpointRPS)
33+
+ feedforward.calculate(leftSetpointRPS);
34+
rightVolts =
35+
rightController.calculate(rightSim.getAngularVelocityRPM() / 60, rightSetpointRPS)
36+
+ feedforward.calculate(rightSetpointRPS);
37+
}
38+
39+
leftSim.setInputVoltage(leftVolts);
40+
rightSim.setInputVoltage(rightVolts);
41+
leftSim.update(Constants.LOOP_PERIOD_SECS);
42+
rightSim.update(Constants.LOOP_PERIOD_SECS);
43+
44+
inputs.leftVelocityRPM = leftSim.getAngularVelocityRPM();
45+
inputs.leftAppliedVolts = leftVolts;
46+
inputs.leftCurrentAmps = leftSim.getCurrentDrawAmps();
47+
48+
inputs.rightVelocityRPM = rightSim.getAngularVelocityRPM();
49+
inputs.rightAppliedVolts = rightVolts;
50+
inputs.rightCurrentAmps = rightSim.getCurrentDrawAmps();
51+
}
52+
53+
@Override
54+
public void setVoltage(double leftVolts, double rightVolts) {
55+
isClosedLoop = false;
56+
this.leftVolts = leftVolts;
57+
this.rightVolts = rightVolts;
58+
}
59+
60+
@Override
61+
public void setSpeeds(double leftRPM, double rightRPM) {
62+
isClosedLoop = true;
63+
leftController.reset();
64+
rightController.reset();
65+
leftSetpointRPS = leftRPM / 60;
66+
rightSetpointRPS = rightRPM / 60;
67+
}
68+
69+
@Override
70+
public void configPID(double kP, double kI, double kD) {
71+
leftController.setPID(kP, kI, kD);
72+
rightController.setPID(kP, kI, kD);
73+
}
74+
}

0 commit comments

Comments
 (0)