-
Notifications
You must be signed in to change notification settings - Fork 5
/
CRServoImpl.java
147 lines (125 loc) · 4.51 KB
/
CRServoImpl.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
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
package com.qualcomm.robotcore.hardware;
import com.qualcomm.robotcore.R;
import com.qualcomm.robotcore.util.Range;
import org.firstinspires.ftc.robotcore.internal.system.AppUtil;
/**
* ContinuousRotationServoImpl provides an implementation of continuous
* rotation servo functionality.
*/
public class CRServoImpl implements CRServo
{
//----------------------------------------------------------------------------------------------
// State
//----------------------------------------------------------------------------------------------
protected ServoController controller = null;
protected int portNumber = -1;
protected Direction direction = Direction.FORWARD;
protected static final double apiPowerMin = -1.0;
protected static final double apiPowerMax = 1.0;
protected static final double apiServoPositionMin = 0.0;
protected static final double apiServoPositionMax = 1.0;
//------------------------------------------------------------------------------------------------
// Construction
//------------------------------------------------------------------------------------------------
/**
* Constructor
*
* @param controller Servo controller that this servo is attached to
* @param portNumber physical port number on the servo controller
*/
public CRServoImpl(ServoController controller, int portNumber)
{
this(controller, portNumber, Direction.FORWARD);
}
/**
* Constructor
*
* @param controller Servo controller that this servo is attached to
* @param portNumber physical port number on the servo controller
* @param direction FORWARD for normal operation, REVERSE to reverse operation
*/
public CRServoImpl(ServoController controller, int portNumber, Direction direction)
{
this.direction = direction;
this.controller = controller;
this.portNumber = portNumber;
}
//----------------------------------------------------------------------------------------------
// HardwareDevice interface
//----------------------------------------------------------------------------------------------
@Override
public Manufacturer getManufacturer()
{
return controller.getManufacturer();
}
@Override
public String getDeviceName()
{
return AppUtil.getDefContext().getString(R.string.configTypeContinuousRotationServo);
}
@Override
public String getConnectionInfo()
{
return controller.getConnectionInfo() + "; port " + portNumber;
}
@Override
public int getVersion()
{
return 1;
}
@Override
public synchronized void resetDeviceConfigurationForOpMode()
{
this.direction = Direction.FORWARD;
}
@Override
public void close()
{
// take no action
}
//----------------------------------------------------------------------------------------------
// ContinuousRotationServo interface
//----------------------------------------------------------------------------------------------
@Override
public ServoController getController()
{
return this.controller;
}
@Override
public int getPortNumber()
{
return this.portNumber;
}
@Override
public synchronized void setDirection(Direction direction)
{
this.direction = direction;
}
@Override
public synchronized Direction getDirection()
{
return this.direction;
}
@Override
public void setPower(double power)
{
// For CR Servos on MR/HiTechnic hardware, internal positions relate to speed as follows:
//
// 0 == full speed reverse
// 128 == stopped
// 255 == full speed forward
//
if (this.direction == Direction.REVERSE) power = -power;
power = Range.clip(power, apiPowerMin, apiPowerMax);
power = Range.scale(power, apiPowerMin, apiPowerMax, apiServoPositionMin, apiServoPositionMax);
this.controller.setServoPosition(this.portNumber, power);
}
@Override
public double getPower()
{
double power = this.controller.getServoPosition(this.portNumber);
power = Range.scale(power, apiServoPositionMin, apiServoPositionMax, apiPowerMin, apiPowerMax);
if (this.direction == Direction.REVERSE) power = -power;
return power;
}
}