/
RangeCalibrationRoutine.cs
73 lines (54 loc) · 2.1 KB
/
RangeCalibrationRoutine.cs
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
using Lego.Core.Extensions;
using Lego.Core.Models.Devices.General;
using Lego.Core.Models.Messaging;
using System;
using System.Threading.Tasks;
namespace Lego.Core
{
public class RangeCalibrationRoutine : BaseRoutine<Motor>
{
public override Func<Motor, bool> StartCondition => (device) => true; // start immediately
public override Func<Motor, bool> StopCondition => (device) => IsCalibrated; // wait until calibration has completed
protected bool IsCalibrated { get; set; } = false;
protected byte Power { get; set; }
public RangeCalibrationRoutine(byte power)
{
Power = power;
}
public async override void Routine(Motor device)
{
//device.SetSpeedForDuration(100, Power, RotateDirection.Clockwise, 500);
//await Task.Delay(750);
//device.SetSpeedForDuration(100, Power, RotateDirection.CounterClockwise, 250);
//await Task.Delay(750);
//await device.SetCombinedInputMode(0);
device.SetSingleInputMode(Motor.INPUT_MODE__SPEED);
await Task.Delay(1000);
device.GotoAbsolutePositionMin(100, Power);
do
{
await Task.Delay(1000);
}
while (device.Speed > 0);
device.SetSingleInputMode(Motor.INPUT_MODE__POSITION);
await Task.Delay(1000);
int? minPosition = device.Position;
device.SetSingleInputMode(Motor.INPUT_MODE__SPEED);
await Task.Delay(1000);
device.GotoAbsolutePositionMax(100, Power);
do
{
await Task.Delay(1000);
}
while (device.Speed > 0);
device.SetSingleInputMode(Motor.INPUT_MODE__POSITION);
await Task.Delay(1000);
int? maxPosition = device.Position;
device.MinPosition = minPosition.Value;
device.MaxPosition = maxPosition.Value;
device.GotoAbsolutePositionMid(100, 100);
await Task.Delay(2000);
IsCalibrated = true;
}
}
}