PlexilPlans/equidistantDosage.ple at master · ECI-Robotics/PlexilPlans · GitHub

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
Date Lookup time;

//Set front wheels angle [-30..30]
Command TurnFrontWheels(Integer);

//Set rear wheels angle [-30..30]
Command TurnRearWheels(Integer);

//Move forward with a given power level [0..100]
Command MoveForward(Integer);

//Move backwards with a given power level [0..100]
Command MoveBackward(Integer);

//apply one drop
Command OpenSprinkler();

//Stop
Command Stop();

Command pprint(...);

//*** Available Lookup Status Variables
//Heading
//StartRequested
//AbortRequested
//LeftSonarMeasuredDistance
//RightSonarMeasuredDistance
//CenterSonarMeasuredDistance


//EXAMPLE

ObstacleDodgeAndPesticideDosage:
{
  Integer steps=0;

  Concurrence{

    Duration dsdelay = Duration ("PT5S");
    Duration delay1 = Duration ("PT3S");
    Duration headFixDelay = Duration ("PT3S");

    Duration tolerance = Duration ("PT0.1S");
    Boolean timingComplete=false;
    Boolean obstacleAhead=false;
    Boolean headRecoveryRequired=false;

    StartMoving:{
        MoveForward(30);
    }

    Colission:{
        Start Lookup(CenterSonarMeasuredDistance)<200;
        Repeat true;
        obstacleAhead=true;
    }

    FrontalObstacleManouver:{
        Start obstacleAhead;
        Sequence{
            TurnFrontWheels(30);
            Wait (delay1), tolerance;
            TurnFrontWheels(0);
            Wait (delay1), tolerance;
            headRecoveryRequired=true;

        }
    }

    TimedDosage:{
        Start headRecoveryRequired;
        Repeat true;
        Wait (dsdelay), tolerance;
        OpenSprinkler();

    }

    HeadingLeftFix:{
        Start headRecoveryRequired && Lookup(Heading)<90  && Lookup(Heading)>=0;
        Repeat true;
        TurnFrontWheels(-15);
        Wait (headFixDelay), tolerance;
        TurnFrontWheels(0);
    }

    HeadingRightFix:{
        Start headRecoveryRequired &&  Lookup(Heading)>90 && Lookup(Heading)<180;
        Repeat true;
        TurnFrontWheels(15);
        Wait (headFixDelay), tolerance;
        TurnFrontWheels(0);
    }
  }
}