|
| 1 | +from matplotlib import pyplot as plt |
| 2 | + |
| 3 | +from fuzzylogic.classes import Domain, Rule |
| 4 | +from fuzzylogic.functions import R, S, trapezoid, triangular |
| 5 | + |
| 6 | +left = Domain("left_obstacle_sensor", 0, 100, res=0.1) |
| 7 | +right = Domain("right_obstacle_sensor", 0, 100, res=0.1) |
| 8 | +theta = Domain("θ", -50.5, 50.5, res=0.1) |
| 9 | + |
| 10 | +left.veryStrong = S(0, 25) |
| 11 | +left.strong = trapezoid(0, 25, 25, 50) |
| 12 | +left.medium = trapezoid(25, 50, 50, 75) |
| 13 | +left.weak = trapezoid(50, 75, 75, 100) |
| 14 | +left.veryWeak = R(75, 100) |
| 15 | + |
| 16 | +right.veryStrong = S(0, 25) |
| 17 | +right.strong = trapezoid(0, 25, 25, 50) |
| 18 | +right.medium = trapezoid(25, 50, 50, 75) |
| 19 | +right.weak = trapezoid(50, 75, 75, 100) |
| 20 | +right.veryWeak = R(75, 100) |
| 21 | + |
| 22 | +theta.mediumRight = triangular(-50.5, -49.5) |
| 23 | +theta.smallRight = triangular(-25.5, -24.5) |
| 24 | +theta.noTurn = triangular(-0.5, 0.5) |
| 25 | +theta.smallLeft = triangular(24.5, 25.5) |
| 26 | +theta.mediumLeft = triangular(49.5, 50.5) |
| 27 | + |
| 28 | +rules = Rule({ |
| 29 | + (left.veryStrong, right.veryStrong): theta.noTurn, |
| 30 | + (left.veryStrong, right.strong): theta.smallRight, |
| 31 | + (left.veryStrong, right.medium): theta.smallRight, |
| 32 | + (left.veryStrong, right.weak): theta.mediumRight, |
| 33 | + (left.veryStrong, right.veryWeak): theta.mediumRight, |
| 34 | + (left.strong, right.veryStrong): theta.smallLeft, |
| 35 | + (left.strong, right.strong): theta.noTurn, |
| 36 | + (left.strong, right.veryWeak): theta.mediumRight, |
| 37 | + (left.strong, right.weak): theta.mediumRight, |
| 38 | + (left.strong, right.medium): theta.smallRight, |
| 39 | + (left.medium, right.veryStrong): theta.smallLeft, |
| 40 | + (left.medium, right.strong): theta.smallLeft, |
| 41 | + (left.medium, right.medium): theta.noTurn, |
| 42 | + (left.medium, right.weak): theta.smallRight, |
| 43 | + (left.medium, right.veryWeak): theta.smallRight, |
| 44 | + (left.weak, right.veryStrong): theta.mediumLeft, |
| 45 | + (left.weak, right.strong): theta.mediumLeft, |
| 46 | + (left.weak, right.medium): theta.smallLeft, |
| 47 | + (left.weak, right.weak): theta.noTurn, |
| 48 | + (left.weak, right.veryWeak): theta.smallRight, |
| 49 | + (left.veryWeak, right.veryStrong): theta.mediumLeft, |
| 50 | + (left.veryWeak, right.strong): theta.mediumLeft, |
| 51 | + (left.veryWeak, right.medium): theta.smallLeft, |
| 52 | + (left.veryWeak, right.weak): theta.smallLeft, |
| 53 | + (left.veryWeak, right.veryWeak): theta.noTurn, |
| 54 | +}) |
| 55 | + |
| 56 | + |
| 57 | +def fuzzyObjectAvoidance(leftDist, rightDist): |
| 58 | + values = {left: leftDist, right: rightDist} |
| 59 | + return rules(values) |
| 60 | + |
| 61 | + |
| 62 | +def main(): |
| 63 | + theta.mediumRight.plot() |
| 64 | + theta.smallRight.plot() |
| 65 | + theta.noTurn.plot() |
| 66 | + theta.smallLeft.plot() |
| 67 | + theta.mediumLeft.plot() |
| 68 | + |
| 69 | + plt.show() |
| 70 | + |
| 71 | + print(fuzzyObjectAvoidance(100, 100)) |
| 72 | + |
| 73 | + |
| 74 | +if __name__ == "__main__": |
| 75 | + main() |
0 commit comments