forked from margenta19/bsmengineering2repository
/
wallfollowing.py
72 lines (59 loc) · 1.39 KB
/
wallfollowing.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
import RoboPiLib as RoboPi
import time as time
import motors as motors
#INPUT = RoboPi.INPUT
#FRONT_BUMPER = 20
LEFT_GO = 0
#RoboPi.pinMode(FRONT_BUMPER, RoboPi.INPUT)
#RoboPi.pinMode(LEFT_GO, RoboPi.INPUT)
#RoboPi.pinMode(RIGHT_GO, RoboPi.INPUT)
def forward():
motors.go_forward()
print "Forward!"
def clockwise():
motors.spin_clockwise()
print "Clockwise!"
def counterclockwise():
motors.spin_counterclockwise()
print "Counter Clockwise"
def stop():
motors.stop()
print "Stop"
def wall_left():
motors.wall_left()
print "wall_left"
def wall_right():
motors.wall_right()
print "wall_right"
##Analog
while 1:
left_sensor = RoboPi.analogRead(LEFT_GO)
L_VOLTS = 1000 / (RoboPi.analogRead(LEFT_GO) + 0.0000000001)
L_CM = RoboPi.analogRead(LEFT_GO)
go_forward = False
go_clockwise = False
go_counterclockwise = False
go_stop = False
go_wall_left = False
go_wall_right = False
print RoboPi.analogRead(LEFT_GO)
if L_CM > 400:
go_wall_left = True
elif L_CM > 200:
go_forward = True
else:
go_wall_right = True
## DECISION
if go_forward:
forward()
elif go_clockwise:
clockwise()
elif go_counterclockwise:
counterclockwise()
elif go_stop:
stop()
elif go_wall_left:
wall_left()
elif go_wall_right:
wall_right()
RoboPi.RoboPiExit()