Skip to content

Commit d66622d

Browse files
junocapraIshan1522github-actions[bot]
authored
Coral intake + algae pivot (#27)
* juno's intake * IntakeAdvantageKit * Intake Advantagekit Done * Edited intake a bit * Intake edits * Formatting fixes * Worked on coral pivot * fjioe * continued algae pivot * htuiew * gfi * jkjlkjlk --------- Co-authored-by: Ishan1522 <[email protected]> Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com>
1 parent 5d23f26 commit d66622d

16 files changed

+545
-70
lines changed

‎build.gradle

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,4 @@
1+
12
plugins {
23
id "java"
34
id "edu.wpi.first.GradleRIO" version "2025.2.1"

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

Lines changed: 39 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -11,14 +11,21 @@
1111
import edu.wpi.first.wpilibj2.command.button.Trigger;
1212
import frc.robot.Constants.FieldConstants;
1313
import frc.robot.Constants.SimulationConstants;
14+
import frc.robot.commands.algaePivot.ManualAlgaePivot;
1415
import frc.robot.commands.autodrive.AutoAlign;
1516
import frc.robot.commands.drive.DriveCommand;
17+
import frc.robot.commands.intake.Eject;
18+
import frc.robot.commands.intake.Intake;
1619
import frc.robot.extras.simulation.field.SimulatedField;
1720
import frc.robot.extras.simulation.mechanismSim.swerve.GyroSimulation;
1821
import frc.robot.extras.simulation.mechanismSim.swerve.SwerveDriveSimulation;
1922
import frc.robot.extras.simulation.mechanismSim.swerve.SwerveModuleSimulation;
2023
import frc.robot.extras.simulation.mechanismSim.swerve.SwerveModuleSimulation.WHEEL_GRIP;
2124
import frc.robot.extras.util.JoystickUtil;
25+
import frc.robot.subsystems.algaePivot.AlgaePivotSubsystem;
26+
import frc.robot.subsystems.algaePivot.PhysicalAlgaePivot;
27+
import frc.robot.subsystems.intake.IntakeSubsystem;
28+
import frc.robot.subsystems.intake.PhysicalIntake;
2229
import frc.robot.subsystems.swerve.SwerveConstants;
2330
import frc.robot.subsystems.swerve.SwerveConstants.DriveConstants;
2431
import frc.robot.subsystems.swerve.SwerveConstants.ModuleConstants;
@@ -42,6 +49,9 @@ public class RobotContainer {
4249

4350
private final CommandXboxController operatorController = new CommandXboxController(1);
4451
private final CommandXboxController driverController = new CommandXboxController(0);
52+
private final IntakeSubsystem intakeSubsystem = new IntakeSubsystem(new PhysicalIntake());
53+
private final AlgaePivotSubsystem algaePivotSubsystem =
54+
new AlgaePivotSubsystem(new PhysicalAlgaePivot());
4555

4656
// Simulation, we store them here in the robot container
4757
// private final SimulatedField simulatedArena;
@@ -171,6 +181,35 @@ private void configureButtonBindings() {
171181
Trigger driverRightDirectionPad = new Trigger(driverController.pov(90));
172182
Trigger driverLeftDirectionPad = new Trigger(driverController.pov(270));
173183

184+
driverController.a().whileTrue(new Intake(intakeSubsystem));
185+
driverController.b().whileTrue(new Eject(intakeSubsystem));
186+
driverController
187+
.x()
188+
.whileTrue(new ManualAlgaePivot(algaePivotSubsystem, operatorController::getLeftY));
189+
190+
// // autodrive
191+
// Trigger driverAButton = new Trigger(driverController::getAButton);
192+
// lol whatever
193+
// // intake
194+
// Trigger operatorLeftTrigger = new Trigger(()->operatorController.getLeftTriggerAxis() > 0.2);
195+
// Trigger operatorLeftBumper = new Trigger(operatorController::getLeftBumper);
196+
// // amp and speaker
197+
// Trigger operatorBButton = new Trigger(operatorController::getBButton);
198+
// Trigger operatorRightBumper = new Trigger(operatorController::getRightBumper);
199+
// Trigger operatorRightTrigger = new Trigger(()->operatorController.getRightTriggerAxis() >
200+
// 0.2);
201+
// Trigger driverRightTrigger = new Trigger(()->driverController.getRightTriggerAxis() > 0.2);
202+
203+
// // manual pivot and intake rollers
204+
// Trigger operatorAButton = new Trigger(operatorController::getAButton);
205+
// Trigger operatorXButton = new Trigger(operatorController::getXButton);
206+
// Trigger operatorYButton = new Trigger(operatorController::getYButton);
207+
// DoubleSupplier operatorRightStickY = operatorController::getRightY;
208+
// // unused
209+
// Trigger operatorUpDirectionPad = new Trigger(()->operatorController.getPOV() == 0);
210+
// Trigger operatorLeftDirectionPad = new Trigger(()->operatorController.getPOV() == 270);
211+
// Trigger operatorDownDirectionPad = new Trigger(()->operatorController.getPOV() == 180);
212+
// Trigger driverLeftTrigger = new Trigger(()->driverController.getLeftTriggerAxis() > 0.2);
174213
Trigger driverLeftBumper = new Trigger(driverController.leftBumper());
175214

176215
// DRIVER BUTTONS

‎src/main/java/frc/robot/commands/algaePivot/ManualAlgaePivot.java

Lines changed: 35 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,35 @@
1+
package frc.robot.commands.algaePivot;
2+
3+
import edu.wpi.first.wpilibj2.command.Command;
4+
import frc.robot.subsystems.algaePivot.AlgaeConstants;
5+
import frc.robot.subsystems.algaePivot.AlgaePivotSubsystem;
6+
import java.util.function.DoubleSupplier;
7+
8+
public class ManualAlgaePivot extends Command {
9+
private final AlgaePivotSubsystem algaePivotSubsystem;
10+
private final DoubleSupplier speed;
11+
12+
public ManualAlgaePivot(AlgaePivotSubsystem algaePivotSubsystem, DoubleSupplier speed) {
13+
this.algaePivotSubsystem = algaePivotSubsystem;
14+
this.speed = speed;
15+
addRequirements(algaePivotSubsystem);
16+
}
17+
18+
@Override
19+
public void initialize() {}
20+
21+
@Override
22+
public void execute() {
23+
algaePivotSubsystem.setAlgaeSpeed(speed.getAsDouble());
24+
}
25+
26+
@Override
27+
public void end(boolean interrupted) {
28+
algaePivotSubsystem.setAlgaeSpeed(AlgaeConstants.ALGAE_NEUTRAL_SPEED);
29+
}
30+
31+
@Override
32+
public boolean isFinished() {
33+
return false;
34+
}
35+
}

‎src/main/java/frc/robot/commands/intake/Eject.java

Lines changed: 29 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
package frc.robot.commands.intake;
2+
3+
import edu.wpi.first.wpilibj2.command.Command;
4+
import frc.robot.subsystems.intake.IntakeConstants;
5+
import frc.robot.subsystems.intake.IntakeSubsystem;
6+
7+
public class Eject extends Command {
8+
private final IntakeSubsystem intakeSubsystem;
9+
10+
public Eject(IntakeSubsystem intakeSubsystem) {
11+
this.intakeSubsystem = intakeSubsystem;
12+
addRequirements(this.intakeSubsystem);
13+
}
14+
15+
@Override
16+
public void execute() {
17+
intakeSubsystem.setIntakeSpeed(IntakeConstants.EJECT_SPEED);
18+
}
19+
20+
@Override
21+
public void end(boolean interrupted) {
22+
intakeSubsystem.setIntakeSpeed(0);
23+
}
24+
25+
@Override
26+
public boolean isFinished() {
27+
return false;
28+
}
29+
}

‎src/main/java/frc/robot/commands/intake/Intake.java

Lines changed: 29 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
package frc.robot.commands.intake;
2+
3+
import edu.wpi.first.wpilibj2.command.Command;
4+
import frc.robot.subsystems.intake.IntakeConstants;
5+
import frc.robot.subsystems.intake.IntakeSubsystem;
6+
7+
public class Intake extends Command {
8+
private final IntakeSubsystem intakeSubsystem;
9+
10+
public Intake(IntakeSubsystem intakeSubsystem) {
11+
this.intakeSubsystem = intakeSubsystem;
12+
addRequirements(this.intakeSubsystem);
13+
}
14+
15+
@Override
16+
public void execute() {
17+
intakeSubsystem.setIntakeSpeed(IntakeConstants.INTAKE_SPEED);
18+
}
19+
20+
@Override
21+
public void end(boolean interrupted) {
22+
intakeSubsystem.setIntakeSpeed(0);
23+
}
24+
25+
@Override
26+
public boolean isFinished() {
27+
return false;
28+
}
29+
}

‎src/main/java/frc/robot/subsystems/AlgaePivot/AlgaeConstants.java

Lines changed: 31 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,31 @@
1+
package frc.robot.subsystems.algaePivot;
2+
3+
import com.ctre.phoenix6.signals.SensorDirectionValue;
4+
5+
public class AlgaeConstants {
6+
public static final int ALGAE_PIVOT_MOTOR_ID = 0;
7+
public static final int ALGAE_ENCODER_MOTOR_ID = 0;
8+
9+
public static final SensorDirectionValue ALGAE_ENCODER_REVERSED =
10+
SensorDirectionValue.Clockwise_Positive;
11+
public static final double MAX_VELOCITY_ROTATIONS_PER_SECOND = 0.0;
12+
public static final double MAX_ACCELERATION_ROTATIONS_PER_SECOND = 0.0;
13+
14+
public static final double MAX_ANGLE = 0.0;
15+
public static final double MIN_ANGLE = 0.0;
16+
17+
public static final double ANGLE_ZERO = 0.0;
18+
public static final double PIVOT_P = 0.0;
19+
public static final double PIVOT_I = 0.0;
20+
public static final double PIVOT_D = 0.0;
21+
public static final double PIVOT_G = 0.0;
22+
23+
public static final double ALGAE_GEAR_RATIO = 0.0;
24+
public static final double ALGAE_PIVOT_MASS = 0.0;
25+
public static final double ALGAE_PIVOT_LENGTH = 0.0;
26+
27+
public static final double ALGAE_PIVOT_ANGLE = 0.0;
28+
public static final double ALGAE_PIVOT_SPEED = 0.0;
29+
public static final double ALGAE_VOLTAGE = 0.0;
30+
public static final double ALGAE_NEUTRAL_SPEED = 0.0;
31+
}

‎src/main/java/frc/robot/subsystems/AlgaePivot/AlgaePivotInterface.java

Lines changed: 36 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,36 @@
1+
package frc.robot.subsystems.algaePivot;
2+
3+
import org.littletonrobotics.junction.AutoLog;
4+
5+
public interface AlgaePivotInterface {
6+
@AutoLog
7+
public static class AlgaePivotInputs {
8+
public boolean isConnected = true;
9+
public double algaeAngle = 0.0;
10+
public double algaeVoltage = 0.0;
11+
public double algaeVelocity = 0.0;
12+
public double algaeTemp = 0.0;
13+
public double algaeSupplyCurrentAmps = 0.0;
14+
public double algaeTorqueCurrentAmps = 0.0;
15+
}
16+
17+
default void updateInputs(AlgaePivotInputs inputs) {}
18+
19+
default void setAlgaeSpeed(double speed) {}
20+
21+
default void setAlgaeAngle(double angle) {}
22+
23+
default void setAlgaeVoltage(double voltage) {}
24+
25+
default boolean isPivotWithinAcceptapleError() {
26+
return true;
27+
}
28+
29+
default double getAlgaeAngle() {
30+
return 0.0;
31+
}
32+
33+
default double getAlgaePivotTarget() {
34+
return 0.0;
35+
}
36+
}

‎src/main/java/frc/robot/subsystems/AlgaePivot/AlgaePivotSubsystem.java

Lines changed: 30 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,30 @@
1+
package frc.robot.subsystems.algaePivot;
2+
3+
import edu.wpi.first.wpilibj2.command.SubsystemBase;
4+
import org.littletonrobotics.junction.Logger;
5+
6+
public class AlgaePivotSubsystem extends SubsystemBase {
7+
private final AlgaePivotInterface algaePivotInterface;
8+
private final AlgaePivotInputsAutoLogged inputs = new AlgaePivotInputsAutoLogged();
9+
10+
public AlgaePivotSubsystem(AlgaePivotInterface algaePivotInterface) {
11+
this.algaePivotInterface = algaePivotInterface;
12+
}
13+
14+
public void setAlgaeSpeed(double speed) {
15+
algaePivotInterface.setAlgaeSpeed(speed);
16+
}
17+
18+
public void setAlgaeAngle(double angle) {
19+
algaePivotInterface.setAlgaeAngle(angle);
20+
}
21+
22+
public void setAlgaeVoltage(double voltage) {
23+
algaePivotInterface.setAlgaeVoltage(voltage);
24+
}
25+
26+
public void periodic() {
27+
algaePivotInterface.updateInputs(inputs);
28+
Logger.processInputs("AlgaePivotSubsystem/", inputs);
29+
}
30+
}

‎src/main/java/frc/robot/subsystems/AlgaePivot/PhysicalAlgaePivot.java

Lines changed: 115 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,115 @@
1+
package frc.robot.subsystems.algaePivot;
2+
3+
import com.ctre.phoenix6.BaseStatusSignal;
4+
import com.ctre.phoenix6.StatusSignal;
5+
import com.ctre.phoenix6.configs.CANcoderConfiguration;
6+
import com.ctre.phoenix6.configs.TalonFXConfiguration;
7+
import com.ctre.phoenix6.controls.MotionMagicVoltage;
8+
import com.ctre.phoenix6.controls.VoltageOut;
9+
import com.ctre.phoenix6.hardware.CANcoder;
10+
import com.ctre.phoenix6.hardware.TalonFX;
11+
import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
12+
import com.ctre.phoenix6.signals.GravityTypeValue;
13+
import com.ctre.phoenix6.signals.InvertedValue;
14+
import com.ctre.phoenix6.signals.NeutralModeValue;
15+
import edu.wpi.first.units.measure.Angle;
16+
import edu.wpi.first.units.measure.AngularVelocity;
17+
import edu.wpi.first.units.measure.Current;
18+
import edu.wpi.first.units.measure.Voltage;
19+
import frc.robot.Constants.HardwareConstants;
20+
21+
public class PhysicalAlgaePivot implements AlgaePivotInterface {
22+
private final TalonFX algaeMotor;
23+
private final CANcoder algaeEncoder;
24+
private final TalonFXConfiguration algaeMotorConfig;
25+
private final CANcoderConfiguration algaeEncoderConfig;
26+
private final StatusSignal<Voltage> algaeVoltage;
27+
private final StatusSignal<AngularVelocity> algaeVelocity;
28+
private StatusSignal<Angle> algaeAngle;
29+
private final StatusSignal<Current> algaeSupplyCurrent;
30+
private final StatusSignal<Current> algaeStatorCurrent;
31+
private final MotionMagicVoltage mmPositionRequest;
32+
private double algaeTargetAngle;
33+
private final VoltageOut voltageOut;
34+
35+
public PhysicalAlgaePivot() {
36+
algaeMotor = new TalonFX(AlgaeConstants.ALGAE_PIVOT_MOTOR_ID);
37+
algaeEncoder = new CANcoder(AlgaeConstants.ALGAE_ENCODER_MOTOR_ID);
38+
algaeMotorConfig = new TalonFXConfiguration();
39+
algaeEncoderConfig = new CANcoderConfiguration();
40+
mmPositionRequest = new MotionMagicVoltage(0);
41+
42+
algaeVoltage = algaeMotor.getMotorVoltage();
43+
algaeVelocity = algaeMotor.getVelocity();
44+
algaeAngle = algaeEncoder.getAbsolutePosition();
45+
algaeSupplyCurrent = algaeMotor.getSupplyCurrent();
46+
algaeStatorCurrent = algaeMotor.getStatorCurrent();
47+
voltageOut = new VoltageOut(0);
48+
49+
algaeEncoderConfig.MagnetSensor.MagnetOffset = -AlgaeConstants.ANGLE_ZERO;
50+
algaeEncoderConfig.MagnetSensor.SensorDirection = AlgaeConstants.ALGAE_ENCODER_REVERSED;
51+
algaeEncoder.getConfigurator().apply(algaeEncoderConfig, HardwareConstants.TIMEOUT_S);
52+
53+
algaeMotorConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
54+
algaeMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
55+
algaeMotorConfig.MotorOutput.DutyCycleNeutralDeadband = HardwareConstants.MIN_FALCON_DEADBAND;
56+
57+
algaeMotorConfig.Slot0.kP = AlgaeConstants.PIVOT_P;
58+
algaeMotorConfig.Slot0.kI = AlgaeConstants.PIVOT_I;
59+
algaeMotorConfig.Slot0.kD = AlgaeConstants.PIVOT_D;
60+
algaeMotorConfig.Slot0.kG = AlgaeConstants.PIVOT_G;
61+
algaeMotorConfig.Slot0.GravityType = GravityTypeValue.Arm_Cosine;
62+
63+
algaeMotorConfig.MotionMagic.MotionMagicAcceleration =
64+
AlgaeConstants.MAX_VELOCITY_ROTATIONS_PER_SECOND;
65+
algaeMotorConfig.MotionMagic.MotionMagicCruiseVelocity =
66+
AlgaeConstants.MAX_ACCELERATION_ROTATIONS_PER_SECOND;
67+
68+
algaeMotorConfig.ClosedLoopGeneral.ContinuousWrap = true;
69+
70+
algaeMotorConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder;
71+
algaeMotorConfig.Feedback.FeedbackRemoteSensorID = algaeEncoder.getDeviceID();
72+
73+
algaeMotorConfig.SoftwareLimitSwitch.ForwardSoftLimitThreshold = AlgaeConstants.MAX_ANGLE;
74+
algaeMotorConfig.SoftwareLimitSwitch.ReverseSoftLimitThreshold = AlgaeConstants.MIN_ANGLE;
75+
algaeMotorConfig.SoftwareLimitSwitch.ForwardSoftLimitEnable = true;
76+
algaeMotorConfig.SoftwareLimitSwitch.ReverseSoftLimitEnable = true;
77+
78+
BaseStatusSignal.setUpdateFrequencyForAll(
79+
100.0, algaeAngle, algaeVelocity, algaeVoltage, algaeSupplyCurrent, algaeStatorCurrent);
80+
}
81+
82+
@Override
83+
public void updateInputs(AlgaePivotInputs inputs) {
84+
inputs.algaeAngle = algaeAngle.getValueAsDouble();
85+
inputs.algaeVelocity = algaeVelocity.getValueAsDouble();
86+
inputs.algaeVoltage = algaeVoltage.getValueAsDouble();
87+
}
88+
89+
@Override
90+
public void setAlgaeSpeed(double speed) {
91+
algaeMotor.set(speed);
92+
}
93+
94+
@Override
95+
public void setAlgaeAngle(double angle) {
96+
algaeTargetAngle = angle;
97+
algaeMotor.setControl(mmPositionRequest.withPosition(angle));
98+
}
99+
100+
@Override
101+
public void setAlgaeVoltage(double voltage) {
102+
algaeMotor.setControl(voltageOut.withOutput(voltage));
103+
}
104+
105+
@Override
106+
public double getAlgaeAngle() {
107+
algaeAngle.refresh();
108+
return algaeAngle.getValueAsDouble();
109+
}
110+
111+
@Override
112+
public double getAlgaePivotTarget() {
113+
return algaeTargetAngle;
114+
}
115+
}

0 commit comments

Comments
 (0)