-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathmain.py
128 lines (109 loc) · 3.52 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
# -*- coding: utf-8 -*-
"""
Created on Mon Jul 13 12:20:59 2020
@author: Nathan Zhang
@author: Jackie Piepkorn
"""
from motors import Motor
from sensors import UltrasonicRangeFinder
import time
leftSensor = UltrasonicRangeFinder("PartNumber", 'LEFT', 20, 21)
rightSensor = UltrasonicRangeFinder("PartNumber", 'RIGHT', 12, 16)
frontSensor = UltrasonicRangeFinder("PartNumber", 'FRONT', 19, 26)
leftMotor = Motor(18, 23, 3.5)
rightMotor = Motor(4, 17, 3.5)
def main():
#testStartup()
stopAllMotors()
while True:
leftBlocked = leftSensor.isBlocked()
rightBlocked = rightSensor.isBlocked()
frontBlocked = frontSensor.isFrontBlocked()
step(leftBlocked, rightBlocked, frontBlocked)
def testStartup():
print("Starting left motor forward")
leftMotor.startPin2()
time.sleep(1)
print("Stopping left motor.")
leftMotor.stop()
print("Starting left motor backward")
leftMotor.startPin1()
time.sleep(1)
print("Stopping left motor.")
leftMotor.stop()
print("Starting right motor forward")
rightMotor.startPin1()
time.sleep(1)
print("Stopping right motor.")
rightMotor.stop()
print("Starting right motor backward")
rightMotor.startPin2()
time.sleep(1)
print("Stopping right motor.")
rightMotor.stop()
print("Left Sensor Distance: " + str(leftSensor.getDistance()))
print("Right Sensor Distance: " + str(rightSensor.getDistance()))
print("Front Sensor Distance: " + str(frontSensor.getDistance()))
time.sleep(1)
print("All systems checked...proceeding with algorithm...")
def step(leftBlocked, rightBlocked, frontBlocked):
if leftBlocked:
print('Left too close, adjusting Right. Left distance: ' + str(leftSensor.getDistance()))
adjustRight()
elif rightBlocked:
print('Right too close, adjusting Left. Right distance: ' + str(rightSensor.getDistance()))
adjustLeft()
time.sleep(0.25)
if not frontBlocked:
print("Front clear, moving forward. Front Distance: " + str(frontSensor.getDistance()))
moveForward()
else:
print("Front Blocked, sensing left and right sides")
stopAllMotors()
time.sleep(0.2)
tempRight = rightSensor.getDistance()
tempLeft = leftSensor.getDistance()
if tempRight >= tempLeft:
print("Turning Right")
turnRight()
elif tempLeft > tempRight:
print("Turning Left")
turnLeft()
time.sleep(0.25)
def adjustLeft():
leftMotor.startPin2()
time.sleep(0.05)
stopAllMotors()
def turnLeft():
leftMotor.startPin2()
time.sleep(0.37)
stopAllMotors()
def adjustRight():
rightMotor.startPin1()
time.sleep(0.05)
stopAllMotors()
def turnRight():
rightMotor.startPin1()
time.sleep(0.37)
stopAllMotors()
def moveForward():
leftMotor.startPin2()
rightMotor.startPin1()
time.sleep(0.1)
stopAllMotors()
time.sleep(0.25)
def moveBackward():
leftMotor.startPin1()
rightMotor.startPin2()
time.sleep(0.1)
stopAllMotors()
time.sleep(0.25)
def stopAllMotors():
leftMotor.stop()
rightMotor.stop()
if __name__ == '__main__':
try:
main()
except KeyboardInterrupt:
stopAllMotors()
print("\n--Process terminated by User--")