-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathwall_follower.py
151 lines (119 loc) · 4.07 KB
/
wall_follower.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
# Import libraries
from controller import Robot, Motor, DistanceSensor, Lidar
import csv
#from functionsLibrary import *
# Literals
TIME_STEP = 16
MAX_SPEED = 10
BASE_SPEED = 5
SAFETY_DISTANCE = 0.40
# Robot instance
robot = Robot()
# Get components of robot
leftMotor = robot.getDevice('left wheel motor')
rightMotor = robot.getDevice('right wheel motor')
lidar = robot.getDevice("LDS-01")
lidarMainMotor = robot.getDevice("LDS-01_main_motor")
lidarSecondaryMotor = robot.getDevice("LDS-01_secondary_motor")
# * PROGRAM FUNCTIONS *
def moveForward():
leftMotor.setPosition(float('inf'))
rightMotor.setPosition(float('inf'))
leftMotor.setVelocity(BASE_SPEED)
rightMotor.setVelocity(BASE_SPEED)
print("Moving forward")
return(BASE_SPEED, BASE_SPEED)
def turnLeft():
leftMotor.setPosition(float('inf'))
rightMotor.setPosition(float('inf'))
leftMotor.setVelocity(BASE_SPEED/9)
rightMotor.setVelocity(BASE_SPEED)
print("Turning left")
return(BASE_SPEED/9, BASE_SPEED)
def turnRight():
leftMotor.setPosition(float('inf'))
rightMotor.setPosition(float('inf'))
leftMotor.setVelocity(BASE_SPEED)
rightMotor.setVelocity(BASE_SPEED/9)
print("Turning right")
return(BASE_SPEED, BASE_SPEED/9)
def rotateLeft():
leftMotor.setPosition(float('inf'))
rightMotor.setPosition(float('inf'))
leftMotor.setVelocity(-BASE_SPEED)
rightMotor.setVelocity(BASE_SPEED)
print("Rotating left")
return(-BASE_SPEED, BASE_SPEED)
def rotateRight():
leftMotor.setPosition(float('inf'))
rightMotor.setPosition(float('inf'))
leftMotor.setVelocity(BASE_SPEED)
rightMotor.setVelocity(-BASE_SPEED)
print("Rotating right")
return(BASE_SPEED, -BASE_SPEED)
def moveBackward():
leftMotor.setPosition(float('inf'))
rightMotor.setPosition(float('inf'))
leftMotor.setVelocity(-BASE_SPEED)
rightMotor.setVelocity(-BASE_SPEED)
print("Moving backward")
return(-BASE_SPEED, -BASE_SPEED)
def stop():
leftMotor.setPosition(0) # Set position es absoluto o relativo?
rightMotor.setPosition(0)
leftMotor.setVelocity(0)
rightMotor.setVelocity(0)
print("Stopping")
return (0, 0)
def leftWallFollow(rangeVec):
# Detection of walls
front_wall = rangeVec[180] < 0.30
left_wall = rangeVec[90] < 0.30
left_corner = rangeVec[135] < 0.30
print("Front wall: ", front_wall)
print("Left corner: ", left_corner)
print("Left wall: ", left_wall)
# Reactive action
if front_wall:
action = rotateRight()
else:
if left_wall:
action = moveForward()
else:
action = turnLeft()
if left_corner:
action = rotateRight()
return(action)
# ++++++>>>>>> PROGRAM <<<<<<++++++
# LIDAR
# Move lidar motors (just visualization)
lidarMainMotor.setPosition(float('inf'))
lidarSecondaryMotor.setPosition(float('inf'))
lidarMainMotor.setVelocity(40.0)
lidarSecondaryMotor.setVelocity(60.0)
#Enable lidar
lidar.enable(TIME_STEP)
lidar.enablePointCloud()
#Variables
lidarWidth = lidar.getHorizontalResolution()
lidarMaxRange = lidar.getMaxRange()
loopCounter = 0
# CSV opening in write mode
with open('data.csv', 'w', newline='') as csvfile:
# Crea un objeto csv.writer
csv_writer = csv.writer(csvfile)
# * PROGRAM LOOP *
while robot.step(TIME_STEP) != -1:
lidarRangeImage = lidar.getRangeImage()
setLidarRangeImage = set(lidarRangeImage) # Take elements that are not repeated
if len(setLidarRangeImage) > 1: # Could avoid lidar non-detection fail
action = leftWallFollow(lidarRangeImage)
#♠ Save values to CSV
frontMeasure = lidarRangeImage[165]
leftMeasure = lidarRangeImage[90]
cornerMeasure = lidarRangeImage[135]
# Write CSV
csv_writer.writerow([frontMeasure, leftMeasure, cornerMeasure, action[0], action[1]])
# Update loop counter
print("Loop count: ", loopCounter)
loopCounter = loopCounter + 1