11
11
12
12
import org .firstinspires .ftc .teamcode .Hardwares .namespace .HardwareDevices ;
13
13
import org .firstinspires .ftc .teamcode .utils .Enums .HardwareState ;
14
+ import org .firstinspires .ftc .teamcode .utils .Exceptions .DeviceDisabledException ;
14
15
15
16
import java .util .HashMap ;
16
17
import java .util .Map ;
@@ -37,15 +38,19 @@ public DeviceMap(HardwareMap hardwareMap){
37
38
}
38
39
39
40
public HardwareDevice getDevice (@ NonNull HardwareDevices hardwareDevices ){
40
- if (hardwareDevices .state ==HardwareState .Disabled )throw new RuntimeException ("Hardware " +hardwareDevices .name ()+" Disabled." );
41
+ if (hardwareDevices .state ==HardwareState .Disabled ) {
42
+ throw new DeviceDisabledException (hardwareDevices .name ());
43
+ }
41
44
if (devices .containsKey (hardwareDevices )){
42
45
return devices .get (hardwareDevices );
43
46
}else {
44
47
throw new NullPointerException ("Device Not Found:" + hardwareDevices .toString ());
45
48
}
46
49
}
47
50
public void setDirection (@ NonNull HardwareDevices hardwareDevices , DcMotorSimple .Direction direction ){
48
- if (hardwareDevices .state ==HardwareState .Disabled )throw new RuntimeException ("Hardware " +hardwareDevices .name ()+" Disabled." );
51
+ if (hardwareDevices .state ==HardwareState .Disabled ) {
52
+ throw new DeviceDisabledException (hardwareDevices .name ());
53
+ }
49
54
HardwareDevice device =getDevice (hardwareDevices );
50
55
if (device instanceof DcMotorEx ){
51
56
((DcMotorEx ) device ).setDirection (direction );
@@ -54,7 +59,9 @@ public void setDirection(@NonNull HardwareDevices hardwareDevices, DcMotorSimple
54
59
}
55
60
}
56
61
public void setPower (@ NonNull HardwareDevices hardwareDevices , double power ){
57
- if (hardwareDevices .state ==HardwareState .Disabled )throw new RuntimeException ("Hardware " +hardwareDevices .name ()+" Disabled." );
62
+ if (hardwareDevices .state ==HardwareState .Disabled ) {
63
+ throw new DeviceDisabledException (hardwareDevices .name ());
64
+ }
58
65
HardwareDevice device =getDevice (hardwareDevices );
59
66
if (device instanceof DcMotorEx ){
60
67
((DcMotorEx ) device ).setPower (power );
@@ -65,7 +72,9 @@ public void setPower(@NonNull HardwareDevices hardwareDevices, double power){
65
72
}
66
73
}
67
74
public void setPosition (@ NonNull HardwareDevices hardwareDevices , double position ){
68
- if (hardwareDevices .state ==HardwareState .Disabled )throw new RuntimeException ("Hardware " +hardwareDevices .name ()+" Disabled." );
75
+ if (hardwareDevices .state ==HardwareState .Disabled ) {
76
+ throw new DeviceDisabledException (hardwareDevices .name ());
77
+ }
69
78
HardwareDevice device =getDevice (hardwareDevices );
70
79
if (device instanceof Servo ){
71
80
((Servo ) device ).setPosition (position );
@@ -74,7 +83,9 @@ public void setPosition(@NonNull HardwareDevices hardwareDevices, double positio
74
83
}
75
84
}
76
85
public double getPosition (@ NonNull HardwareDevices hardwareDevices ){
77
- if (hardwareDevices .state ==HardwareState .Disabled )throw new RuntimeException ("Hardware " +hardwareDevices .name ()+" Disabled." );
86
+ if (hardwareDevices .state ==HardwareState .Disabled ) {
87
+ throw new DeviceDisabledException (hardwareDevices .name ());
88
+ }
78
89
HardwareDevice device =getDevice (hardwareDevices );
79
90
if (device instanceof Servo ) {
80
91
return ((Servo ) device ).getPosition ();
0 commit comments