Skip to content

Commit f327b94

Browse files
ShmayaRShmayaR
committed
fixed logic
1 parent 4b60ce6 commit f327b94

File tree

4 files changed

+30
-15
lines changed

4 files changed

+30
-15
lines changed

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

Lines changed: 14 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -83,6 +83,10 @@ public void sysIDDrive(double targetVoltage) {
8383
armMasterMotor.setControl(voltageRequest.withOutput(targetVoltage));
8484
}
8585

86+
public boolean isArmAboveSafeZone() {
87+
return getAngle().getDegrees() >= 90;
88+
}
89+
8690
public boolean atState(ArmConstants.ArmState targetState, boolean isStateReversed) {
8791
if (isStateReversed)
8892
return this.targetState == targetState && atTargetAngle(isStateReversed);
@@ -95,7 +99,7 @@ public boolean atState(ArmConstants.ArmState targetState) {
9599

96100
public boolean atTargetAngle(boolean isStateReversed) {
97101
if (isStateReversed) {
98-
final double currentToTargetStateDifferenceDegrees = Math.abs(ArmConstants.FULL_ROTATION.minus(targetState.targetAngle).minus(getAngle()).getDegrees());
102+
final double currentToTargetStateDifferenceDegrees = Math.abs(subtractFrom360(targetState.targetAngle).minus(getAngle()).getDegrees());
99103
return currentToTargetStateDifferenceDegrees < ArmConstants.ANGLE_TOLERANCE.getDegrees();
100104
}
101105
return atTargetAngle();
@@ -122,7 +126,7 @@ void setTargetState(ArmConstants.ArmState targetState, boolean isStateReversed)
122126

123127
if (isStateReversed) {
124128
setTargetState(
125-
ArmConstants.FULL_ROTATION.minus(targetState.targetAngle)
129+
subtractFrom360(targetState.targetAngle)
126130
, targetState.targetEndEffectorVoltage
127131
);
128132
return;
@@ -146,14 +150,16 @@ void prepareForState(ArmConstants.ArmState targetState, boolean isStateReversed)
146150
this.targetState = targetState;
147151

148152
if (isStateReversed) {
149-
setTargetAngle(ArmConstants.FULL_ROTATION.minus(targetState.targetAngle));
153+
subtractFrom360(targetState.targetAngle);
150154
return;
151155
}
152156
setTargetAngle(targetState.targetAngle);
153157
}
154158

155159
private void setTargetAngle(Rotation2d targetAngle) {
156-
Rotation2d minimumSafeAngle = Rotation2d.fromDegrees(Math.acos((RobotContainer.ELEVATOR.getPositionMeters() - ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS) / ArmConstants.ARM_LENGTH_METERS));
160+
final double heightFromSafeZone = RobotContainer.ELEVATOR.getPositionMeters() - ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS;
161+
final double cosMinimumAngle = heightFromSafeZone / ArmConstants.ARM_LENGTH_METERS;
162+
final Rotation2d minimumSafeAngle = Rotation2d.fromRadians(RobotContainer.ELEVATOR.isElevatorAboveSafeZone() ? 0 : Math.acos(cosMinimumAngle));
157163
armMasterMotor.setControl(positionRequest.withPosition(Math.max(targetAngle.getRotations(), minimumSafeAngle.getRotations())));
158164
}
159165

@@ -162,6 +168,10 @@ private void setTargetVoltage(double targetVoltage) {
162168
endEffectorMotor.setControl(voltageRequest.withOutput(targetVoltage));
163169
}
164170

171+
public static Rotation2d subtractFrom360(Rotation2d angleToSubtract) {
172+
return Rotation2d.fromDegrees(360 - angleToSubtract.getDegrees());
173+
}
174+
165175
private Pose3d calculateVisualizationPose() {
166176
final Transform3d armTransform = new Transform3d(
167177
new Translation3d(0, 0, RobotContainer.ELEVATOR.getPositionMeters()),

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

Lines changed: 9 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -57,8 +57,8 @@ public class ArmConstants {
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() ? 1.93 : 0,
61-
ARM_DEFAULT_MAXIMUM_ACCELERATION = RobotHardwareStats.isSimulation() ? 189 : 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

@@ -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

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

125124
static {
126125
configureArmMasterMotor();
@@ -143,13 +142,13 @@ private static void configureArmMasterMotor() {
143142
config.Feedback.FeedbackRemoteSensorID = ARM_MASTER_MOTOR.getID();
144143
config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder;
145144

146-
config.Slot0.kP = RobotHardwareStats.isSimulation() ? 50 : 0;
145+
config.Slot0.kP = RobotHardwareStats.isSimulation() ? 100 : 0;
147146
config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0;
148147
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;
148+
config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.026331 : 0;
149+
config.Slot0.kV = RobotHardwareStats.isSimulation() ? 4.8752 : 0;
150+
config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.17848 : 0;
151+
config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0.1117 : 0;
153152

154153
config.Slot0.GravityType = GravityTypeValue.Arm_Cosine;
155154
config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign;
@@ -236,7 +235,7 @@ public enum ArmState {
236235
PREPARE_SCORE_L1(Rotation2d.fromDegrees(80), 0),
237236
SCORE_L1(Rotation2d.fromDegrees(75), 4),
238237
PREPARE_SCORE_L2(Rotation2d.fromDegrees(95), 0),
239-
SCORE_L2(Rotation2d.fromDegrees(90), 4),
238+
SCORE_L2(Rotation2d.fromDegrees(180), 4),
240239
PREPARE_SCORE_L3(Rotation2d.fromDegrees(95), 0),
241240
SCORE_L3(Rotation2d.fromDegrees(90), 4),
242241
PREPARE_SCORE_L4(Rotation2d.fromDegrees(95), 0),

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

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -81,6 +81,10 @@ public double getPositionMeters() {
8181
return rotationsToMeters(getPositionRotations());
8282
}
8383

84+
public boolean isElevatorAboveSafeZone() {
85+
return getPositionMeters() >= ElevatorConstants.MAXIMUM_ELEVATOR_SAFE_ZONE_METERS;
86+
}
87+
8488
void loadCoralFromElevator() {
8589
this.targetState = ElevatorConstants.ElevatorState.LOAD_CORAL;
8690
scalePositionRequestSpeed(targetState.speedScalar);
@@ -94,7 +98,7 @@ void setTargetState(ElevatorConstants.ElevatorState targetState) {
9498
}
9599

96100
void setTargetPositionRotations(double targetPositionRotations) {
97-
double minimumSafeHeightMeters = Math.cos(RobotContainer.ARM.getAngle().getDegrees()) * ArmConstants.ARM_LENGTH_METERS + ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS;
101+
double minimumSafeHeightMeters = RobotContainer.ARM.isArmAboveSafeZone() ? 0 : Math.cos(RobotContainer.ARM.getAngle().getDegrees()) * ArmConstants.ARM_LENGTH_METERS + ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS;
98102
double minimumSafeHeightRotations = metersToRotations(minimumSafeHeightMeters);
99103
masterMotor.setControl(positionRequest.withPosition(Math.max(targetPositionRotations, minimumSafeHeightRotations)));
100104
}

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

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,7 @@
1515
import edu.wpi.first.wpilibj.util.Color;
1616
import edu.wpi.first.wpilibj2.command.CommandScheduler;
1717
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
18+
import frc.trigon.robot.subsystems.arm.ArmConstants;
1819
import lib.hardware.RobotHardwareStats;
1920
import lib.hardware.misc.simplesensor.SimpleSensor;
2021
import lib.hardware.phoenix6.talonfx.TalonFXMotor;
@@ -92,6 +93,7 @@ public class ElevatorConstants {
9293
).debounce(REVERSE_LIMIT_SENSOR_DEBOUNCE_TIME_SECONDS);
9394

9495
public static final double MINIMUM_ELEVATOR_SAFE_ZONE_METERS = 0.05;
96+
public static final double MAXIMUM_ELEVATOR_SAFE_ZONE_METERS = MINIMUM_ELEVATOR_SAFE_ZONE_METERS + ArmConstants.ARM_LENGTH_METERS;
9597
private static final DoubleSupplier REVERSE_LIMIT_SENSOR_SIMULATION_SUPPLIER = () -> 0;
9698

9799
static {

0 commit comments

Comments
 (0)