Skip to content

Commit 81b31a1

Browse files
BrodyKarrBrodyKarr
authored
Merge branch 'main' into Mechanism2d
2 parents ac13871 + 49290ab commit 81b31a1

File tree

3 files changed

+99
-7
lines changed

3 files changed

+99
-7
lines changed

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

Lines changed: 53 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -9,12 +9,14 @@
99
import java.util.Optional;
1010

1111
import com.ctre.phoenix6.configs.CANcoderConfiguration;
12+
import com.ctre.phoenix6.configs.CANrangeConfiguration;
1213
import com.ctre.phoenix6.configs.TalonFXConfiguration;
1314
import com.ctre.phoenix6.signals.GravityTypeValue;
1415
import com.ctre.phoenix6.signals.InvertedValue;
1516
import com.ctre.phoenix6.signals.NeutralModeValue;
1617
import com.ctre.phoenix6.signals.SensorDirectionValue;
1718
import com.ctre.phoenix6.signals.StaticFeedforwardSignValue;
19+
import com.ctre.phoenix6.signals.UpdateModeValue;
1820
import com.frcteam3255.components.swerve.SN_SwerveConstants;
1921
import com.pathplanner.lib.config.ModuleConfig;
2022
import com.pathplanner.lib.config.PIDConstants;
@@ -221,6 +223,57 @@ public static class constIntake {
221223
// TODO: Replace with actual measurements
222224
public static final Current ALGAE_INTAKE_HAS_GP_CURRENT = Units.Amps.of(15);
223225
public static final AngularVelocity ALGAE_INTAKE_HAS_GP_VELOCITY = Units.RotationsPerSecond.of(2102 / 60);
226+
public static final TalonFXConfiguration INTAKE_PIVOT_CONFIG = new TalonFXConfiguration();
227+
public static final TalonFXConfiguration ALGAE_INTAKE_CONFIG = new TalonFXConfiguration();
228+
public static final TalonFXConfiguration CORAL_INTAKE_CONFIG = new TalonFXConfiguration();
229+
public static final CANrangeConfiguration CORAL_INTAKE_SENSOR_CONFIG = new CANrangeConfiguration();
230+
231+
static {
232+
// intake pivot motor
233+
INTAKE_PIVOT_CONFIG.MotorOutput.NeutralMode = NeutralModeValue.Brake;
234+
INTAKE_PIVOT_CONFIG.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
235+
236+
INTAKE_PIVOT_CONFIG.SoftwareLimitSwitch.ForwardSoftLimitEnable = true;
237+
INTAKE_PIVOT_CONFIG.SoftwareLimitSwitch.ForwardSoftLimitThreshold = Units.Rotations.of(57)
238+
.in(Units.Degrees);
239+
INTAKE_PIVOT_CONFIG.SoftwareLimitSwitch.ReverseSoftLimitEnable = true;
240+
INTAKE_PIVOT_CONFIG.SoftwareLimitSwitch.ReverseSoftLimitThreshold = Units.Rotations.of(-37)
241+
.in(Units.Degrees);
242+
243+
INTAKE_PIVOT_CONFIG.Feedback.SensorToMechanismRatio = 1000 / 27;// just like intake pivot, we still need
244+
// to get the ratio from fab
245+
INTAKE_PIVOT_CONFIG.Slot0.GravityType = GravityTypeValue.Arm_Cosine;
246+
INTAKE_PIVOT_CONFIG.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseClosedLoopSign;
247+
248+
INTAKE_PIVOT_CONFIG.MotionMagic.MotionMagicCruiseVelocity = 9999;
249+
INTAKE_PIVOT_CONFIG.MotionMagic.MotionMagicAcceleration = 9999;
250+
251+
INTAKE_PIVOT_CONFIG.CurrentLimits.SupplyCurrentLimitEnable = true;
252+
INTAKE_PIVOT_CONFIG.CurrentLimits.SupplyCurrentLowerLimit = 30;
253+
INTAKE_PIVOT_CONFIG.CurrentLimits.SupplyCurrentLimit = 45;
254+
INTAKE_PIVOT_CONFIG.CurrentLimits.SupplyCurrentLowerTime = 0.5;
255+
256+
// algae intake motor config
257+
ALGAE_INTAKE_CONFIG.MotorOutput.NeutralMode = NeutralModeValue.Brake;
258+
ALGAE_INTAKE_CONFIG.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
259+
260+
ALGAE_INTAKE_CONFIG.CurrentLimits.SupplyCurrentLimitEnable = true;
261+
ALGAE_INTAKE_CONFIG.CurrentLimits.SupplyCurrentLowerLimit = 30;
262+
ALGAE_INTAKE_CONFIG.CurrentLimits.SupplyCurrentLimit = 60;
263+
ALGAE_INTAKE_CONFIG.CurrentLimits.SupplyCurrentLowerTime = 0.5;
264+
// coral intake motor config
265+
CORAL_INTAKE_CONFIG.MotorOutput.NeutralMode = NeutralModeValue.Brake;
266+
CORAL_INTAKE_CONFIG.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
267+
268+
CORAL_INTAKE_CONFIG.CurrentLimits.SupplyCurrentLimitEnable = true;
269+
CORAL_INTAKE_CONFIG.CurrentLimits.SupplyCurrentLowerLimit = 30;
270+
CORAL_INTAKE_CONFIG.CurrentLimits.SupplyCurrentLimit = 60;
271+
CORAL_INTAKE_CONFIG.CurrentLimits.SupplyCurrentLowerTime = 0.5;
272+
// coral intake sensor config
273+
CORAL_INTAKE_SENSOR_CONFIG.ToFParams.UpdateMode = UpdateModeValue.ShortRange100Hz;
274+
CORAL_INTAKE_SENSOR_CONFIG.ProximityParams.ProximityThreshold = Units.Inches.of(3.95).in(Units.Meters);
275+
}
276+
224277
}
225278

226279
public static class constField {

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

Lines changed: 5 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -37,10 +37,12 @@ public static class mapDrivetrain {
3737
public static class mapIntake {
3838
// coral intake motors and pivot
3939
public static final int INTAKE_PIVOT_CAN = 11;
40-
public static final int CORAL_LEFT_CAN = 12;
41-
public static final int CORAL_RIGHT_CAN = 13;
40+
public static final int CORAL_INTAKE_CAN = 12;
4241
// Algae intake motor
43-
public static final int INTAKE_ALGAE_CAN = 14;
42+
public static final int INTAKE_ALGAE_CAN = 13;
43+
44+
// Coral intake sensors
45+
public static final int CORAL_INTAKE_SENSOR = 14;
4446
}
4547

4648
// Elevator ID 21-30

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

Lines changed: 41 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -4,8 +4,13 @@
44

55
package frc.robot.subsystems;
66

7+
import com.ctre.phoenix6.controls.MotionMagicExpoVoltage;
8+
import com.ctre.phoenix6.hardware.CANrange;
79
import com.ctre.phoenix6.hardware.TalonFX;
10+
import com.ctre.phoenix6.signals.NeutralModeValue;
811

12+
import static edu.wpi.first.units.Units.Degrees;
13+
import edu.wpi.first.units.measure.Angle;
914
import edu.wpi.first.units.measure.AngularVelocity;
1015
import edu.wpi.first.units.measure.Current;
1116
import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -14,21 +19,27 @@
1419

1520
public class Intake extends SubsystemBase {
1621
TalonFX intakePivotMotor;
17-
TalonFX coralLeftMotor;
18-
TalonFX coralRightMotor;
22+
TalonFX coralIntakeMotor;
1923
TalonFX algaeIntakeMotor;
24+
CANrange coralSensor;
2025
public boolean hasCoral = false;
2126
public boolean hasAlgae = false;
27+
private Angle lastDesiredAngle = Degrees.zero();
28+
MotionMagicExpoVoltage positionRequest = new MotionMagicExpoVoltage(0);
2229

2330
/** Creates a new Intake. */
2431
public Intake() {
2532
intakePivotMotor = new TalonFX(mapIntake.INTAKE_PIVOT_CAN); // Intake pivot motor
26-
coralLeftMotor = new TalonFX(mapIntake.CORAL_LEFT_CAN); // Coral left intake motor
27-
coralRightMotor = new TalonFX(mapIntake.CORAL_RIGHT_CAN); // Coral right intake motor
33+
coralIntakeMotor = new TalonFX(mapIntake.CORAL_INTAKE_CAN); // Coral left intake motor
2834
algaeIntakeMotor = new TalonFX(mapIntake.INTAKE_ALGAE_CAN); // Algae intake motor
35+
coralSensor = new CANrange(mapIntake.CORAL_INTAKE_SENSOR); // Coral intake sensor
2936

3037
// Set default motor configurations if needed
3138
// e.g., intakePivotMotor.configFactoryDefault();
39+
intakePivotMotor.getConfigurator().apply(constIntake.INTAKE_PIVOT_CONFIG);
40+
coralIntakeMotor.getConfigurator().apply(constIntake.CORAL_INTAKE_CONFIG);
41+
algaeIntakeMotor.getConfigurator().apply(constIntake.ALGAE_INTAKE_CONFIG);
42+
coralSensor.getConfigurator().apply(constIntake.CORAL_INTAKE_SENSOR_CONFIG);
3243
}
3344

3445
public boolean hasCoral() {
@@ -53,6 +64,32 @@ public boolean hasAlgae() {
5364
}
5465
}
5566

67+
public void getIntakePivtoMotorAngle() {
68+
double angle = intakePivotMotor.getPosition().getValueAsDouble();
69+
70+
}
71+
72+
public Angle getLastDesiredPivotAngle() {
73+
return lastDesiredAngle;
74+
}
75+
76+
public void setPivotAngle(Angle angle) {
77+
intakePivotMotor.setControl(positionRequest.withPosition(angle.in(Degrees)));
78+
}
79+
80+
public void setCoralIntakeMotor(double speed) {
81+
coralIntakeMotor.setVoltage(speed);
82+
}
83+
84+
public void setAlgaeIntakeMotor(double speed) {
85+
algaeIntakeMotor.setVoltage(speed);
86+
}
87+
88+
public void setIntakeMotorNeutralMode(NeutralModeValue mode) {
89+
intakePivotMotor.setNeutralMode(mode);
90+
algaeIntakeMotor.setNeutralMode(mode);
91+
}
92+
5693
@Override
5794
public void periodic() {
5895
// This method will be called once per scheduler run

0 commit comments

Comments
 (0)