Skip to content

Commit e23f9a4

Browse files
mimizh2418mimizh2418
committed
Merge branch 'main' into simulation
2 parents 22f70db + 14bbadf commit e23f9a4

15 files changed

+836
-24
lines changed

‎build.gradle

+1-1
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
plugins {
22
id "java"
3-
id "edu.wpi.first.GradleRIO" version "2024.1.1"
3+
id "edu.wpi.first.GradleRIO" version "2024.2.1"
44
id "com.peterabeles.gversion" version "1.10"
55
}
66

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

+53-4
Original file line numberDiff line numberDiff line change
@@ -13,9 +13,10 @@
1313
*/
1414
public final class Constants {
1515
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
1618
private static final Mode simMode = Mode.SIM; // Can also be Mode.REPLAY
1719

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

2122
public enum Mode {
@@ -62,8 +63,56 @@ public static class Elevator {
6263
public static final double ELEVATOR_MAX_HEIGHT = Units.inchesToMeters(21.0);
6364
}
6465

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

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

+22-11
Original file line numberDiff line numberDiff line change
@@ -9,10 +9,11 @@
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.*;
16+
import org.team1540.robot2024.subsystems.shooter.*;
1617
import org.team1540.robot2024.util.swerve.SwerveFactory;
1718
import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
1819

@@ -27,6 +28,7 @@
2728
public class RobotContainer {
2829
// Subsystems
2930
public final Drivetrain drivetrain;
31+
public final Shooter shooter;
3032

3133
// Controller
3234
public final CommandXboxController driver = new CommandXboxController(0);
@@ -35,6 +37,10 @@ public class RobotContainer {
3537
// Dashboard inputs
3638
public final LoggedDashboardChooser<Command> autoChooser;
3739

40+
// TODO: testing dashboard inputs, remove for comp
41+
public final LoggedDashboardNumber leftFlywheelSetpoint = new LoggedDashboardNumber("Shooter/Flywheels/leftSetpoint", 6000);
42+
public final LoggedDashboardNumber rightFlywheelSetpoint = new LoggedDashboardNumber("Shooter/Flywheels/rightSetpoint", 6000);
43+
3844
/**
3945
* The container for the robot. Contains subsystems, OI devices, and commands.
4046
*/
@@ -49,6 +55,7 @@ public RobotContainer() {
4955
new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.FRONT_RIGHT, SwerveFactory.SwerveCorner.FRONT_RIGHT)),
5056
new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_LEFT, SwerveFactory.SwerveCorner.BACK_LEFT)),
5157
new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_RIGHT, SwerveFactory.SwerveCorner.BACK_RIGHT)));
58+
shooter = new Shooter(new ShooterPivotIOTalonFX(), new FlywheelsIOTalonFX());
5259
break;
5360

5461
case SIM:
@@ -60,22 +67,19 @@ public RobotContainer() {
6067
new ModuleIOSim(),
6168
new ModuleIOSim(),
6269
new ModuleIOSim());
70+
shooter = new Shooter(new ShooterPivotIOSim(), new FlywheelsIOSim());
6371
break;
6472

6573
default:
6674
// Replayed robot, disable IO implementations
6775
drivetrain =
6876
new Drivetrain(
69-
new GyroIO() {
70-
},
71-
new ModuleIO() {
72-
},
73-
new ModuleIO() {
74-
},
75-
new ModuleIO() {
76-
},
77-
new ModuleIO() {
78-
});
77+
new GyroIO() {},
78+
new ModuleIO() {},
79+
new ModuleIO() {},
80+
new ModuleIO() {},
81+
new ModuleIO() {});
82+
shooter = new Shooter(new ShooterPivotIO() {}, new FlywheelsIO() {});
7983
break;
8084
}
8185

@@ -88,6 +92,10 @@ public RobotContainer() {
8892
"Drive FF Characterization",
8993
new FeedForwardCharacterization(
9094
drivetrain, drivetrain::runCharacterizationVolts, drivetrain::getCharacterizationVelocity));
95+
autoChooser.addOption(
96+
"Flywheels FF Characterization",
97+
new FeedForwardCharacterization(
98+
shooter, volts -> shooter.setFlywheelVolts(volts, volts), () -> shooter.getLeftFlywheelSpeed() / 60));
9199

92100
// Configure the button bindings
93101
configureButtonBindings();
@@ -108,6 +116,9 @@ private void configureButtonBindings() {
108116
drivetrain
109117
).ignoringDisable(true)
110118
);
119+
120+
copilot.a().onTrue(shooter.spinUpCommand(leftFlywheelSetpoint.get(), rightFlywheelSetpoint.get()))
121+
.onFalse(Commands.runOnce(shooter::stopFlywheels, shooter));
111122
}
112123

113124
/**

‎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)