Skip to content

Commit 843f615

Browse files
crkalapatcrkalapat
committed
Merged LiftSubsystem.java with Rachit
1 parent b2daa96 commit 843f615

File tree

1 file changed

+67
-25
lines changed

1 file changed

+67
-25
lines changed

‎src/main/java/frc/robot/subsystems/lift/LiftSubsystem.java‎

Lines changed: 67 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,6 @@
11
package frc.robot.subsystems.lift;
22

3+
import static edu.wpi.first.units.Units.Centimeters;
34
import static edu.wpi.first.units.Units.Degrees;
45
import static edu.wpi.first.units.Units.Inches;
56
import static edu.wpi.first.units.Units.Meters;
@@ -43,6 +44,12 @@ public static enum USER_INPUT {
4344
static final DutyCycleOut HOMING_SPEED = new DutyCycleOut(-0.05);
4445
static final Distance HOMING_EPSILON = Millimeters.of(5);
4546

47+
// Tolerance in cm of top and bottom minimum clearance
48+
static final Distance ELEVATOR_TOLERANCE = Centimeters.of(0.5);
49+
50+
// Tolerance in degrees of arm
51+
static final Angle ARM_TOLERANCE = Degrees.of(0.5);
52+
4653
static final Angle SCORING_L1_ANGLE = Degrees.of(0);
4754
static final Angle SCORING_L2_ANGLE = Degrees.of(0);
4855
static final Angle SCORING_L3_ANGLE = Degrees.of(0);
@@ -61,6 +68,7 @@ public static enum USER_INPUT {
6168
static final Distance L3_HEIGHT = Inches.of(0);
6269
static final Distance L4_HEIGHT = Inches.of(0);
6370

71+
6472
/* TODO: Actually get user input */
6573
private static USER_INPUT getUserInput() { return USER_INPUT.STOW; }
6674

@@ -96,6 +104,7 @@ public LiftStates nextState() {
96104
@Override
97105
public void initialize() {
98106
s_liftinstance.setElevatorHeight(STOW_HEIGHT);
107+
s_liftinstance.setArmAngle(STOW_ANGLE);
99108
}
100109

101110
@Override
@@ -165,7 +174,7 @@ public void initialize() {
165174

166175
@Override
167176
public SystemState nextState() {
168-
if (s_liftinstance.getArmAngle().gte(SAFE_INTAKE_ANGLE)) {
177+
if (s_liftinstance.getArmAngle().lte(SAFE_INTAKE_ANGLE)) {
169178
return STOW_L4_S1;
170179
}
171180
return this;
@@ -302,22 +311,21 @@ public void initialize() {
302311
s_liftinstance.setElevatorHeight(L4_HEIGHT);
303312
}
304313

305-
@Override
306-
public void execute() {
307-
if (s_liftinstance.getElevatorHeight() == CLEAR_HEIGHT && s_liftinstance.getArmAngle() == STOW_ANGLE) {
308-
s_liftinstance.setArmAngle(SCORING_L4_ANGLE);
309-
}
310-
}
311-
312314
@Override
313315
public SystemState nextState() {
314-
if (s_liftinstance.getElevatorHeight() == L4_HEIGHT && s_liftinstance.getArmAngle() == SCORING_L4_ANGLE) {
316+
if (s_liftinstance.getElevatorHeight().gte(CLEAR_HEIGHT)) {
315317
return L4;
316318
}
317319
return this;
318320
}
319321
},
320322
L4 {
323+
@Override
324+
public void initialize() {
325+
s_liftinstance.setElevatorHeight(L4_HEIGHT);
326+
s_liftinstance.setArmAngle(SCORING_L4_ANGLE);
327+
}
328+
321329
@Override
322330
public SystemState nextState() {
323331
if (getUserInput() == USER_INPUT.STOW) {
@@ -343,7 +351,7 @@ public void initialize() {
343351

344352
@Override
345353
public SystemState nextState() {
346-
if (s_liftinstance.getArmAngle() == STOW_ANGLE) {
354+
if (s_liftinstance.getArmAngle().gte(STOW_ANGLE)) {
347355
return STOW;
348356
}
349357
return this;
@@ -357,7 +365,7 @@ public void initialize() {
357365

358366
@Override
359367
public SystemState nextState() {
360-
if (s_liftinstance.getArmAngle() == STOW_ANGLE) {
368+
if (s_liftinstance.getArmAngle().gte(STOW_ANGLE)) {
361369
return STOW;
362370
}
363371
return this;
@@ -371,7 +379,7 @@ public void initialize() {
371379

372380
@Override
373381
public SystemState nextState() {
374-
if (s_liftinstance.getElevatorHeight() == CLEAR_HEIGHT) {
382+
if (s_liftinstance.getElevatorHeight().gte(CLEAR_HEIGHT)) {
375383
return L3_STOW_S2;
376384
}
377385
return this;
@@ -385,7 +393,7 @@ public void initialize() {
385393

386394
@Override
387395
public SystemState nextState() {
388-
if (s_liftinstance.getArmAngle() == STOW_ANGLE) {
396+
if (s_liftinstance.getArmAngle().gte(STOW_ANGLE)) {
389397
return STOW;
390398
}
391399
return this;
@@ -399,7 +407,7 @@ public void initialize() {
399407

400408
@Override
401409
public SystemState nextState() {
402-
if (s_liftinstance.getArmAngle() == STOW_ANGLE) {
410+
if (s_liftinstance.getArmAngle().gte(STOW_ANGLE)) {
403411
return STOW;
404412
}
405413
return this;
@@ -413,7 +421,7 @@ public void initialize() {
413421

414422
@Override
415423
public SystemState nextState() {
416-
if (s_liftinstance.getArmAngle() == SAFE_REEF_ANGLE_BOTTOM) {
424+
if (s_liftinstance.getArmAngle().gte(SAFE_REEF_ANGLE_BOTTOM)) {
417425
return STOW_L3_S1;
418426
}
419427
return this;
@@ -427,7 +435,7 @@ public void initialize() {
427435

428436
@Override
429437
public SystemState nextState() {
430-
if (s_liftinstance.getArmAngle() == SAFE_REEF_ANGLE_BOTTOM) {
438+
if (s_liftinstance.getArmAngle().gte(SAFE_REEF_ANGLE_BOTTOM)) {
431439
return STOW_L4_S1;
432440
}
433441
return this;
@@ -441,7 +449,7 @@ public void initialize() {
441449

442450
@Override
443451
public SystemState nextState() {
444-
if (s_liftinstance.getArmAngle() == SAFE_REEF_ANGLE_BOTTOM) {
452+
if (s_liftinstance.getArmAngle().gte(SAFE_REEF_ANGLE_BOTTOM)) {
445453
return STOW_L1_S1;
446454
}
447455
return this;
@@ -455,7 +463,7 @@ public void initialize() {
455463

456464
@Override
457465
public SystemState nextState() {
458-
if (s_liftinstance.getArmAngle() == SAFE_REEF_ANGLE_BOTTOM) {
466+
if (s_liftinstance.getArmAngle().gte(SAFE_REEF_ANGLE_BOTTOM)) {
459467
return STOW_L3_S1;
460468
}
461469
return this;
@@ -469,7 +477,7 @@ public void initialize() {
469477

470478
@Override
471479
public SystemState nextState() {
472-
if (s_liftinstance.getArmAngle() == SAFE_REEF_ANGLE_BOTTOM) {
480+
if (s_liftinstance.getArmAngle().gte(SAFE_REEF_ANGLE_BOTTOM)) {
473481
return STOW_L4_S1;
474482
}
475483
return this;
@@ -483,7 +491,7 @@ public void initialize() {
483491

484492
@Override
485493
public SystemState nextState() {
486-
if (s_liftinstance.getArmAngle() == SAFE_REEF_ANGLE_TOP) {
494+
if (s_liftinstance.getArmAngle().gte(SAFE_REEF_ANGLE_TOP)) {
487495
return L3_L1_S2;
488496
}
489497
return this;
@@ -497,7 +505,7 @@ public void initialize() {
497505

498506
@Override
499507
public SystemState nextState() {
500-
if (s_liftinstance.getElevatorHeight() == CLEAR_HEIGHT) {
508+
if (s_liftinstance.getElevatorHeight().gte(CLEAR_HEIGHT)) {
501509
return L3_L1_S3;
502510
}
503511
return this;
@@ -506,17 +514,31 @@ public SystemState nextState() {
506514
L3_L1_S3 {
507515
@Override
508516
public void initialize() {
509-
s_liftinstance.setElevatorHeight(CLEAR_HEIGHT);
517+
s_liftinstance.setArmAngle(SAFE_REEF_ANGLE_BOTTOM);
510518
}
511519

512520
@Override
513521
public SystemState nextState() {
514-
if (s_liftinstance.getElevatorHeight() == CLEAR_HEIGHT) {
515-
return L3_L1_S3;
522+
if (s_liftinstance.getArmAngle().gte(SAFE_REEF_ANGLE_BOTTOM)) {
523+
return L3_L1_S4;
524+
}
525+
return this;
526+
}
527+
},
528+
L3_L1_S4 {
529+
@Override
530+
public void initialize() {
531+
s_liftinstance.setElevatorHeight(L1_HEIGHT);
532+
}
533+
534+
@Override
535+
public SystemState nextState() {
536+
if (s_liftinstance.getElevatorHeight().lte(L1_HEIGHT)) {
537+
return L1;
516538
}
517539
return this;
518540
}
519-
};
541+
},
520542
}
521543

522544
private static LiftSubsystem s_liftinstance;
@@ -705,4 +727,24 @@ public void close() {
705727
m_pivotMotor.close();
706728
s_liftinstance = null;
707729
}
730+
731+
/**
732+
* Return whether the elevator is at a target height or not
733+
* @return Boolean of if elevator is at target height
734+
*/
735+
public boolean elevatorAt(Distance target_height) {
736+
737+
Distance cur_height = getElevatorHeight();
738+
return (cur_height.gte(target_height.minus(ELEVATOR_TOLERANCE)) && cur_height.lte(target_height.plus(ELEVATOR_TOLERANCE)));
739+
}
740+
741+
/**
742+
* Return whether the arm is at a target angle or not
743+
* @return Boolean of if arm is at target angle
744+
*/
745+
public boolean armAt(Angle target_angle) {
746+
747+
Angle cur_angle = getArmAngle();
748+
return (cur_angle.gte(target_angle.minus(ARM_TOLERANCE)) && cur_angle.lte(target_angle.plus(ARM_TOLERANCE)));
749+
}
708750
}

0 commit comments

Comments
 (0)