Skip to content

Arm logic #16

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 44 commits into from
Sep 18, 2025
Merged

Arm logic #16

Show file tree
Hide file tree
Changes from 14 commits
Commits
Show all changes
44 commits
Select commit Hold shift + click to select a range
26ede8b
changed elevator so arm has access
ShmayaR Sep 14, 2025
b969772
added logic to stop arm from hitting anything
ShmayaR Sep 14, 2025
b93b24d
changed commands to work with logic
ShmayaR Sep 14, 2025
8bcbe19
Update ElevatorConstants.java
ShmayaR Sep 14, 2025
cbf1359
added load coral command which overrides arm logic
ShmayaR Sep 14, 2025
d3546dd
updated poses
ShmayaR Sep 14, 2025
4b60ce6
updated stuff
ShmayaR Sep 14, 2025
f327b94
fixed logic
ShmayaR Sep 15, 2025
bd98958
fixed logic issue and added d to arm pid
ShmayaR Sep 16, 2025
eb64ed0
fixed stuff
ShmayaR Sep 16, 2025
024fb62
made function more readable
ShmayaR Sep 18, 2025
d1c4d2d
Update Elevator.java
ShmayaR Sep 18, 2025
4dd87f5
rename
ShmayaR Sep 18, 2025
379867e
Update Arm.java
ShmayaR Sep 18, 2025
1b9c09f
removed magic number
ShmayaR Sep 18, 2025
4820ccc
moved into more variables
ShmayaR Sep 18, 2025
bbe7ac4
added javadoc
ShmayaR Sep 18, 2025
140137d
switched current limit type to stator
ShmayaR Sep 18, 2025
2b0037f
renamed
ShmayaR Sep 18, 2025
482bd8f
renamed stuff
ShmayaR Sep 18, 2025
1aeb7bb
extracted into multiple meathods
ShmayaR Sep 18, 2025
07b24f6
added command for checking if at an abgle
ShmayaR Sep 18, 2025
94c264b
Update Arm.java
ShmayaR Sep 18, 2025
936826d
Update Arm.java
ShmayaR Sep 18, 2025
9b9fc93
fixed arm logic
ShmayaR Sep 18, 2025
c2591b7
better pid
ShmayaR Sep 18, 2025
8159e68
Update Arm.java
ShmayaR Sep 18, 2025
dc3319b
added gravity offset to the getAngle method
ShmayaR Sep 18, 2025
d748b5e
Update src/main/java/frc/trigon/robot/subsystems/arm/Arm.java
ShmayaR Sep 18, 2025
248ca5c
siwtched set target state commands from execute to functional so that…
ShmayaR Sep 18, 2025
3a8dc54
Merge branch 'arm-logic' of https://github.com/Programming-TRIGON/Rob…
ShmayaR Sep 18, 2025
c0db5e1
made load coral function part of setTargetState
ShmayaR Sep 18, 2025
43757f9
Update Arm.java
ShmayaR Sep 18, 2025
62fcd32
got rid of unnecesary functions
ShmayaR Sep 18, 2025
440dc79
put logic in setTargetAngle into different function
ShmayaR Sep 18, 2025
fae2066
Update Arm.java
ShmayaR Sep 18, 2025
33beb6d
separated setTargetPosition logic into differant logic
ShmayaR Sep 18, 2025
cf68be9
Update ArmConstants.java
ShmayaR Sep 18, 2025
d1c7edc
Update ArmConstants.java
ShmayaR Sep 18, 2025
5bf8194
Update Arm.java
ShmayaR Sep 18, 2025
bd19f72
Update Arm.java
ShmayaR Sep 18, 2025
4ea93e3
cleaned commands
ShmayaR Sep 18, 2025
df0a3f4
Update Elevator.java
ShmayaR Sep 18, 2025
fb1233d
changed names
ShmayaR Sep 18, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
33 changes: 21 additions & 12 deletions src/main/java/frc/trigon/robot/subsystems/arm/Arm.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,9 @@
import edu.wpi.first.units.Units;
import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import frc.trigon.robot.RobotContainer;
import frc.trigon.robot.subsystems.MotorSubsystem;
import frc.trigon.robot.subsystems.elevator.ElevatorConstants;
import lib.hardware.phoenix6.cancoder.CANcoderEncoder;
import lib.hardware.phoenix6.cancoder.CANcoderSignal;
import lib.hardware.phoenix6.talonfx.TalonFXMotor;
Expand Down Expand Up @@ -81,6 +83,10 @@ public void sysIDDrive(double targetVoltage) {
armMasterMotor.setControl(voltageRequest.withOutput(targetVoltage));
}

public boolean isArmAboveSafeZone() {
return getAngle().getDegrees() >= 90;
}

public boolean atState(ArmConstants.ArmState targetState, boolean isStateReversed) {
if (isStateReversed)
return this.targetState == targetState && atTargetAngle(isStateReversed);
Expand All @@ -93,7 +99,7 @@ public boolean atState(ArmConstants.ArmState targetState) {

public boolean atTargetAngle(boolean isStateReversed) {
if (isStateReversed) {
final double currentToTargetStateDifferenceDegrees = Math.abs(ArmConstants.FULL_ROTATION.minus(targetState.targetAngle).minus(getAngle()).getDegrees());
final double currentToTargetStateDifferenceDegrees = Math.abs(subtractFrom360Degrees(targetState.targetAngle).minus(getAngle()).getDegrees());
return currentToTargetStateDifferenceDegrees < ArmConstants.ANGLE_TOLERANCE.getDegrees();
}
return atTargetAngle();
Expand All @@ -104,8 +110,8 @@ public boolean atTargetAngle() {
return currentToTargetStateDifferenceDegrees < ArmConstants.ANGLE_TOLERANCE.getDegrees();
}

public boolean hasGamePiece() {
return ArmConstants.COLLECTION_DETECTION_BOOLEAN_EVENT.getAsBoolean();
public Rotation2d getAngle() {
return Rotation2d.fromRotations(angleEncoder.getSignal(CANcoderSignal.POSITION));
}

void setTargetState(ArmConstants.ArmState targetState) {
Expand All @@ -120,7 +126,7 @@ void setTargetState(ArmConstants.ArmState targetState, boolean isStateReversed)

if (isStateReversed) {
setTargetState(
ArmConstants.FULL_ROTATION.minus(targetState.targetAngle)
subtractFrom360Degrees(targetState.targetAngle)
, targetState.targetEndEffectorVoltage
);
return;
Expand All @@ -142,30 +148,33 @@ void prepareForState(ArmConstants.ArmState targetState) {
void prepareForState(ArmConstants.ArmState targetState, boolean isStateReversed) {
this.isStateReversed = isStateReversed;
this.targetState = targetState;

if (isStateReversed) {
setTargetAngle(ArmConstants.FULL_ROTATION.minus(targetState.targetAngle));
setTargetAngle(subtractFrom360Degrees(targetState.targetAngle));
return;
}
setTargetAngle(targetState.targetAngle);
}

private Rotation2d getAngle() {
return Rotation2d.fromRotations(angleEncoder.getSignal(CANcoderSignal.POSITION));
}

private void setTargetAngle(Rotation2d targetAngle) {
armMasterMotor.setControl(positionRequest.withPosition(targetAngle.getRotations()));
final double distanceFromSafeZone = RobotContainer.ELEVATOR.getPositionMeters() - ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS;
final double cosMinimumAngle = distanceFromSafeZone / ArmConstants.ARM_LENGTH_METERS;
final Rotation2d minimumSafeAngle = Rotation2d.fromRadians(RobotContainer.ELEVATOR.isElevatorAboveSafeZone() ? 0 : Math.acos(cosMinimumAngle));
armMasterMotor.setControl(positionRequest.withPosition(Math.max(targetAngle.getRotations(), minimumSafeAngle.getRotations())));
}

private void setTargetVoltage(double targetVoltage) {
ArmConstants.END_EFFECTOR_MECHANISM.setTargetVelocity(targetVoltage);
endEffectorMotor.setControl(voltageRequest.withOutput(targetVoltage));
}

public static Rotation2d subtractFrom360Degrees(Rotation2d angleToSubtract) {
return Rotation2d.fromDegrees(360 - angleToSubtract.getDegrees());
}

private Pose3d calculateVisualizationPose() {
final Transform3d armTransform = new Transform3d(
new Translation3d(),
new Translation3d(0, 0, RobotContainer.ELEVATOR.getPositionMeters()),
new Rotation3d(0, getAngle().getRadians(), 0)
);
return ArmConstants.ARM_VISUALIZATION_ORIGIN_POINT.transformBy(armTransform);
Expand Down
9 changes: 5 additions & 4 deletions src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.StartEndCommand;
import frc.trigon.robot.RobotContainer;
import lib.commands.ExecuteEndCommand;
import lib.commands.GearRatioCalculationCommand;
import lib.commands.NetworkTablesCommand;

Expand Down Expand Up @@ -33,31 +34,31 @@ public static Command getGearRatioCalulationCommand() {
}

public static Command getSetTargetStateCommand(ArmConstants.ArmState targetState, boolean isStateReversed) {
return new StartEndCommand(
return new ExecuteEndCommand(
() -> RobotContainer.ARM.setTargetState(targetState, isStateReversed),
RobotContainer.ARM::stop,
RobotContainer.ARM
);
}

public static Command getSetTargetStateCommand(ArmConstants.ArmState targetState) {
return new StartEndCommand(
return new ExecuteEndCommand(
() -> RobotContainer.ARM.setTargetState(targetState),
RobotContainer.ARM::stop,
RobotContainer.ARM
);
}

public static Command getPrepareForStateCommand(ArmConstants.ArmState targetState, boolean isStateReversed) {
return new StartEndCommand(
return new ExecuteEndCommand(
() -> RobotContainer.ARM.prepareForState(targetState, isStateReversed),
RobotContainer.ARM::stop,
RobotContainer.ARM
);
}

public static Command getPrepareForStateCommand(ArmConstants.ArmState targetState) {
return new StartEndCommand(
return new ExecuteEndCommand(
() -> RobotContainer.ARM.prepareForState(targetState),
RobotContainer.ARM::stop,
RobotContainer.ARM
Expand Down
33 changes: 17 additions & 16 deletions src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -50,26 +50,26 @@ public class ArmConstants {
static final SimpleSensor DISTANCE_SENSOR = SimpleSensor.createDigitalSensor(DISTANCE_SENSOR_CHANNEL, DISTANCE_SENSOR_NAME);

private static final double
ARM_GEAR_RATIO = 50,
ARM_GEAR_RATIO = 40,
END_EFFECTOR_GEAR_RATIO = 17;
private static final double ARM_MOTOR_CURRENT_LIMIT = 50;
private static final double ANGLE_ENCODER_GRAVITY_OFFSET = 0;
static final double POSITION_OFFSET_FROM_GRAVITY_OFFSET = RobotHardwareStats.isSimulation() ? 0 - Conversions.degreesToRotations(90) : 0 + Conversions.degreesToRotations(0) - ANGLE_ENCODER_GRAVITY_OFFSET;
private static final boolean SHOULD_ARM_FOLLOWER_OPPOSE_MASTER = false;
static final double
ARM_DEFAULT_MAXIMUM_VELOCITY = RobotHardwareStats.isSimulation() ? 0.6 : 0,
ARM_DEFAULT_MAXIMUM_ACCELERATION = RobotHardwareStats.isSimulation() ? 1.5 : 0,
ARM_DEFAULT_MAXIMUM_VELOCITY = RobotHardwareStats.isSimulation() ? 2.4614 : 0,
ARM_DEFAULT_MAXIMUM_ACCELERATION = RobotHardwareStats.isSimulation() ? 67.2344 : 0,
ARM_DEFAULT_MAXIMUM_JERK = ARM_DEFAULT_MAXIMUM_ACCELERATION * 10;
static final boolean FOC_ENABLED = true;

public static final double ARM_LENGTH_METERS = 0.52;
private static final int
ARM_MOTOR_AMOUNT = 2,
END_EFFECTOR_MOTOR_AMOUNT = 1;
private static final DCMotor
ARM_GEARBOX = DCMotor.getKrakenX60Foc(ARM_MOTOR_AMOUNT),
END_EFFECTOR_GEARBOX = DCMotor.getKrakenX60Foc(END_EFFECTOR_MOTOR_AMOUNT);
private static final double
ARM_LENGTH_METERS = 0.67,
ARM_MASS_KILOGRAMS = 3.5,
END_EFFECTOR_MOMENT_OF_INERTIA = 0.003,
END_EFFECTOR_MAXIMUM_DISPLAYABLE_VELOCITY = 12;
Expand All @@ -95,7 +95,7 @@ public class ArmConstants {

static final SysIdRoutine.Config ARM_SYSID_CONFIG = new SysIdRoutine.Config(
Units.Volts.of(1.5).per(Units.Seconds),
Units.Volts.of(1.5),
Units.Volts.of(3),
Units.Second.of(1000)
);

Expand All @@ -110,7 +110,7 @@ public class ArmConstants {
);

static final Pose3d ARM_VISUALIZATION_ORIGIN_POINT = new Pose3d(
new Translation3d(0, 0.0954, 0.9517),
new Translation3d(0, -0.0954, 0.3573),
new Rotation3d(0, 0, 0)
);

Expand All @@ -120,7 +120,6 @@ public class ArmConstants {
CommandScheduler.getInstance().getActiveButtonLoop(),
DISTANCE_SENSOR::getBinaryValue
).debounce(COLLECTION_DETECTION_DEBOUNCE_TIME_SECONDS);
static final Rotation2d FULL_ROTATION = Rotation2d.fromDegrees(360);

static {
configureArmMasterMotor();
Expand All @@ -143,13 +142,13 @@ private static void configureArmMasterMotor() {
config.Feedback.FeedbackRemoteSensorID = ARM_MASTER_MOTOR.getID();
config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder;

config.Slot0.kP = RobotHardwareStats.isSimulation() ? 50 : 0;
config.Slot0.kP = RobotHardwareStats.isSimulation() ? 100 : 0;
config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0;
config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0 : 0;
config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.0067258 : 0;
config.Slot0.kV = RobotHardwareStats.isSimulation() ? 6.2 : 0;
config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.063357 : 0;
config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0.15048 : 0;
config.Slot0.kD = RobotHardwareStats.isSimulation() ? 1 : 0;
config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.026331 : 0;
config.Slot0.kV = RobotHardwareStats.isSimulation() ? 4.8752 : 0;
config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.17848 : 0;
config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0.1117 : 0;

config.Slot0.GravityType = GravityTypeValue.Arm_Cosine;
config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign;
Expand Down Expand Up @@ -179,7 +178,8 @@ private static void configureArmFollowerMotor() {
config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;

config.CurrentLimits.SupplyCurrentLimit = ARM_MOTOR_CURRENT_LIMIT;
config.CurrentLimits.StatorCurrentLimitEnable = true;
config.CurrentLimits.StatorCurrentLimit = ARM_MOTOR_CURRENT_LIMIT;

ARM_FOLLOWER_MOTOR.applyConfiguration(config);

Expand All @@ -196,7 +196,8 @@ private static void configureEndEffectorMotor() {
config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
config.Feedback.RotorToSensorRatio = END_EFFECTOR_GEAR_RATIO;

config.CurrentLimits.SupplyCurrentLimit = 80;
config.CurrentLimits.StatorCurrentLimitEnable = true;
config.CurrentLimits.StatorCurrentLimit = 80;

END_EFFECTOR_MOTOR.applyConfiguration(config);
END_EFFECTOR_MOTOR.setPhysicsSimulation(END_EFFECTOR_SIMULATION);
Expand Down Expand Up @@ -234,7 +235,7 @@ public enum ArmState {
PREPARE_SCORE_L1(Rotation2d.fromDegrees(80), 0),
SCORE_L1(Rotation2d.fromDegrees(75), 4),
PREPARE_SCORE_L2(Rotation2d.fromDegrees(95), 0),
SCORE_L2(Rotation2d.fromDegrees(90), 4),
SCORE_L2(Rotation2d.fromDegrees(180), 4),
PREPARE_SCORE_L3(Rotation2d.fromDegrees(95), 0),
SCORE_L3(Rotation2d.fromDegrees(90), 4),
PREPARE_SCORE_L4(Rotation2d.fromDegrees(95), 0),
Expand Down
29 changes: 22 additions & 7 deletions src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,11 +9,13 @@
import edu.wpi.first.units.Units;
import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import frc.trigon.robot.RobotContainer;
import frc.trigon.robot.subsystems.MotorSubsystem;
import org.littletonrobotics.junction.Logger;
import frc.trigon.robot.subsystems.arm.ArmConstants;
import lib.hardware.phoenix6.talonfx.TalonFXMotor;
import lib.hardware.phoenix6.talonfx.TalonFXSignal;
import lib.utilities.Conversions;
import org.littletonrobotics.junction.Logger;

public class Elevator extends MotorSubsystem {
private final TalonFXMotor masterMotor = ElevatorConstants.MASTER_MOTOR;
Expand All @@ -23,7 +25,7 @@ public class Elevator extends MotorSubsystem {
ElevatorConstants.DEFAULT_MAXIMUM_VELOCITY,
ElevatorConstants.DEFAULT_MAXIMUM_ACCELERATION,
ElevatorConstants.DEFAULT_MAXIMUM_ACCELERATION * 10
).withEnableFOC(ElevatorConstants.FOC_ENABLED);
).withEnableFOC(ElevatorConstants.FOC_ENABLED);
private ElevatorConstants.ElevatorState targetState = ElevatorConstants.ElevatorState.REST;

public Elevator() {
Expand Down Expand Up @@ -75,14 +77,31 @@ public void updatePeriodically() {
Logger.recordOutput("Elevator/CurrentPositionMeters", getPositionMeters());
}

public double getPositionMeters() {
return rotationsToMeters(getPositionRotations());
}

public boolean isElevatorAboveSafeZone() {
return getPositionMeters() >= ElevatorConstants.MAXIMUM_ELEVATOR_SAFE_ZONE_METERS;
}

void prepareLoadCoral() {
this.targetState = ElevatorConstants.ElevatorState.LOAD_CORAL;
scalePositionRequestSpeed(targetState.speedScalar);
masterMotor.setControl(positionRequest.withPosition(metersToRotations(ElevatorConstants.ElevatorState.LOAD_CORAL.targetPositionMeters)));
}

void setTargetState(ElevatorConstants.ElevatorState targetState) {
this.targetState = targetState;
scalePositionRequestSpeed(targetState.speedScalar);
setTargetPositionRotations(metersToRotations(targetState.targetPositionMeters));
}

void setTargetPositionRotations(double targetPositionRotations) {
masterMotor.setControl(positionRequest.withPosition(targetPositionRotations));
final double elevatorHeightFromArm = Math.cos(RobotContainer.ARM.getAngle().getRadians()) * ArmConstants.ARM_LENGTH_METERS;
final double minimumSafeHeightMeters = (RobotContainer.ARM.isArmAboveSafeZone() ? 0 : elevatorHeightFromArm) + ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS;
final double minimumSafeHeightRotations = metersToRotations(minimumSafeHeightMeters);
masterMotor.setControl(positionRequest.withPosition(Math.max(targetPositionRotations, minimumSafeHeightRotations)));
}

private Pose3d getFirstStageComponentPose() {
Expand Down Expand Up @@ -117,10 +136,6 @@ private void scalePositionRequestSpeed(double speedScalar) {
positionRequest.Jerk = positionRequest.Acceleration * 10;
}

private double getPositionMeters() {
return rotationsToMeters(getPositionRotations());
}

private double rotationsToMeters(double positionsRotations) {
return Conversions.rotationsToDistance(positionsRotations, ElevatorConstants.DRUM_DIAMETER_METERS);
}
Expand Down
16 changes: 13 additions & 3 deletions src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,8 @@
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.StartEndCommand;
import frc.trigon.robot.RobotContainer;
import frc.trigon.robot.subsystems.arm.ArmConstants;
import lib.commands.ExecuteEndCommand;
import lib.commands.NetworkTablesCommand;

import java.util.Set;
Expand All @@ -17,16 +19,24 @@ public static Command getDebbugingCommand() {
);
}

public static Command getSetTargetStateCommand(ElevatorConstants.ElevatorState targetState) {
public static Command getPrepareLoadCoral() {
return new StartEndCommand(
RobotContainer.ELEVATOR::prepareLoadCoral,
RobotContainer.ELEVATOR::stop,
RobotContainer.ELEVATOR
).onlyIf(() -> RobotContainer.ARM.atState(ArmConstants.ArmState.REST));
}

public static Command getSetTargetStateCommand(ElevatorConstants.ElevatorState targetState) {
return new ExecuteEndCommand(
() -> RobotContainer.ELEVATOR.setTargetState(targetState),
RobotContainer.ELEVATOR::stop,
RobotContainer.ELEVATOR
);
}

public static Command getSetTargetStateCommand(double targetPositionRotations) {
return new StartEndCommand(
return new ExecuteEndCommand(
() -> RobotContainer.ELEVATOR.setTargetPositionRotations(targetPositionRotations),
RobotContainer.ELEVATOR::stop,
RobotContainer.ELEVATOR
Expand Down
Loading
Loading