Skip to content

Commit a274b53

Browse files
ShmayaRStrflightmight09
ShmayaR
and
Strflightmight09
authored
Arm logic (#16)
* changed elevator so arm has access * added logic to stop arm from hitting anything * changed commands to work with logic * Update ElevatorConstants.java * added load coral command which overrides arm logic * updated poses * updated stuff * fixed logic * fixed logic issue and added d to arm pid * fixed stuff * made function more readable * Update Elevator.java * rename * Update Arm.java * removed magic number * moved into more variables * added javadoc * switched current limit type to stator * renamed * renamed stuff * extracted into multiple meathods * added command for checking if at an abgle * Update Arm.java * Update Arm.java * fixed arm logic * better pid * Update Arm.java * added gravity offset to the getAngle method * Update src/main/java/frc/trigon/robot/subsystems/arm/Arm.java Co-authored-by: Strflightmight09 <[email protected]> * siwtched set target state commands from execute to functional so that the end effector only gets applid voltage once and not every loop * made load coral function part of setTargetState * Update Arm.java * got rid of unnecesary functions * put logic in setTargetAngle into different function * Update Arm.java * separated setTargetPosition logic into differant logic * Update ArmConstants.java * Update ArmConstants.java * Update Arm.java * Update Arm.java * cleaned commands * Update Elevator.java * changed names --------- Co-authored-by: Strflightmight09 <[email protected]>
1 parent fd3f95f commit a274b53

File tree

7 files changed

+143
-85
lines changed

7 files changed

+143
-85
lines changed

‎src/main/java/frc/trigon/robot/subsystems/arm/Arm.java‎

Lines changed: 43 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -2,10 +2,12 @@
22

33
import com.ctre.phoenix6.controls.DynamicMotionMagicVoltage;
44
import com.ctre.phoenix6.controls.VoltageOut;
5+
import edu.wpi.first.math.MathUtil;
56
import edu.wpi.first.math.geometry.*;
67
import edu.wpi.first.units.Units;
78
import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog;
89
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
10+
import frc.trigon.robot.RobotContainer;
911
import frc.trigon.robot.subsystems.MotorSubsystem;
1012
import lib.hardware.phoenix6.cancoder.CANcoderEncoder;
1113
import lib.hardware.phoenix6.cancoder.CANcoderSignal;
@@ -81,31 +83,29 @@ public void sysIDDrive(double targetVoltage) {
8183
armMasterMotor.setControl(voltageRequest.withOutput(targetVoltage));
8284
}
8385

86+
public boolean isArmAboveSafeAngle() {
87+
return getAngle().getDegrees() >= ArmConstants.MAXIMUM_ARM_SAFE_ANGLE.getDegrees();
88+
}
89+
8490
public boolean atState(ArmConstants.ArmState targetState, boolean isStateReversed) {
8591
if (isStateReversed)
86-
return this.targetState == targetState && atTargetAngle(isStateReversed);
92+
return this.targetState == targetState && atAngle(subtractFrom360Degrees(targetState.targetAngle));
8793
return atState(targetState);
8894
}
8995

9096
public boolean atState(ArmConstants.ArmState targetState) {
91-
return this.targetState == targetState && atTargetAngle();
97+
return this.targetState == targetState && atAngle(targetState.targetAngle);
9298
}
9399

94-
public boolean atTargetAngle(boolean isStateReversed) {
100+
public boolean atTargetAngle() {
95101
if (isStateReversed) {
96-
final double currentToTargetStateDifferenceDegrees = Math.abs(ArmConstants.FULL_ROTATION.minus(targetState.targetAngle).minus(getAngle()).getDegrees());
97-
return currentToTargetStateDifferenceDegrees < ArmConstants.ANGLE_TOLERANCE.getDegrees();
102+
return atAngle(subtractFrom360Degrees(targetState.targetAngle));
98103
}
99-
return atTargetAngle();
100-
}
101-
102-
public boolean atTargetAngle() {
103-
final double currentToTargetStateDifferenceDegrees = Math.abs(targetState.targetAngle.minus(getAngle()).getDegrees());
104-
return currentToTargetStateDifferenceDegrees < ArmConstants.ANGLE_TOLERANCE.getDegrees();
104+
return atAngle(targetState.targetAngle);
105105
}
106106

107-
public boolean hasGamePiece() {
108-
return ArmConstants.COLLECTION_DETECTION_BOOLEAN_EVENT.getAsBoolean();
107+
public Rotation2d getAngle() {
108+
return Rotation2d.fromRotations(angleEncoder.getSignal(CANcoderSignal.POSITION));
109109
}
110110

111111
void setTargetState(ArmConstants.ArmState targetState) {
@@ -120,7 +120,7 @@ void setTargetState(ArmConstants.ArmState targetState, boolean isStateReversed)
120120

121121
if (isStateReversed) {
122122
setTargetState(
123-
ArmConstants.FULL_ROTATION.minus(targetState.targetAngle)
123+
subtractFrom360Degrees(targetState.targetAngle)
124124
, targetState.targetEndEffectorVoltage
125125
);
126126
return;
@@ -132,40 +132,58 @@ void setTargetState(ArmConstants.ArmState targetState, boolean isStateReversed)
132132

133133
void setTargetState(Rotation2d targetAngle, double targetVoltage) {
134134
setTargetAngle(targetAngle);
135-
setTargetVoltage(targetVoltage);
135+
setEndEffectorTargetVoltage(targetVoltage);
136136
}
137137

138-
void prepareForState(ArmConstants.ArmState targetState) {
139-
prepareForState(targetState, false);
138+
void setArmState(ArmConstants.ArmState targetState) {
139+
setArmState(targetState, false);
140140
}
141141

142-
void prepareForState(ArmConstants.ArmState targetState, boolean isStateReversed) {
142+
void setArmState(ArmConstants.ArmState targetState, boolean isStateReversed) {
143143
this.isStateReversed = isStateReversed;
144144
this.targetState = targetState;
145-
145+
146146
if (isStateReversed) {
147-
setTargetAngle(ArmConstants.FULL_ROTATION.minus(targetState.targetAngle));
147+
setTargetAngle(subtractFrom360Degrees(targetState.targetAngle));
148148
return;
149149
}
150150
setTargetAngle(targetState.targetAngle);
151151
}
152152

153-
private Rotation2d getAngle() {
154-
return Rotation2d.fromRotations(angleEncoder.getSignal(CANcoderSignal.POSITION));
153+
void setEndEffectorState(ArmConstants.ArmState targetState) {
154+
setEndEffectorTargetVoltage(targetState.targetEndEffectorVoltage);
155155
}
156156

157157
private void setTargetAngle(Rotation2d targetAngle) {
158-
armMasterMotor.setControl(positionRequest.withPosition(targetAngle.getRotations()));
158+
armMasterMotor.setControl(positionRequest.withPosition(Math.max(targetAngle.getRotations(), calculateMinimumArmSafeAngle().getRotations())));
159+
}
160+
161+
private Rotation2d calculateMinimumArmSafeAngle() {
162+
final boolean isElevatorAboveSafeZone = RobotContainer.ELEVATOR.isElevatorAboveSafeZone();
163+
final double heightFromSafeZone = RobotContainer.ELEVATOR.getElevatorHeightFromMinimumSafeZone();
164+
final double cosOfMinimumSafeAngle = MathUtil.clamp(heightFromSafeZone / ArmConstants.ARM_LENGTH_METERS, 0, 1);
165+
return isElevatorAboveSafeZone
166+
? Rotation2d.fromRadians(0)
167+
: Rotation2d.fromRadians(Math.acos(cosOfMinimumSafeAngle));
159168
}
160169

161-
private void setTargetVoltage(double targetVoltage) {
170+
private void setEndEffectorTargetVoltage(double targetVoltage) {
162171
ArmConstants.END_EFFECTOR_MECHANISM.setTargetVelocity(targetVoltage);
163172
endEffectorMotor.setControl(voltageRequest.withOutput(targetVoltage));
164173
}
165174

175+
private boolean atAngle(Rotation2d targetAngle) {
176+
final double currentToTargetAngleDifferenceDegrees = Math.abs(targetAngle.minus(getAngle()).getDegrees());
177+
return currentToTargetAngleDifferenceDegrees < ArmConstants.ANGLE_TOLERANCE.getDegrees();
178+
}
179+
180+
private static Rotation2d subtractFrom360Degrees(Rotation2d angleToSubtract) {
181+
return Rotation2d.fromDegrees(Rotation2d.k180deg.getDegrees() * 2 - angleToSubtract.getDegrees());
182+
}
183+
166184
private Pose3d calculateVisualizationPose() {
167185
final Transform3d armTransform = new Transform3d(
168-
new Translation3d(),
186+
new Translation3d(0, 0, RobotContainer.ELEVATOR.getPositionMeters()),
169187
new Rotation3d(0, getAngle().getRadians(), 0)
170188
);
171189
return ArmConstants.ARM_VISUALIZATION_ORIGIN_POINT.transformBy(armTransform);

‎src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java‎

Lines changed: 15 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -2,8 +2,9 @@
22

33
import edu.wpi.first.math.geometry.Rotation2d;
44
import edu.wpi.first.wpilibj2.command.Command;
5-
import edu.wpi.first.wpilibj2.command.StartEndCommand;
5+
import edu.wpi.first.wpilibj2.command.FunctionalCommand;
66
import frc.trigon.robot.RobotContainer;
7+
import lib.commands.ExecuteEndCommand;
78
import lib.commands.GearRatioCalculationCommand;
89
import lib.commands.NetworkTablesCommand;
910

@@ -32,33 +33,27 @@ public static Command getGearRatioCalulationCommand() {
3233
);
3334
}
3435

35-
public static Command getSetTargetStateCommand(ArmConstants.ArmState targetState, boolean isStateReversed) {
36-
return new StartEndCommand(
37-
() -> RobotContainer.ARM.setTargetState(targetState, isStateReversed),
38-
RobotContainer.ARM::stop,
39-
RobotContainer.ARM
40-
);
41-
}
42-
4336
public static Command getSetTargetStateCommand(ArmConstants.ArmState targetState) {
44-
return new StartEndCommand(
45-
() -> RobotContainer.ARM.setTargetState(targetState),
46-
RobotContainer.ARM::stop,
47-
RobotContainer.ARM
48-
);
37+
return getSetTargetStateCommand(targetState, false);
4938
}
5039

51-
public static Command getPrepareForStateCommand(ArmConstants.ArmState targetState, boolean isStateReversed) {
52-
return new StartEndCommand(
53-
() -> RobotContainer.ARM.prepareForState(targetState, isStateReversed),
54-
RobotContainer.ARM::stop,
40+
public static Command getSetTargetStateCommand(ArmConstants.ArmState targetState, boolean isStateReversed) {
41+
return new FunctionalCommand(
42+
() -> RobotContainer.ARM.setEndEffectorState(targetState),
43+
() -> RobotContainer.ARM.setArmState(targetState, isStateReversed),
44+
interrupted -> RobotContainer.ARM.stop(),
45+
() -> false,
5546
RobotContainer.ARM
5647
);
5748
}
5849

5950
public static Command getPrepareForStateCommand(ArmConstants.ArmState targetState) {
60-
return new StartEndCommand(
61-
() -> RobotContainer.ARM.prepareForState(targetState),
51+
return getPrepareForStateCommand(targetState, false);
52+
}
53+
54+
public static Command getPrepareForStateCommand(ArmConstants.ArmState targetState, boolean isStateReversed) {
55+
return new ExecuteEndCommand(
56+
() -> RobotContainer.ARM.setArmState(targetState, isStateReversed),
6257
RobotContainer.ARM::stop,
6358
RobotContainer.ARM
6459
);

‎src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java‎

Lines changed: 23 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -50,26 +50,26 @@ public class ArmConstants {
5050
static final SimpleSensor DISTANCE_SENSOR = SimpleSensor.createDigitalSensor(DISTANCE_SENSOR_CHANNEL, DISTANCE_SENSOR_NAME);
5151

5252
private static final double
53-
ARM_GEAR_RATIO = 50,
53+
ARM_GEAR_RATIO = 40,
5454
END_EFFECTOR_GEAR_RATIO = 17;
5555
private static final double ARM_MOTOR_CURRENT_LIMIT = 50;
5656
private static final double ANGLE_ENCODER_GRAVITY_OFFSET = 0;
5757
static final double POSITION_OFFSET_FROM_GRAVITY_OFFSET = RobotHardwareStats.isSimulation() ? 0 - Conversions.degreesToRotations(90) : 0 + Conversions.degreesToRotations(0) - ANGLE_ENCODER_GRAVITY_OFFSET;
5858
private static final boolean SHOULD_ARM_FOLLOWER_OPPOSE_MASTER = false;
5959
static final double
60-
ARM_DEFAULT_MAXIMUM_VELOCITY = RobotHardwareStats.isSimulation() ? 0.6 : 0,
61-
ARM_DEFAULT_MAXIMUM_ACCELERATION = RobotHardwareStats.isSimulation() ? 1.5 : 0,
60+
ARM_DEFAULT_MAXIMUM_VELOCITY = RobotHardwareStats.isSimulation() ? 2.4614 : 0,
61+
ARM_DEFAULT_MAXIMUM_ACCELERATION = RobotHardwareStats.isSimulation() ? 67.2344 : 0,
6262
ARM_DEFAULT_MAXIMUM_JERK = ARM_DEFAULT_MAXIMUM_ACCELERATION * 10;
6363
static final boolean FOC_ENABLED = true;
6464

65+
public static final double ARM_LENGTH_METERS = 0.52;
6566
private static final int
6667
ARM_MOTOR_AMOUNT = 2,
6768
END_EFFECTOR_MOTOR_AMOUNT = 1;
6869
private static final DCMotor
6970
ARM_GEARBOX = DCMotor.getKrakenX60Foc(ARM_MOTOR_AMOUNT),
7071
END_EFFECTOR_GEARBOX = DCMotor.getKrakenX60Foc(END_EFFECTOR_MOTOR_AMOUNT);
7172
private static final double
72-
ARM_LENGTH_METERS = 0.67,
7373
ARM_MASS_KILOGRAMS = 3.5,
7474
END_EFFECTOR_MOMENT_OF_INERTIA = 0.003,
7575
END_EFFECTOR_MAXIMUM_DISPLAYABLE_VELOCITY = 12;
@@ -95,7 +95,7 @@ public class ArmConstants {
9595

9696
static final SysIdRoutine.Config ARM_SYSID_CONFIG = new SysIdRoutine.Config(
9797
Units.Volts.of(1.5).per(Units.Seconds),
98-
Units.Volts.of(1.5),
98+
Units.Volts.of(3),
9999
Units.Second.of(1000)
100100
);
101101

@@ -110,17 +110,19 @@ public class ArmConstants {
110110
);
111111

112112
static final Pose3d ARM_VISUALIZATION_ORIGIN_POINT = new Pose3d(
113-
new Translation3d(0, 0.0954, 0.9517),
113+
new Translation3d(0, -0.0954, 0.3573),
114114
new Rotation3d(0, 0, 0)
115115
);
116-
116+
/**
117+
* The highest point of the arms angular zone where the safety logic applies.
118+
*/
119+
static final Rotation2d MAXIMUM_ARM_SAFE_ANGLE = Rotation2d.fromDegrees(90);
117120
static final Rotation2d ANGLE_TOLERANCE = Rotation2d.fromDegrees(0);
118121
private static final double COLLECTION_DETECTION_DEBOUNCE_TIME_SECONDS = 0.2;
119122
static final BooleanEvent COLLECTION_DETECTION_BOOLEAN_EVENT = new BooleanEvent(
120123
CommandScheduler.getInstance().getActiveButtonLoop(),
121124
DISTANCE_SENSOR::getBinaryValue
122125
).debounce(COLLECTION_DETECTION_DEBOUNCE_TIME_SECONDS);
123-
static final Rotation2d FULL_ROTATION = Rotation2d.fromDegrees(360);
124126

125127
static {
126128
configureArmMasterMotor();
@@ -143,13 +145,13 @@ private static void configureArmMasterMotor() {
143145
config.Feedback.FeedbackRemoteSensorID = ARM_MASTER_MOTOR.getID();
144146
config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder;
145147

146-
config.Slot0.kP = RobotHardwareStats.isSimulation() ? 50 : 0;
148+
config.Slot0.kP = RobotHardwareStats.isSimulation() ? 38 : 0;
147149
config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0;
148-
config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0 : 0;
149-
config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.0067258 : 0;
150-
config.Slot0.kV = RobotHardwareStats.isSimulation() ? 6.2 : 0;
151-
config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.063357 : 0;
152-
config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0.15048 : 0;
150+
config.Slot0.kD = RobotHardwareStats.isSimulation() ? 1 : 0;
151+
config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.026331 : 0;
152+
config.Slot0.kV = RobotHardwareStats.isSimulation() ? 4.8752 : 0;
153+
config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.17848 : 0;
154+
config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0.1117 : 0;
153155

154156
config.Slot0.GravityType = GravityTypeValue.Arm_Cosine;
155157
config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign;
@@ -158,7 +160,8 @@ private static void configureArmMasterMotor() {
158160
config.MotionMagic.MotionMagicAcceleration = ARM_DEFAULT_MAXIMUM_ACCELERATION;
159161
config.MotionMagic.MotionMagicJerk = config.MotionMagic.MotionMagicAcceleration * 10;
160162

161-
config.CurrentLimits.SupplyCurrentLimit = ARM_MOTOR_CURRENT_LIMIT;
163+
config.CurrentLimits.StatorCurrentLimitEnable = true;
164+
config.CurrentLimits.StatorCurrentLimit = ARM_MOTOR_CURRENT_LIMIT;
162165

163166
ARM_MASTER_MOTOR.applyConfiguration(config);
164167
ARM_MASTER_MOTOR.setPhysicsSimulation(ARM_SIMULATION);
@@ -179,7 +182,8 @@ private static void configureArmFollowerMotor() {
179182
config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
180183
config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
181184

182-
config.CurrentLimits.SupplyCurrentLimit = ARM_MOTOR_CURRENT_LIMIT;
185+
config.CurrentLimits.StatorCurrentLimitEnable = true;
186+
config.CurrentLimits.StatorCurrentLimit = ARM_MOTOR_CURRENT_LIMIT;
183187

184188
ARM_FOLLOWER_MOTOR.applyConfiguration(config);
185189

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

199-
config.CurrentLimits.SupplyCurrentLimit = 80;
203+
config.CurrentLimits.StatorCurrentLimitEnable = true;
204+
config.CurrentLimits.StatorCurrentLimit = 80;
200205

201206
END_EFFECTOR_MOTOR.applyConfiguration(config);
202207
END_EFFECTOR_MOTOR.setPhysicsSimulation(END_EFFECTOR_SIMULATION);
@@ -234,7 +239,7 @@ public enum ArmState {
234239
PREPARE_SCORE_L1(Rotation2d.fromDegrees(80), 0),
235240
SCORE_L1(Rotation2d.fromDegrees(75), 4),
236241
PREPARE_SCORE_L2(Rotation2d.fromDegrees(95), 0),
237-
SCORE_L2(Rotation2d.fromDegrees(90), 4),
242+
SCORE_L2(Rotation2d.fromDegrees(180), 4),
238243
PREPARE_SCORE_L3(Rotation2d.fromDegrees(95), 0),
239244
SCORE_L3(Rotation2d.fromDegrees(90), 4),
240245
PREPARE_SCORE_L4(Rotation2d.fromDegrees(95), 0),

‎src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java‎

Lines changed: 31 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -9,11 +9,13 @@
99
import edu.wpi.first.units.Units;
1010
import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog;
1111
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
12+
import frc.trigon.robot.RobotContainer;
1213
import frc.trigon.robot.subsystems.MotorSubsystem;
13-
import org.littletonrobotics.junction.Logger;
14+
import frc.trigon.robot.subsystems.arm.ArmConstants;
1415
import lib.hardware.phoenix6.talonfx.TalonFXMotor;
1516
import lib.hardware.phoenix6.talonfx.TalonFXSignal;
1617
import lib.utilities.Conversions;
18+
import org.littletonrobotics.junction.Logger;
1719

1820
public class Elevator extends MotorSubsystem {
1921
private final TalonFXMotor masterMotor = ElevatorConstants.MASTER_MOTOR;
@@ -23,7 +25,7 @@ public class Elevator extends MotorSubsystem {
2325
ElevatorConstants.DEFAULT_MAXIMUM_VELOCITY,
2426
ElevatorConstants.DEFAULT_MAXIMUM_ACCELERATION,
2527
ElevatorConstants.DEFAULT_MAXIMUM_ACCELERATION * 10
26-
).withEnableFOC(ElevatorConstants.FOC_ENABLED);
28+
).withEnableFOC(ElevatorConstants.FOC_ENABLED);
2729
private ElevatorConstants.ElevatorState targetState = ElevatorConstants.ElevatorState.REST;
2830

2931
public Elevator() {
@@ -75,14 +77,40 @@ public void updatePeriodically() {
7577
Logger.recordOutput("Elevator/CurrentPositionMeters", getPositionMeters());
7678
}
7779

80+
public double getPositionMeters() {
81+
return rotationsToMeters(getPositionRotations());
82+
}
83+
84+
public boolean isElevatorAboveSafeZone() {
85+
return getPositionMeters() >= ElevatorConstants.MAXIMUM_ELEVATOR_SAFE_ZONE_METERS;
86+
}
87+
88+
public double getElevatorHeightFromMinimumSafeZone() {
89+
return getPositionMeters() - ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS;
90+
}
91+
7892
void setTargetState(ElevatorConstants.ElevatorState targetState) {
7993
this.targetState = targetState;
8094
scalePositionRequestSpeed(targetState.speedScalar);
95+
if (targetState == ElevatorConstants.ElevatorState.LOAD_CORAL) {
96+
masterMotor.setControl(positionRequest.withPosition(metersToRotations(targetState.targetPositionMeters)));
97+
return;
98+
}
8199
setTargetPositionRotations(metersToRotations(targetState.targetPositionMeters));
82100
}
83101

84102
void setTargetPositionRotations(double targetPositionRotations) {
85-
masterMotor.setControl(positionRequest.withPosition(targetPositionRotations));
103+
masterMotor.setControl(positionRequest.withPosition(Math.max(targetPositionRotations, calculateMinimumSafeElevatorHeightRotations())));
104+
}
105+
106+
private double calculateMinimumSafeElevatorHeightRotations() {
107+
final double armCos = RobotContainer.ARM.getAngle().getRadians();
108+
final double elevatorHeightFromArm = Math.cos(armCos) * ArmConstants.ARM_LENGTH_METERS;
109+
final double minimumElevatorSafeZone = ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS;
110+
final double minimumSafeHeightMeters = (RobotContainer.ARM.isArmAboveSafeAngle()
111+
? 0 : elevatorHeightFromArm)
112+
+ minimumElevatorSafeZone;
113+
return metersToRotations(minimumSafeHeightMeters);
86114
}
87115

88116
private Pose3d getFirstStageComponentPose() {
@@ -117,10 +145,6 @@ private void scalePositionRequestSpeed(double speedScalar) {
117145
positionRequest.Jerk = positionRequest.Acceleration * 10;
118146
}
119147

120-
private double getPositionMeters() {
121-
return rotationsToMeters(getPositionRotations());
122-
}
123-
124148
private double rotationsToMeters(double positionsRotations) {
125149
return Conversions.rotationsToDistance(positionsRotations, ElevatorConstants.DRUM_DIAMETER_METERS);
126150
}

0 commit comments

Comments
 (0)