Skip to content

Commit 01de5a8

Browse files
Liquid2112Liquid2112
authored
Merge pull request #12 from FRC-7525/Intake
2 parents 00e5fe7 + 5a157d7 commit 01de5a8

File tree

16 files changed

+1103
-30
lines changed

16 files changed

+1103
-30
lines changed

‎.github/scripts/constants.py‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,10 @@
33
import sys
44

55
# List of files to excuse (constants and things we didnt make and stuff we wont use)
6-
excused_files = ["Drive.java", "ShooterMath.java", "Vision.java", "DriveStates.java", "VisionIOPhotonVision.java"]
6+
excused_files = ["GlobalConstants.java", "Drive.java", "TunerConstants.java", "BuildConstants.java", "LocalADStarAK.java", "VisionUtil.java", "SwerveModule.java", "VisionIOSim.java", "ElevatorConstants.java", "IntakeConstants.java",
7+
"RepulsorFieldPlanner.java", "Force.java", "SubsystemManagerConstants.java", "AutoAlignConstants.java", "SubsystemManager.java", "AutoAlign.java", "AutoAlignStates.java", "CoralerConstants.java", "AlgaerConstants.java", "VisionConstants.java",
8+
"LEDStates.java", "LEDIOSim.java", "LEDIOReal.java", "AutoManager.java", "TippingCalculator.java", "WheelRadiusCharacterization.java", "ElevatorIOReal.java", "ElevatorIOSim.java", "FaultManager.java", "AlgaerIOSim.java", "DriveStates.java", "VisionIOPhotonVision.java", "Vision.java", "AutoConstants.java", "AutoManager.java", "SubsystemManagerStates.java", "CoralerStates.java",
9+
"PassthroughConstants.java", "RobotState.java", "ShooterConstants.java", "IntakeIOSim.java", "IntakeIOTalonFX.java", "ShooterMath.java"]
710

811
# Not really dirs becasue the full ones didnt work
912
excused_dirs = [

‎build.gradle‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -7,7 +7,7 @@ java {
77
sourceCompatibility = JavaVersion.VERSION_17
88
targetCompatibility = JavaVersion.VERSION_17
99
}
10-
10+
1111
def ROBOT_MAIN_CLASS = "frc.robot.Main"
1212

1313
task(replayWatch, type: JavaExec) {

‎src/main/java/frc/robot/GlobalConstants.java‎

Lines changed: 21 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -3,13 +3,22 @@
33
import static edu.wpi.first.units.Units.*;
44

55
import edu.wpi.first.units.measure.LinearAcceleration;
6+
import edu.wpi.first.units.measure.Mass;
7+
import edu.wpi.first.wpilibj.GenericHID;
68
import edu.wpi.first.wpilibj.XboxController;
9+
import edu.wpi.first.wpilibj.XboxController;
10+
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
711
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
12+
import java.util.ArrayList;
13+
import java.util.Arrays;
814

915
public class GlobalConstants {
1016

1117
public static final LinearAcceleration GRAVITY = MetersPerSecondPerSecond.of(9.81);
1218
public static final double SIMULATION_PERIOD = 0.02;
19+
// TODO: This is wrong
20+
public static final Mass ROBOT_MASS = Kilograms.of(60);
21+
1322
public static final Field2d FIELD = new Field2d();
1423

1524
public enum RobotMode {
@@ -23,7 +32,19 @@ public enum RobotMode {
2332
public static class Controllers {
2433

2534
public static final XboxController DRIVER_CONTROLLER = new XboxController(0);
35+
public static final GenericHID OPERATOR_CONTROLLER = new GenericHID(1);
36+
public static final XboxController TEST_CONTROLLER = new XboxController(4);
37+
38+
// NOTE: Set to 0.1 on trash controllers
2639
public static final double DEADBAND = 0.01;
2740
public static final double TRIGGERS_REGISTER_POINT = 0.5;
2841
}
42+
43+
public static class FaultManagerConstants {
44+
45+
public static final ArrayList<Integer> CANIVORE_DEVICE_ORDER = new ArrayList<Integer>(Arrays.asList(39, 56, 6, 4, 58, 9, 5, 11, 12, 2, 59, 3, 8));
46+
}
47+
48+
public static final double DEADBAND = 0.01;
49+
public static final double TRIGGERS_REGISTER_POINT = 0.5;
2950
}

‎src/main/java/frc/robot/Subsystems/Drive/Drive.java‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -115,7 +115,7 @@ public void logOutputs(SwerveDriveState state) {
115115
public void driveFieldRelative(double xVelocity, double yVelocity, double angularVelocity) {
116116
driveIO.setControl(
117117
new SwerveRequest.FieldCentric()
118-
.withDeadband(DEADBAND)
118+
.withDeadband(frc.robot.GlobalConstants.Controllers.DEADBAND)
119119
.withVelocityX(xVelocity)
120120
.withVelocityY(yVelocity)
121121
.withRotationalRate(angularVelocity)
@@ -128,7 +128,7 @@ public void driveFieldRelative(double xVelocity, double yVelocity, double angula
128128
public void driveRobotRelative(double xVelocity, double yVelocity, double angularVelocity) {
129129
driveIO.setControl(
130130
new SwerveRequest.RobotCentric()
131-
.withDeadband(DEADBAND)
131+
.withDeadband(frc.robot.GlobalConstants.Controllers.DEADBAND)
132132
.withVelocityX(xVelocity)
133133
.withVelocityY(yVelocity)
134134
.withRotationalRate(angularVelocity)

‎src/main/java/frc/robot/subsystems/Intake/Intake.java‎

Lines changed: 61 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,61 @@
1+
package frc.robot.Subsystems.Intake;
2+
3+
import static edu.wpi.first.units.Units.*;
4+
import static frc.robot.GlobalConstants.ROBOT_MODE;
5+
import static frc.robot.Subsystems.Intake.IntakeConstants.*;
6+
7+
import com.ctre.phoenix6.hardware.TalonFX;
8+
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
9+
import org.littletonrobotics.junction.Logger;
10+
import org.team7525.subsystem.Subsystem;
11+
12+
public class Intake extends Subsystem<IntakeStates> {
13+
14+
private static Intake instance;
15+
private final IntakeIO io;
16+
private final IntakeIO.IntakeIOInputs inputs = new IntakeIO.IntakeIOInputs();
17+
private final SimpleMotorFeedforward spinFF = new SimpleMotorFeedforward(SPIN_kS, SPIN_kV, SPIN_kA);
18+
19+
private Intake() {
20+
super(SUBSYSTEM_NAME, IntakeStates.IN);
21+
this.io = switch (ROBOT_MODE) {
22+
case SIM -> new IntakeIOSim();
23+
case REAL -> new IntakeIOTalonFX();
24+
case TESTING -> new IntakeIOTalonFX();
25+
};
26+
}
27+
28+
public static Intake getInstance() {
29+
if (instance == null) {
30+
instance = new Intake();
31+
}
32+
return instance;
33+
}
34+
35+
@Override
36+
protected void runState() {
37+
//asking for units explicitly
38+
double linearPos = getState().linearPos.in(Meters);
39+
double spinSpeed = getState().spinSpeed.in(RotationsPerSecond);
40+
41+
io.setLinearPosition(linearPos);
42+
io.setSpinVelocity(spinSpeed, spinFF.calculate(spinSpeed));
43+
io.updateInputs(inputs);
44+
45+
Logger.recordOutput(SUBSYSTEM_NAME + "/SpinVelocityRPS", inputs.spinVelocityRPS);
46+
Logger.recordOutput(SUBSYSTEM_NAME + "/SpinAppliedVolts", inputs.spinAppliedVolts);
47+
Logger.recordOutput(SUBSYSTEM_NAME + "/SpinCurrentAmps", inputs.spinCurrentAmps);
48+
Logger.recordOutput(SUBSYSTEM_NAME + "/LinearPositionMeters", inputs.linearPositionMeters);
49+
Logger.recordOutput(SUBSYSTEM_NAME + "/LinearVelocityMetersPerSec", inputs.linearVelocityMetersPerSec);
50+
Logger.recordOutput(SUBSYSTEM_NAME + "/LinearAppliedVolts", inputs.linearAppliedVolts);
51+
Logger.recordOutput(SUBSYSTEM_NAME + "/LinearCurrentAmps", inputs.linearCurrentAmps);
52+
}
53+
54+
public TalonFX getSpinMotor() {
55+
return io.getSpinMotor();
56+
}
57+
58+
public double getStateTime() {
59+
return super.getStateTime();
60+
}
61+
}

‎src/main/java/frc/robot/subsystems/Intake/IntakeConstants.java‎

Lines changed: 40 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,40 @@
1+
package frc.robot.Subsystems.Intake;
2+
3+
import com.ctre.phoenix6.configs.Slot0Configs; //need to add via phoenix tuner
4+
5+
public class IntakeConstants {
6+
7+
public static final String SUBSYSTEM_NAME = "Intake";
8+
9+
public static final double GEARING = 1;
10+
public static final double LINEAR_GEARING = 10.0; // figure out on CAD
11+
public static final double LINEAR_METERS_PER_ROTATION = 0.005; // Check CAD/ motor specs
12+
13+
// States
14+
public static final double INTAKE_IN_POS = 0.0;
15+
public static final double INTAKE_OUT_POS = 0.2; // Meters
16+
17+
public static final double SPIN_SPEED_INTAKE = 60.0; // RPS
18+
19+
public static final double SPIN_kS = 0.1; //tune
20+
public static final double SPIN_kV = 0.12; // 12v I think?
21+
public static final double SPIN_kA = 0.0; //tune
22+
23+
public static final Slot0Configs LINEAR_SLOT_0_CONFIGS = new Slot0Configs().withKP(0.5).withKI(0).withKD(0);
24+
public static final Slot0Configs SPIN_SLOT_0_CONFIGS = new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKV(SPIN_kV);
25+
26+
public static class Real {
27+
28+
public static final int SPIN_MOTOR_ID = 20; //make real
29+
public static final int LINEAR_ACTUATOR_ID = 21;
30+
}
31+
32+
public record PIDConstants(double kP, double kI, double kD) {}
33+
34+
public static class Sim {
35+
36+
public static final int NUM_MOTORS = 1;
37+
public static final double MOTOR_MOI = 0.00001;
38+
public static final PIDConstants LINEAR_PID = new PIDConstants(10.0, 0.0, 0.0); // Tune in sim
39+
}
40+
}

‎src/main/java/frc/robot/subsystems/Intake/IntakeIO.java‎

Lines changed: 25 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,25 @@
1+
package frc.robot.Subsystems.Intake;
2+
3+
import com.ctre.phoenix6.hardware.TalonFX;
4+
5+
public interface IntakeIO {
6+
public static class IntakeIOInputs {
7+
8+
public double spinVelocityRPS;
9+
public double spinAppliedVolts;
10+
public double spinCurrentAmps;
11+
12+
public double linearPositionMeters;
13+
public double linearVelocityMetersPerSec;
14+
public double linearAppliedVolts;
15+
public double linearCurrentAmps;
16+
}
17+
18+
public void updateInputs(IntakeIOInputs inputs);
19+
20+
public void setSpinVelocity(double rps, double feedforward);
21+
22+
public void setLinearPosition(double meters);
23+
24+
public TalonFX getSpinMotor();
25+
}

‎src/main/java/frc/robot/subsystems/Intake/IntakeIOSim.java‎

Lines changed: 67 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,67 @@
1+
package frc.robot.Subsystems.Intake;
2+
3+
import static edu.wpi.first.units.Units.*;
4+
import static frc.robot.Subsystems.Intake.IntakeConstants.*;
5+
6+
import com.ctre.phoenix6.hardware.TalonFX;
7+
import com.ctre.phoenix6.sim.TalonFXSimState;
8+
import edu.wpi.first.math.controller.PIDController;
9+
import edu.wpi.first.math.system.plant.DCMotor;
10+
import edu.wpi.first.math.system.plant.LinearSystemId;
11+
import edu.wpi.first.wpilibj.simulation.DCMotorSim;
12+
import frc.robot.GlobalConstants;
13+
import frc.robot.Subsystems.Intake.IntakeConstants.Sim;
14+
15+
public class IntakeIOSim implements IntakeIO {
16+
17+
private DCMotorSim spinSim;
18+
private TalonFXSimState spinSimState;
19+
private TalonFX spinMotor;
20+
21+
private DCMotorSim linearSim;
22+
private TalonFXSimState linearSimState;
23+
private TalonFX linearMotor;
24+
private final PIDController linearController;
25+
26+
public IntakeIOSim() {
27+
spinSim = new DCMotorSim(LinearSystemId.createDCMotorSystem(DCMotor.getFalcon500(1), Sim.MOTOR_MOI, 1), DCMotor.getFalcon500(1));
28+
spinMotor = new TalonFX(Real.SPIN_MOTOR_ID);
29+
spinSimState = new TalonFXSimState(spinMotor);
30+
31+
linearSim = new DCMotorSim(LinearSystemId.createDCMotorSystem(DCMotor.getFalcon500(1), Sim.MOTOR_MOI, LINEAR_GEARING), DCMotor.getFalcon500(1));
32+
linearMotor = new TalonFX(Real.LINEAR_ACTUATOR_ID);
33+
linearSimState = new TalonFXSimState(linearMotor);
34+
linearController = new PIDController(Sim.LINEAR_PID.kP(), Sim.LINEAR_PID.kI(), Sim.LINEAR_PID.kD());
35+
}
36+
37+
@Override
38+
public void updateInputs(IntakeIOInputs inputs) {
39+
spinSim.update(GlobalConstants.SIMULATION_PERIOD);
40+
spinSimState.setRotorVelocity(spinSim.getAngularVelocityRPM() / 60);
41+
spinSimState.setRawRotorPosition(spinSim.getAngularPositionRotations());
42+
spinSimState.setSupplyVoltage(12.0);
43+
44+
linearSim.update(GlobalConstants.SIMULATION_PERIOD);
45+
linearSimState.setRotorVelocity((linearSim.getAngularVelocityRPM() / 60) * LINEAR_GEARING);
46+
linearSimState.setRawRotorPosition(linearSim.getAngularPositionRotations() * LINEAR_GEARING);
47+
linearSimState.setSupplyVoltage(12.0);
48+
49+
inputs.spinVelocityRPS = spinMotor.getVelocity().getValue().in(RotationsPerSecond);
50+
inputs.linearPositionMeters = linearSim.getAngularPositionRotations() * LINEAR_METERS_PER_ROTATION;
51+
}
52+
53+
@Override
54+
public void setSpinVelocity(double rps, double feedforward) {
55+
spinSim.setInputVoltage(feedforward);
56+
}
57+
58+
@Override
59+
public void setLinearPosition(double meters) {
60+
linearSim.setInputVoltage(linearController.calculate(linearSim.getAngularPositionRotations(), meters / LINEAR_METERS_PER_ROTATION));
61+
} //pid only for sim, irl uses talon pid
62+
63+
@Override
64+
public TalonFX getSpinMotor() {
65+
return spinMotor;
66+
}
67+
}

‎src/main/java/frc/robot/subsystems/Intake/IntakeIOTalonFX.java‎

Lines changed: 66 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,66 @@
1+
package frc.robot.Subsystems.Intake;
2+
3+
import static edu.wpi.first.units.Units.*;
4+
import static frc.robot.GlobalConstants.*;
5+
import static frc.robot.Subsystems.Intake.IntakeConstants.*;
6+
7+
import com.ctre.phoenix6.configs.TalonFXConfiguration;
8+
import com.ctre.phoenix6.controls.PositionVoltage;
9+
import com.ctre.phoenix6.controls.VelocityVoltage;
10+
import com.ctre.phoenix6.hardware.TalonFX;
11+
import com.ctre.phoenix6.signals.NeutralModeValue;
12+
import frc.robot.Subsystems.Intake.IntakeConstants.Real;
13+
14+
public class IntakeIOTalonFX implements IntakeIO {
15+
16+
private final TalonFX spinMotor;
17+
private final TalonFX linearMotor; // idk what to call it but the linear actuator?
18+
19+
private final PositionVoltage linearRequest = new PositionVoltage(0); //talon pid cool
20+
private final VelocityVoltage spinRequest = new VelocityVoltage(0);
21+
22+
public IntakeIOTalonFX() {
23+
spinMotor = new TalonFX(Real.SPIN_MOTOR_ID);
24+
spinMotor.setNeutralMode(NeutralModeValue.Coast);
25+
var spinConfig = new TalonFXConfiguration();
26+
spinConfig.CurrentLimits.StatorCurrentLimit = 60; // Change after testing
27+
spinConfig.Slot0 = SPIN_SLOT_0_CONFIGS;
28+
spinMotor.getConfigurator().apply(spinConfig);
29+
30+
linearMotor = new TalonFX(Real.LINEAR_ACTUATOR_ID);
31+
linearMotor.setNeutralMode(NeutralModeValue.Brake);
32+
var linearConfig = new TalonFXConfiguration();
33+
linearConfig.Slot0 = LINEAR_SLOT_0_CONFIGS;
34+
//talon pid is at 1kHz, roborio at 50Hz so I think this is better for something linear?
35+
linearConfig.CurrentLimits.StatorCurrentLimit = 40; //change after testing
36+
linearConfig.Feedback.SensorToMechanismRatio = LINEAR_GEARING; //1 spin of motor = some geared extension
37+
linearMotor.getConfigurator().apply(linearConfig);
38+
}
39+
40+
@Override
41+
public void updateInputs(IntakeIOInputs inputs) {
42+
inputs.spinVelocityRPS = spinMotor.getVelocity().getValue().in(RotationsPerSecond);
43+
inputs.spinAppliedVolts = spinMotor.getMotorVoltage().getValueAsDouble();
44+
inputs.spinCurrentAmps = spinMotor.getStatorCurrent().getValueAsDouble();
45+
46+
inputs.linearPositionMeters = linearMotor.getPosition().getValueAsDouble() * LINEAR_METERS_PER_ROTATION;
47+
inputs.linearVelocityMetersPerSec = linearMotor.getVelocity().getValueAsDouble() * LINEAR_METERS_PER_ROTATION;
48+
inputs.linearAppliedVolts = linearMotor.getMotorVoltage().getValueAsDouble();
49+
inputs.linearCurrentAmps = linearMotor.getStatorCurrent().getValueAsDouble();
50+
}
51+
52+
@Override
53+
public void setSpinVelocity(double rps, double feedforward) {
54+
spinMotor.setControl(spinRequest.withVelocity(rps).withFeedForward(feedforward));
55+
}
56+
57+
@Override
58+
public void setLinearPosition(double meters) {
59+
linearMotor.setControl(linearRequest.withPosition(meters / LINEAR_METERS_PER_ROTATION));
60+
}
61+
62+
@Override
63+
public TalonFX getSpinMotor() {
64+
return spinMotor;
65+
}
66+
}

‎src/main/java/frc/robot/subsystems/Intake/IntakeStates.java‎

Lines changed: 29 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
package frc.robot.Subsystems.Intake;
2+
3+
import static edu.wpi.first.units.Units.*;
4+
import static frc.robot.Subsystems.Intake.IntakeConstants.*;
5+
6+
import edu.wpi.first.units.measure.AngularVelocity;
7+
import edu.wpi.first.units.measure.Distance;
8+
import org.team7525.subsystem.SubsystemStates;
9+
10+
public enum IntakeStates implements SubsystemStates {
11+
IN("In", Meters.of(INTAKE_IN_POS), RotationsPerSecond.of(0.0)),
12+
OUT("Out", Meters.of(INTAKE_OUT_POS), RotationsPerSecond.of(0.0)),
13+
INTAKE("Intake", Meters.of(INTAKE_OUT_POS), RotationsPerSecond.of(SPIN_SPEED_INTAKE));
14+
15+
private String stateString;
16+
public final Distance linearPos; //force units
17+
public final AngularVelocity spinSpeed;
18+
19+
IntakeStates(String stateString, Distance linearPos, AngularVelocity spinSpeed) {
20+
this.stateString = stateString;
21+
this.linearPos = linearPos;
22+
this.spinSpeed = spinSpeed;
23+
}
24+
25+
@Override
26+
public String getStateString() {
27+
return stateString;
28+
}
29+
}

0 commit comments

Comments
 (0)