forked from DexterInd/GoPiGo
-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathEncoderController.cs
More file actions
50 lines (42 loc) · 1.5 KB
/
EncoderController.cs
File metadata and controls
50 lines (42 loc) · 1.5 KB
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
namespace GoPiGo
{
public interface IEncoderController
{
IEncoderController SetEncoderTargetingOn(State motorOneState, State motorTwoStatem, int target);
int ReadEncoder(Motor motor);
IEncoderController EnableEncoders();
IEncoderController DisableEncoders();
}
public class EncoderController : IEncoderController
{
private readonly GoPiGo _goPiGo;
public EncoderController(GoPiGo goPiGo)
{
_goPiGo = goPiGo;
}
public IEncoderController SetEncoderTargetingOn(State motorOneState, State motorTwoState, int target)
{
var motorSelect = (int)motorOneState * 2 + (int)motorTwoState;
_goPiGo.RunCommand(Commands.SetEncoderTargeting, (byte)motorSelect, (byte)(target / 256), (byte)(target % 256));
return this;
}
public int ReadEncoder(Motor motor)
{
var buffer = new[] { (byte)Commands.ReadEncoder, (byte)motor, Constants.Unused, Constants.Unused };
_goPiGo.DirectAccess.Write(buffer);
_goPiGo.DirectAccess.Read(buffer);
int encoder = buffer[1] * 256 + buffer[2];
return encoder;
}
public IEncoderController EnableEncoders()
{
_goPiGo.RunCommand(Commands.EnableEncoder);
return this;
}
public IEncoderController DisableEncoders()
{
_goPiGo.RunCommand(Commands.DisableEncoder);
return this;
}
}
}