-
Notifications
You must be signed in to change notification settings - Fork 0
/
main.py
117 lines (93 loc) · 2.45 KB
/
main.py
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
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
import xbox
import sys
import time
import math
import RPi.GPIO as GPIO
import ultrasonic
import SimpleMFRC522
def initial():
GPIO.setmode(GPIO.BCM);
GPIO.setup(17, GPIO.OUT);
GPIO.setup(22, GPIO.OUT);
GPIO.setup(23, GPIO.OUT);
GPIO.setup(24, GPIO.OUT);
def move_wheel(IN1, IN2, IN3, IN4):
GPIO.output(17, IN1);
GPIO.output(22, IN2);
GPIO.output(23, IN3);
GPIO.output(24, IN4);
def joypad(x,y):
angle = 0.0
if x >= 0.0 and y > 0.0:
# first quadrant
angle = math.degrees(math.atan(y/x)) if x!=0.0 else 90.0
elif x < 0.0 and y >= 0.0:
# second quadrant
angle = math.degrees(math.atan(y/x))
angle += 180.0
elif x < 0.0 and y < 0.0:
# third quadrant
angle = math.degrees(math.atan(y/x))
angle += 180.0
elif x >= 0.0 and y < 0.0:
# third quadrant
angle = math.degrees(math.atan(y/x)) if x!=0.0 else -90.0
angle += 360.0
return angle
reader = SimpleMFRC522.SimpleMFRC522() #object
id = reader.read()
print("Scan your RFID Card...")
while not id:
id = reader.read()
if id == 115173304638:
print(" ")
print("Welcome Bibash Shrestha")
print(" ")
joy = xbox.Joystick()
while not joy.Back():
if joy.connected():
print ("Connected "),
else:
print("Disconnected"),
angle = joypad(joy.leftX(), joy.leftY())
if (60 <= angle < 120):
distance = ultrasonic.dist()
if distance < 20:
pass
print("Object Ahead"),
print("Distance: "+ str(distance)),
else:
initial()
move_wheel(1,0,1,0) #forward
time.sleep(0.1)
elif (120 <= angle < 180):
initial()
move_wheel(0,0,1,0) #forward_left
time.sleep(0.001)
elif (0 < angle < 60):
initial()
move_wheel(1,0,0,0) #forward_right
time.sleep(0.001)
elif (240 <= angle < 300):
initial()
move_wheel(0,1,0,1) #backward
time.sleep(0.001)
elif (300 <= angle < 360):
initial()
move_wheel(0,1,0,0) #backward_left
time.sleep(0.001)
elif (180 < angle < 240):
initial()
move_wheel(0,0,0,1) #backward_right
time.sleep(0.001)
else:
print(" "),
print(" "),
initial()
move_wheel(0,0,0,0)
GPIO.cleanup()
# Move cursor back to start of line
print chr(13),
joy.close()
else:
print("Acess Denied")