-
Notifications
You must be signed in to change notification settings - Fork 78
/
SimpleServo.java
107 lines (86 loc) · 2.55 KB
/
SimpleServo.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
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
package com.arcrobotics.ftclib.hardware;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;
public class SimpleServo implements ServoEx {
Servo servo;
double maxAngle, minAngle;
final double maxPosition = 1;
final double minPosition = 0;
public SimpleServo(HardwareMap hw, String servoName) {
servo = hw.get(Servo.class, servoName);
maxAngle = 180;
minAngle = 0;
}
public SimpleServo(HardwareMap hw, String servoName, double maxAngle, double minAngle) {
servo = hw.get(Servo.class, servoName);
this.maxAngle = maxAngle;
this.minAngle = minAngle;
}
@Override
public void rotateDegrees(double angle) {
angle = getAngle() + angle;
turnToAngle(angle);
}
@Override
public void turnToAngle(double angle) {
if(angle > maxAngle)
angle = maxAngle;
else if(angle < minAngle)
angle = minAngle;
setPosition((angle - minAngle) / (getAngleRange()));
}
@Override
public void rotate(double position) {
position = getPosition() + position;
setPosition(position);
}
@Override
public void setPosition(double position) {
if(position > maxPosition)
servo.setPosition(maxPosition);
else if(position < minAngle)
servo.setPosition(minPosition);
else
servo.setPosition(position);
}
@Override
public void setRange(double min, double max) {
this.minAngle = min;
this.maxAngle = max;
}
@Override
public void setInverted(boolean isInverted) {
if(isInverted)
servo.setDirection(Servo.Direction.REVERSE);
else
servo.setDirection(Servo.Direction.FORWARD);
}
@Override
public boolean getInverted() {
if(Servo.Direction.REVERSE == servo.getDirection())
return true;
else
return false;
}
@Override
public double getPosition() {
return servo.getPosition();
}
@Override
public double getAngle() {
return getPosition() * getAngleRange() + minAngle;
}
public double getAngleRange() {
return maxAngle - minAngle;
}
@Override
public void disable() {
servo.close();
}
@Override
public String getDeviceType() {
String port = Integer.toString(servo.getPortNumber());
String controller = servo.getController().toString();
return "SimpleServo: " + port + "; " + controller;
}
}