-
Notifications
You must be signed in to change notification settings - Fork 1
/
main.py
185 lines (150 loc) · 5.91 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
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
'''
Autonomous robot main driver to avoid obstacles and navigate through its surroundings.
Python code used to run an autonomous robot built using Arduino, Raspberry Pi, servos, motors,
ultrasonic sensors, LIDAR sensors, XBee, and blockchain technology. The robot initially turns
the LIDAR to the left, right, and center to determine the longest direction through multiple
readings, removes outliers, and returns a median value. It then moves the motors towards the
direction of the farthest distance. The robot takes readings while in motion for collision
checking using the ultrasonic sensors. If it encounters an obstacle, it stops and
calibrates again to determine the next longest direction and then moves towards that direction.
'''
import motorControl, servoControl, ultrasonicControl, auxiliary, LIDARcontrol
import time
#-------------------------------------------------------------------------------
# Setting direction (finding the longest way without obstacles)
# Duration for motors to operate
_MOTOR_DELAY = 0.05
# Duration for servo turning speed
_SERVO_DELAY = 0.25
# Duration for right and left motor turn amount
_MOTOR_TURN_DELAY = 0.15
# Ultrasonic sensitivity for collision checking. Increase for greater collision avoidance
_SENSITIVITY = 20.0
# Ultrasonic max sensor reading. Any values over this reading are invalid
_ULTRASONIC_MAX = 500.0
#-------------------------------------------------------------------------------
# Poll ultrasonic sensor and ensure readings are valid
def ultrasonic_reading():
ultrasonic_distance = ultrasonic.distance(1)
while ultrasonic_distance > _ULTRASONIC_MAX:
ultrasonic_distance = ultrasonic.distance(1)
print("ultrasonic_distance is: ", ultrasonic_distance)
return ultrasonic_distance
# Turn LIDAR sensor to the left, right, and center to determine the longest direction through multiple
# readings. Then uses the motor to shift the robot towards the longest direction.
def findDirection():
distanceArray = [0,0,0]
global PAST_DIRECTION
# Previous was right, move left
if PAST_DIRECTION == RIGHT:
# Pan left
servo.panLeft()
time.sleep(_SERVO_DELAY)
distanceArray[0] = lidar.distance()
print("left distance: ", distanceArray[0])
# aux.writetofile('Pan Left Distace', distanceArray[0])
# Pan center
servo.panCenter()
time.sleep(_SERVO_DELAY)
distanceArray[1] = lidar.distance()
print("center distance: ", distanceArray[1])
# aux.writetofile('Pan Center Distace', distanceArray[1])
# Pan right
servo.panRight()
time.sleep(_SERVO_DELAY)
distanceArray[2] = lidar.distance()
print("right distance: ", distanceArray[2])
# aux.writetofile('Pan Right Distace', distanceArray[2])
PAST_DIRECTION = LEFT
servo.panCenter()
# Previous was left, move right
else:
# Pan right
servo.panRight()
time.sleep(_SERVO_DELAY)
distanceArray[2] = lidar.distance()
print("right distance: ", distanceArray[2])
# aux.writetofile('Pan Right Distace', distanceArray[2])
# Pan center
servo.panCenter()
time.sleep(_SERVO_DELAY)
distanceArray[1] = lidar.distance()
print("center distance: ", distanceArray[1])
# aux.writetofile('Pan Center Distace', distanceArray[1])
# Pan left
servo.panLeft()
time.sleep(_SERVO_DELAY)
distanceArray[0] = lidar.distance()
print("left distance: ", distanceArray[0])
# aux.writetofile('Pan Left Distace', distanceArray[0])
PAST_DIRECTION = RIGHT
servo.panCenter()
maxdistance = max(distanceArray)
maxindex = distanceArray.index(maxdistance)
'''
maxindex 0 left
maxindex 1 center
maxindex 2 right
'''
if maxindex == 0:
motor.left()
time.sleep(_MOTOR_TURN_DELAY)
motor.stop()
aux.writetofile('Turning Left', distanceArray[maxindex])
elif maxindex == 2:
motor.right()
time.sleep(_MOTOR_TURN_DELAY)
motor.stop()
aux.writetofile('Turning Right', distanceArray[maxindex])
else:
aux.writetofile('Not Turning', distanceArray[maxindex])
print(distanceArray)
del distanceArray[:]
#-------------------------------------------------------------------------------
# Move the robot forward or backward by using the ultrasonic sensor to determine collision checking.
def move():
# Move robot forward
ultrasonic_distance = ultrasonic_reading()
while ultrasonic_distance >= _SENSITIVITY and ultrasonic_distance <= _ULTRASONIC_MAX:
motor.forward()
time.sleep(_MOTOR_DELAY)
print("moving forward")
ultrasonic_distance = ultrasonic_reading()
print("ultrasonic_distance forward is: ", ultrasonic_distance)
motor.stop()
time.sleep(_MOTOR_DELAY)
# Move robot backwards
ultrasonic_distance = ultrasonic_reading()
while ultrasonic_distance < _SENSITIVITY:
motor.backward()
time.sleep(_MOTOR_DELAY)
print("moving backward")
ultrasonic_distance = ultrasonic_reading()
print("ultrasonic_distance backward is: ", ultrasonic_distance)
motor.stop()
time.sleep(_MOTOR_DELAY)
print("resetting position")
#-------------------------------------------------------------------------------
# Instantiate objects
motor = motorControl.MotorControl()
servo = servoControl.ServoControl()
ultrasonic = ultrasonicControl.UltrasonicControl()
lidar = LIDARcontrol.LIDARControl()
aux = auxiliary.AuxiliaryHelp()
LEFT = 0
CENTER = 1
RIGHT = 2
# Initially search left
global PAST_DIRECTION
PAST_DIRECTION = 0
# Center servos
servo.resetServo()
# Start logging data
aux.writetofile('Servos are dead center', 0)
try:
while True:
findDirection()
move()
except KeyboardInterrupt:
servo.resetServo()
aux.cleanup()