Test.jadd 2.67 KB
Newer Older
1
aspect GrammarTypes {
2
3
4
5
6
7
8
9
10
11
12
13
  public class TestCounter {
    public static TestCounter INSTANCE = new TestCounter();
    public int numberParseLinkState = 0;
    public int numberSerializeRobotConfig = 0;
    public int numberLinkStateToIntPosition = 0;
    public int numberCreateSpeedMessage = 0;
    public int numberInSafetyZone = 0;
    public static void reset() {
      TestCounter.INSTANCE = new TestCounter();
    }
  }

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
  public class IntPosition {
    private final int x, y, z;

    private IntPosition(int x, int y, int z) {
      this.x = x;
      this.y = y;
      this.z = z;
    }

    public static IntPosition of(int x, int y, int z) {
      return new IntPosition(x, y, z);
    }

    public int getX() {
      return x;
    }

    public int getY() {
      return y;
    }

    public int getZ() {
      return z;
    }

    @Override
    public boolean equals(Object o) {
      if (this == o) return true;
      if (o == null || getClass() != o.getClass()) return false;
      IntPosition that = (IntPosition) o;
      return x == that.x &&
          y == that.y &&
          z == that.z;
    }

    @Override
    public int hashCode() {
      return java.util.Objects.hash(x, y, z);
    }

    @Override
    public String toString() {
      return "(" + x + ", " + y + ", " + z + ")";
    }
  }
59
60
61
62
63
64
65
66
67

  inh Model RobotArm.model();
  eq Model.getRobotArm().model() = this;

  inh RobotArm Joint.containingRobotArm();
  eq RobotArm.getJoint().containingRobotArm() = this;
  eq RobotArm.getEndEffector().containingRobotArm() = this;

  syn boolean RobotArm.isInSafetyZone() {
68
    TestCounter.INSTANCE.numberInSafetyZone += 1;
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
    for (Joint joint : getJointList()) {
      if (model().getZoneModel().isInSafetyZone(joint.getCurrentPosition())) {
        return true;
      }
    }
    return model().getZoneModel().isInSafetyZone(getEndEffector().getCurrentPosition());
  }

  cache ZoneModel.isInSafetyZone(IntPosition pos);
  syn boolean ZoneModel.isInSafetyZone(IntPosition pos) {
    for (Zone sz : getSafetyZoneList()) {
      for (Coordinate coordinate : sz.getCoordinateList()) {
        IntPosition inside = coordinate.getPosition();
        if (inside.getX() <= pos.getX() &&
              inside.getY() <= pos.getY() &&
              inside.getZ() <= pos.getZ() &&
              pos.getX() <= inside.getX() + getSize().getX() &&
              pos.getY() <= inside.getY() + getSize().getY() &&
              pos.getZ() <= inside.getZ() + getSize().getZ()) {
          return true;
        }
      }
    }
    return false;
  }

95
96
97
  syn double RobotArm.speedLow() = 0.4d;
  syn double RobotArm.speedHigh() = 1.0d;

98
  syn double RobotArm.getAppropriateSpeed() {
99
    return isInSafetyZone() ? speedLow() : speedHigh();
100
  }
101
}