-
Notifications
You must be signed in to change notification settings - Fork 12
/
TimedLEDState.java
79 lines (65 loc) · 3.39 KB
/
TimedLEDState.java
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
package com.team254.frc2022.states;
public interface TimedLEDState {
void getCurrentLEDState(LEDState desiredState, double timestamp);
class BlinkingLEDState implements TimedLEDState {
public static BlinkingLEDState kBlinkingTurretZeroingFault = new BlinkingLEDState(
LEDState.kOff, LEDState.kTurretZeroingFault, 1.0);
public static BlinkingLEDState kBlinkingHoodZeroingFault = new BlinkingLEDState(
LEDState.kOff, LEDState.kHoodZeroingFault, 1.0);
public static BlinkingLEDState kClimbing = new BlinkingLEDState(
LEDState.kOff, LEDState.kClimbing, 0.5);
public static BlinkingLEDState kBlinkingShooting = new BlinkingLEDState(
LEDState.kOff, LEDState.kShooting, 0.1);
public static BlinkingLEDState kBlinkingSpinningUp = new BlinkingLEDState(LEDState.kOff, LEDState.kSpinningUp, 0.1);
public static BlinkingLEDState kBlinkingEject = new BlinkingLEDState(LEDState.kEjecting, LEDState.kOff, 0.5);
public static BlinkingLEDState kBlinkingTwoBalls = new BlinkingLEDState(LEDState.kHasTwoBalls, LEDState.kOff, 0.09, 0.03);
LEDState mStateOne = new LEDState(0, 0, 0);
LEDState mStateTwo = new LEDState(0, 0, 0);
private boolean mAsymmetricDuration = false;
private double mDuration;
private double mDurationTwo;
public BlinkingLEDState(LEDState stateOne, LEDState stateTwo, double duration) {
mStateOne.copyFrom(stateOne);
mStateTwo.copyFrom(stateTwo);
mDuration = duration;
}
public BlinkingLEDState(LEDState stateOne, LEDState stateTwo, double durationOne, double durationTwo) {
mStateOne.copyFrom(stateOne);
mStateTwo.copyFrom(stateTwo);
mDuration = durationOne;
mDurationTwo = durationTwo;
mAsymmetricDuration = true;
}
@Override
public void getCurrentLEDState(LEDState desiredState, double timestamp) {
if(mAsymmetricDuration) {
if (timestamp % (mDuration + mDurationTwo) > mDuration) {
desiredState.copyFrom(mStateTwo);
} else {
desiredState.copyFrom(mStateOne);
}
} else {
if ((int) (timestamp / mDuration) % 2 == 0) {
desiredState.copyFrom(mStateOne);
} else {
desiredState.copyFrom(mStateTwo);
}
}
}
}
class StaticLEDState implements TimedLEDState {
public static StaticLEDState kStaticOff = new StaticLEDState(LEDState.kOff);
public static StaticLEDState kStaticRobotZeroedWithGoodBattery = new StaticLEDState(LEDState.kRobotZeroedWithGoodBattery);
public static StaticLEDState kStaticOneBallInRobot = new StaticLEDState(LEDState.kHasOneBall);
public static StaticLEDState kStaticTwoBallInRobot = new StaticLEDState(LEDState.kHasTwoBalls);
public static StaticLEDState kStaticBatteryLow = new StaticLEDState(LEDState.kBatteryLow);
LEDState mStaticState = new LEDState(0, 0, 0);
public StaticLEDState(LEDState staticState) {
mStaticState.copyFrom(staticState);
}
@Override
public void getCurrentLEDState(LEDState desiredState, double timestamp) {
desiredState.copyFrom(mStaticState);
}
}
}