-
Notifications
You must be signed in to change notification settings - Fork 5
/
callibration.py
176 lines (154 loc) · 5.49 KB
/
callibration.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
from ultrasonic import callibration_ultra
from time import sleep
import bot_movement as bm
from bot_globals import bot
from math import asin, degrees, sqrt, pow
def linear_callibrate(reading, distance):
# center is actually extra spacing on the sides
center = 3
error = 2
distance = distance + center
if reading > distance + error:
bm.move_forward(reading - distance)
elif reading < distance - error:
bm.move_backward(distance - reading)
else:
print "no need for ultrasonic callibration, good work encoders"
def angle_callibrate(reading_left, reading_back, distance):
ultra_diff = 17.45
#center more than linear to incorporate the tires and ultra position
center = 5
distance_error = 4
angle_error = 1
distance = distance + center
average_reading = (reading_left + reading_back)/2
difference = reading_back - reading_left
degree = asin(difference / sqrt(pow(ultra_diff, 2)+pow(difference, 2)))
degree = degrees(degree)
#correcting the distance from left
if average_reading < distance - distance_error:
# can add the below commented if angle correction wanted before
"""if reading_left > reading_back + angle_error:
bm.turn_left(degree)
elif reading_left < reading_back - angle_error:
bm.turn_right(degree)"""
bm.turn_right(90)
bm.move_forward(distance - average_reading)
bm.turn_left(90)
elif average_reading > distance + distance_error:
# can add the below commented if angle correction wanted before
"""if reading_left > reading_back + angle_error:
bm.turn_left(degree)
elif reading_left < reading_back - angle_error:
bm.turn_right(degree)"""
bm.turn_left(90)
bm.move_forward(average_reading - distance)
bm.turn_right(90)
#now correcting the angle
if reading_left > reading_back + angle_error:
bm.turn_left(degree)
elif reading_left < reading_back - angle_error:
bm.turn_right(degree)
def callibrate(rows, columns, cx, cy, mapp):
# [0] = move_forward, [1] = left, [2] = back
readings = callibration_ultra()
#for linear callibration
if bot.direction == 'n':
if cx == 0:
distance = 0
else:
found_obstacle = False
for i in range(1, cx):
if mapp[i][cy] == 1:
found_obstacle = True
distance = (cx - i - 1) * 25
break
if found_obstacle==False:
distance = cx * 25
elif bot.direction == 's':
if cx == rows:
distance = 0
else:
found_obstacle = False
for i in range(cx, rows):
if mapp[i][cy] == 1:
found_obstacle = True
distance = (i - cx - 1) * 25
break
if found_obstacle==False:
distance = (rows - cx - 1) * 25
elif bot.direction == 'w':
if cy == 0:
distance = 0
else:
found_obstacle = False
for i in range(1, cy):
if mapp[cx][i] == 1:
found_obstacle = True
distance = (cy - i - 1) * 25
break
if found_obstacle==False:
distance = cy * 25
elif bot.direction == 'e':
if cy == columns:
distance = 0
else:
found_obstacle = False
for i in range(cy, columns):
if mapp[cx][i] == 1:
found_obstacle = True
distance = (i - cy - 1) * 25
break
if found_obstacle==False:
distance = (columns - cy - 1) * 25
linear_callibrate(readings[0], distance)
# for angle callibration
if bot.direction == 'n':
if cy == 0:
distance = 0
else:
found_obstacle = False
for i in range(1, cy):
if mapp[cx][i] == 1:
found_obstacle = True
distance = (cy - i - 1) * 25
break
if found_obstacle==False:
distance = cy * 25
elif bot.direction == 's':
if cy == columns:
distance = 0
else:
found_obstacle = False
for i in range(cy, columns):
if mapp[cx][i] == 1:
found_obstacle = True
distance = (i - cy - 1) * 25
break
if found_obstacle==False:
distance = (columns - cy - 1) * 25
elif bot.direction == 'w':
if cx == rows:
distance = 0
else:
found_obstacle = False
for i in range(cx, rows):
if mapp[i][cy] == 1:
found_obstacle = True
distance = (i - cx - 1) * 25
break
if found_obstacle==False:
distance = (rows - cx - 1) * 25
elif bot.direction == 'e':
if cx == 0:
distance = 0
else:
found_obstacle = False
for i in range(1, cx):
if mapp[i][cy] == 1:
found_obstacle = True
distance = (cx - i - 1) * 25
break
if found_obstacle==False:
distance = cx * 25
angle_callibrate(readings[1], readings[2], distance)