/
nabiralec1.py
746 lines (648 loc) · 25.4 KB
/
nabiralec1.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
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
#!/usr/bin/env python3
"""
Program za vodenje robota EV3 po seznamu točk na poligonu.
[Robo liga FRI 2019: Sadovnjak]
"""
__author__ = "Laboratory for adaptive systems and parallel processing"
__copyright__ = "Copyright 2019, UL FRI - LASPP"
__credits__ = ["Laboratory for adaptive systems and parallel processing"]
__license__ = "GPL"
__version__ = "0.1"
__maintainer__ = "Nejc Ilc"
__email__ = "nejc.ilc@fri.uni-lj.si"
__status__ = "Active"
# Če želite na svojem računalniku namestiti ev3dev knjižnico za Python:
# pip install python-ev3dev
from ev3dev.ev3 import TouchSensor, Button, LargeMotor, Sound
# Na EV3 robotu je potrebno namestiti paketa ujson in pycurl:
# sudo apt-get update
# sudo apt-get install python3-pycurl
# sudo apt-get install python3-ujson
import pycurl
import ujson
import sys
import math
from io import BytesIO
from time import time, sleep
from enum import Enum
from collections import deque
# ID robota. Spremenite, da ustreza številki označbe, ki je določena vaši ekipi.
ROBOT_ID = 35
# Konfiguracija povezave na strežnik. LASPP strežnik ima naslov "192.168.0.3".
SERVER_IP = "192.168.0.153"
# Datoteka na strežniku s podatki o tekmi.
GAME_STATE_FILE = "game.json"
# Priklop motorjev na izhode.
MOTOR_LEFT_PORT = 'outA'
MOTOR_RIGHT_PORT = 'outD'
# Najvišja dovoljena hitrost motorjev.
SPEED_MAX = 600
# Najvišja dovoljena nazivna hitrost motorjev pri vožnji naravnost.
# Naj bo manjša kot SPEED_MAX, da ima robot še možnost zavijati.
SPEED_BASE_MAX = 500
# Parametri za PID
# Obračanje na mestu in zavijanje med vožnjo naravnost
PID_TURN_KP = 1.2
PID_TURN_KI = 0.0
PID_TURN_KD = 0.2
PID_TURN_INT_MAX = 100
# Nazivna hitrost pri vožnji naravnost.
PID_STRAIGHT_KP = 1.2
PID_STRAIGHT_KI = 0.0
PID_STRAIGHT_KD = 0.2
PID_STRAIGHT_INT_MAX = 100
# Dolžina FIFO vrste za hranjenje meritev (oddaljenost in kot do cilja).
HIST_QUEUE_LENGTH = 3
# Razdalje - tolerance
# Dovoljena napaka v oddaljenosti do cilja [mm].
DIST_EPS = 60
# Dovoljena napaka pri obračanju [stopinje].
DIR_EPS = 5
# Bližina cilja [mm].
DIST_NEAR = 100
# Koliko sekund je robot lahko stanju vožnje naravnost v bližini cilja
# (oddaljen manj kot DIST_NEAR), preden sprožimo varnostni mehanizem
# in ga damo v stanje obračanja na mestu.
TIMER_NEAR_TARGET = 3
#bit, ki name pove, ali imamo jabolko v klescah ali ne
gotApple = False
# bit, ki nam pove, ce imamo tarco lockano
targetLocked = False
target = None
appleKoord = None
class State(Enum):
"""
Stanja robota.
"""
def __str__(self):
return str(self.name)
IDLE = 0
TURN = 1
DRIVE_STRAIGHT = 2
LOAD_NEXT_TARGET = 3
GO_GET_APPLE = 4
GO_HOME = 5
BACK_OFF = 6
class Connection():
"""
Objekt za vzpostavljanje povezave s strežnikom.
"""
def __init__(self, url: str):
"""
Inicializacija nove povezave.
Argumenti:
url: pot do datoteke na strežniku (URL)
"""
self._url = url
self._buffer = BytesIO()
self._pycurlObj = pycurl.Curl()
self._pycurlObj.setopt(self._pycurlObj.URL, self._url)
self._pycurlObj.setopt(self._pycurlObj.CONNECTTIMEOUT, 10)
self._pycurlObj.setopt(self._pycurlObj.WRITEDATA, self._buffer)
def request(self, debug=False):
"""
Nalaganje podatkov s strežnika.
"""
# Počistimo pomnilnik za shranjevanje sporočila
self._buffer.seek(0, 0)
self._buffer.truncate()
# Pošljemo zahtevek na strežnik
self._pycurlObj.perform()
# Dekodiramo sporočilo
msg = self._buffer.getvalue().decode()
# Izluščimo podatke iz JSON
try:
return ujson.loads(msg)
except ValueError as err:
if debug:
print('Napaka pri razclenjevanju datoteke JSON: ' + str(err))
print('Sporocilo streznika:')
print(msg)
return -1
def test_delay(self, num_iters: int = 10):
"""
Merjenje zakasnitve pri pridobivanju podatkov o tekmi s strežnika.
Zgolj informativno.
"""
sum_time = 0
for i in range(num_iters):
start_time = time()
if self.request() == -1:
robot_die()
elapsed_time = time() - start_time
sum_time += elapsed_time
return sum_time / num_iters
class PID():
"""
Implementacija algoritma za regulacijo PID.
Nekaj virov za razjasnitev osnovnega načela delovanja:
- https://en.wikipedia.org/wiki/PID_controller
- https://blog.opticontrols.com/archives/344
- https://www.youtube.com/watch?v=d2AWIA6j0NU
"""
def __init__(
self,
setpoint: float,
Kp: float,
Ki: float = None,
Kd: float = None,
integral_limit: float = None):
"""
Ustvarimo nov regulator PID s pripadajočimi parametri.
Argumenti:
setpoint: ciljna vrednost regulirane spremenljivke
Kp: ojačitev proporcionalnega dela regulatorja.
Visoke vrednosti pomenijo hitrejši odziv sistema,
vendar previsoke vrednosti povzročijo oscilacije in nestabilnost.
Ki: ojačitev integralnega člena regulatorja.
Izniči napako v ustaljenem stanju. Zmanjša odzivnost.
Kd: ojačitev odvoda napake.
Zmanjša čas umirjanja in poveča odzivnost.
integral_limit: najvišja vrednost integrala
"""
self._setpoint = setpoint
self._Kp = Kp
self._Ki = Ki
self._Kd = Kd
self._integral_limit = integral_limit
self._error = None
self._time = None
self._integral = None
self._value = None
def reset(
self,
setpoint: float = None,
Kp: float = None,
Ki: float = None,
Kd: float = None,
integral_limit: float = None):
"""
Ponastavitev regulatorja.
Lahko mu tudi spremenimo katero od vrednosti parametrov.
Napaka, integral napake in čas se ponastavijo.
"""
if setpoint is not None:
self._setpoint = setpoint
if Kp is not None:
self._Kp = Kp
if Ki is not None:
self._Ki = Ki
if Kd is not None:
self._Kd = Kd
if integral_limit is not None:
self._integral_limit = integral_limit
self._error = None
self._time = None
self._integral = None
self._value = None
def update(self, measurement: float) -> float:
"""
Izračunamo vrednost izhoda regulatorja (regulirna veličina)
glede na izmerjeno vrednost regulirane veličine (measurement)
in ciljno vrednost (setpoint).
Argumenti:
measurement: s tipali izmerjena vrednost regulirane veličine
Izhodna vrednost:
regulirna veličina, s katero želimo popraviti delovanje sistema
(regulirano veličino), da bo dosegel ciljno vrednost
"""
if self._value is None:
# Na začetku še nimamo zgodovine meritev, zato inicializiramo
# integral in vrnemo samo proporcionalni člen.
self._value = measurement
# Zapomnimo si začetni čas.
self._time = time()
# Ponastavimo integral napake.
self._integral = 0
# Napaka = ciljna vrednost - izmerjena vrednost regulirane veličine.
self._error = self._setpoint - measurement
return self._Kp * self._error
else:
# Sprememba časa
time_now = time()
delta_time = time_now - self._time
self._time = time_now
# Izmerjena vrednost regulirane veličine.
self._value = measurement
# Napaka = ciljna vrednost - izmerjena vrednost regulirane veličine.
error = self._setpoint - self._value
# Proporcionalni del
P = self._Kp * error
# Integralni in odvodni člen sta opcijska.
if self._Ki is None:
I = 0
else:
# Integral se poveča za (sprememba napake) / (sprememba časa).
self._integral += error * delta_time
# Ojačitev integralnega dela.
I = self._Ki * self._integral
if self._integral_limit is not None:
I = max(min(I, self._integral_limit),
(-1)*(self._integral_limit))
if self._Kd is None:
D = 0
else:
# Odvod napake z ojačitvijo.
D = self._Kd * (error - self._error) / delta_time
# Posodobimo napako.
self._error = error
# Vrnemo regulirno veličino, sestavljeno iz proporcionalnega,
# integralnega in odvodnega člena.
return P + I + D
class Point():
"""
Točka na poligonu.
"""
def __init__(self, position):
self.x = position[0]
self.y = position[1]
def __str__(self):
return '('+str(self.x)+', '+str(self.y)+')'
def get_angle(p1, a1, p2) -> float:
"""
Izračunaj kot, za katerega se mora zavrteti robot, da bo obrnjen proti točki p2.
Robot se nahaja v točki p1 in ima smer (kot) a1.
"""
a = math.degrees(math.atan2(p2.y-p1.y, p2.x - p1.x))
a_rel = a - a1
if abs(a_rel) > 180:
if a_rel > 0:
a_rel = a_rel - 360
else:
a_rel = a_rel + 360
return a_rel
def get_distance(p1: Point, p2: Point) -> float:
"""
Evklidska razdalja med dvema točkama na poligonu.
"""
return math.sqrt((p2.x-p1.x)**2 + (p2.y-p1.y)**2)
def init_large_motor(port: str) -> LargeMotor:
"""
Preveri, ali je motor priklopljen na izhod `port`.
Vrne objekt za motor (LargeMotor).
"""
motor = LargeMotor(port)
while not motor.connected:
print('\nPriklopi motor na izhod ' + port +
' in pritisni + spusti gumb DOL.')
wait_for_button('down')
motor = LargeMotor(port)
return motor
def init_sensor_touch() -> TouchSensor:
"""
Preveri, ali je tipalo za dotik priklopljeno na katerikoli vhod.
Vrne objekt za tipalo.
"""
sensor = TouchSensor()
while not sensor.connected:
print('\nPriklopi tipalo za dotik in pritisni + spusti gumb DOL.')
wait_for_button('down')
sensor = TouchSensor()
return sensor
def wait_for_button(btn_name: str = 'down'):
"""
Čakaj v zanki dokler ni gumb z imenom `btn_name` pritisnjen in nato sproščen.
"""
while not getattr(btn, btn_name):
pass
flag = False
while getattr(btn, btn_name):
if not flag:
flag = True
def beep(duration=1000, freq=440):
"""
Potrobi s frekvenco `freq` za čas `duration`. Klic ne blokira.
"""
Sound.tone(freq, duration)
# Če želimo, da blokira, dokler se pisk ne konča.
#Sound.tone(freq, duration).wait()
def robot_die():
"""
Končaj s programom na robotu. Ustavi motorje.
"""
print('KONEC')
motor_left.stop(stop_action='brake')
motor_right.stop(stop_action='brake')
Sound.play_song((
('D4', 'e'),
('C4', 'e'),
('A3', 'h')))
sys.exit(0)
# -----------------------------------------------------------------------------
# NASTAVITVE TIPAL, MOTORJEV IN POVEZAVE S STREŽNIKOM
# -----------------------------------------------------------------------------
# Nastavimo tipala in gumbe.
print('Priprava tipal ... ', end='', flush=True)
btn = Button()
#sensor_touch = init_sensor_touch()
print('OK!')
# Nastavimo velika motorja. Priklopljena naj bosta na izhoda A in D.
print('Priprava motorjev ... ', end='')
motor_left = init_large_motor(MOTOR_LEFT_PORT)
motor_right = init_large_motor(MOTOR_RIGHT_PORT)
print('OK!')
# Nastavimo povezavo s strežnikom.
url = SERVER_IP+'/'+GAME_STATE_FILE
print('Vspostavljanje povezave z naslovom ' + url + ' ... ', end='', flush=True)
conn = Connection(url)
print('OK!')
# Izmerimo zakasnitev pri pridobivanju podatkov (povprečje num_iters meritev)
print('Zakasnitev v komunikaciji s streznikom ... ', end='', flush=True)
print('%.4f s' % (conn.test_delay(num_iters=10)))
# -----------------------------------------------------------------------------
# PRIPRAVA NA TEKMO
# -----------------------------------------------------------------------------
# Pridobimo podatke o tekmi.
game_state = conn.request()
# Ali naš robot sploh tekmuje? Če tekmuje, ali je team1 ali team2?
if ROBOT_ID == game_state['team1']['id']:
team_my_tag = 'team1'
team_op_tag = 'team2'
elif ROBOT_ID == game_state['team2']['id']:
team_my_tag = 'team2'
team_op_tag = 'team1'
else:
print('Robot ne tekmuje.')
robot_die()
print('Robot tekmuje in ima interno oznako "' + team_my_tag + '"')
# Doloci cilj za robota (seznam točk na poligonu).
# Našem primeru se bo vozil po notranjih kotih obeh košar.
targets_list = [
Point(game_state['field']['baskets'][team_my_tag]['bottomRight']),
Point(game_state['field']['baskets'][team_my_tag]['topRight']),
Point(game_state['field']['baskets'][team_op_tag]['topLeft']),
Point(game_state['field']['baskets'][team_op_tag]['bottomLeft']),
]
print('Seznam ciljnih tock:')
for trgt in targets_list:
print('\t' + str(trgt))
# -----------------------------------------------------------------------------
# GLAVNA ZANKA
# -----------------------------------------------------------------------------
print('Izvajam glavno zanko. Prekini jo s pritiskon na tipko DOL.')
print('Cakam na zacetek tekme ...')
# Začetno stanje.
state = State.IDLE
# Prejšnje stanje.
state_old = -1
# Indeks trenutne ciljne lokacije.
target_idx = 0
# Regulator PID za obračanje na mestu.
# setpoint=0 pomeni, da naj bo kot med robotom in ciljem (target_angle) enak 0.
# Naša regulirana veličina je torej kar napaka kota, ki mora biti 0.
# To velja tudi za regulacijo vožnje naravnost.
PID_turn = PID(
setpoint=0,
Kp=PID_TURN_KP,
Ki=PID_TURN_KI,
Kd=PID_TURN_KD,
integral_limit=PID_TURN_INT_MAX)
# PID za vožnjo naravnost - regulira nazivno hitrost za oba motorja,
# ki je odvisna od oddaljenosti od cilja.
# setpoint=0 pomeni, da mora biti razdalja med robotom in ciljem enaka 0.
PID_frwd_base = PID(
setpoint=0,
Kp=PID_STRAIGHT_KP,
Ki=PID_STRAIGHT_KI,
Kd=PID_STRAIGHT_KD,
integral_limit=PID_STRAIGHT_INT_MAX)
# PID za obračanje med vožnjo naravnost.
# setpoint=0 pomeni, da naj bo kot med robotom in ciljem (target_angle) enak 0.
PID_frwd_turn = PID(
setpoint=0,
Kp=PID_TURN_KP,
Ki=PID_TURN_KI,
Kd=PID_TURN_KD,
integral_limit=PID_TURN_INT_MAX)
# Hitrost na obeh motorjih.
speed_right = 0
speed_left = 0
# Zgodovina (okno) zadnjih nekaj vrednosti meritev,
# implementirana kot vrsta FIFO.
robot_dir_hist = deque([180.0] * HIST_QUEUE_LENGTH)
robot_dist_hist = deque([math.inf] * HIST_QUEUE_LENGTH)
# Merimo čas obhoda zanke. Za visoko odzivnost robota je zelo pomembno,
# da je ta čas čim krajši.
t_old = time()
do_main_loop = True
while do_main_loop and not btn.down:
time_now = time()
loop_time = time_now - t_old
t_old = time_now
# Zaznaj spremembo stanja.
if state != state_old:
state_changed = True
else:
state_changed = False
state_old = state
# Iz seznama ciljev izberi trenutnega.
#target = targets_list[target_idx]
# Osveži stanje tekme.
game_state = conn.request()
if game_state == -1:
print('Napaka v paketu, ponovni poskus ...')
else:
game_on = game_state['gameOn']
time_left = game_state['timeLeft']
# dodamo vsa dobra jabolka v seznam
good_apples = []
for apple in game_state['apples']:
if apple['type'] == "appleGood":
good_apples.append(Point(apple['position']))
print(str(targetLocked) + " " + str(gotApple))
if not (targetLocked):
if not (gotApple):
print("ALI SE TO ZGODI???")
target = good_apples[0]
targetLocked = True
home = Point(game_state['field']['baskets'][team_my_tag]['topLeft'])
home.x += 230
home.y -= 360
# Pridobi pozicijo in orientacijo svojega robota;
# najprej pa ga poišči v tabeli vseh robotov na poligonu.
robot_pos = None
robot_dir = None
for robot_data in game_state['robots']:
if robot_data['id'] == ROBOT_ID:
robot_pos = Point(robot_data['position'][0:2])
robot_dir = robot_data['direction']
#robot_pos.x += 30 * math.cos(robot_dir)
#robot_pos.y += 30 * math.sin(robot_dir)
# Ali so podatki o robotu veljavni? Če niso, je zelo verjetno,
# da sistem ne zazna oznake na robotu.
robot_alive = (robot_pos is not None) and (robot_dir is not None)
# Če tekma poteka in je oznaka robota vidna na kameri,
# potem izračunamo novo hitrost na motorjih.
# Sicer motorje ustavimo.
if game_on and robot_alive:
# Razdalja med robotom in ciljem.
target_dist = get_distance(robot_pos, target)
# Kot med robotom in ciljem.
target_angle = get_angle(robot_pos, robot_dir, target)
# Spremljaj zgodovino meritev kota in oddaljenosti.
# Odstrani najstarejši element in dodaj novega - princip FIFO.
robot_dir_hist.popleft()
robot_dir_hist.append(target_angle)
robot_dist_hist.popleft()
robot_dist_hist.append(target_dist)
if state == State.IDLE:
# Stanje mirovanja - tu se odločamo, kaj bo robot sedaj počel.
print("Stanje mirovanja")
speed_right = 0
speed_left = 0
# Preverimo, ali je robot na ciljni točki.
# Če ni, ga tja pošljemo.
if target_dist > DIST_EPS:
print("||Nismo na cilju||")
state = State.TURN
robot_near_target_old = False
else:
print("||Smo na cilju||")
if(gotApple):
state = State.GO_GET_APPLE
gotApple = False
else:
state = State.GO_HOME
gotApple = True
#state = State.LOAD_NEXT_TARGET
elif state == State.LOAD_NEXT_TARGET:
# Naložimo naslednjo ciljno točko iz seznama.
print("Nalaga naslednjo ciljno tocko")
#target_idx = target_idx + 1
# Če smo prišli do konca seznama, gremo spet od začetka
#if target_idx >= len(targets_list):
# target_idx = 0
target = good_apples[0]
state = State.IDLE
elif state == State.GO_GET_APPLE:
# Naložimo naslednjo ciljno točko iz seznama.
print("Nalaga pozicijo jabolka kot tocko")
#target_idx = target_idx + 1
# Če smo prišli do konca seznama, gremo spet od začetka
#if target_idx >= len(targets_list):
# target_idx = 0
target = good_apples[0]
good_apples.pop(0)
state = State.IDLE
elif state == State.GO_HOME:
# Naložimo naslednjo ciljno točko iz seznama.
print("Nalaga dom kot ciljno tocko")
#target_idx = target_idx + 1
# Če smo prišli do konca seznama, gremo spet od začetka
#if target_idx >= len(targets_list):
# target_idx = 0
target = home
state = State.IDLE
elif state == State.TURN:
# Obračanje robota na mestu, da bo obrnjen proti cilju.
print("Stanje obracanja")
if state_changed:
# Če smo ravno prišli v to stanje, najprej ponastavimo PID.
PID_turn.reset()
# Ali smo že dosegli ciljni kot?
# Zadnjih nekaj obhodov zanke mora biti absolutna vrednost
# napake kota manjša od DIR_EPS.
err = [abs(a) > DIR_EPS for a in robot_dir_hist]
if sum(err) == 0:
# Vse vrednosti so znotraj tolerance, zamenjamo stanje.
speed_right = 0
speed_left = 0
state = State.DRIVE_STRAIGHT
else:
# Reguliramo obračanje.
# Ker se v regulatorju trenutna napaka izračuna kot:
# error = setpoint - measurement,
# dobimo negativno vrednost, ko se moramo zavrteti
# v pozitivno smer.
# Primer:
# Robot ima smer 90 stopinj (obrnjen je proti "severu").
# Cilj se nahaja na njegovi levi in da ga doseže,
# se mora obrniti za 90 stopinj.
# setpoint=0
# target_angle = measurement = 90
# error = setpoint - measurement = -90
# u = funkcija, odvisna od error in parametrov PID.
# Če imamo denimo Kp = 1, Ki = Kd = 0, potem bo u = -90.
# Robot se mora zavrteti v pozitivno smer,
# torej z desnim kolesom naprej in levim nazaj.
# Zato:
# speed_right = -u
# speed_left = u
# Lahko bi tudi naredili droben trik in bi rekli:
# measurement= -target_angle.
# V tem primeru bi bolj intuitivno nastavili
# speed_right = u in speed_left = -u.
u = PID_turn.update(measurement=target_angle)
speed_right = -u
speed_left = u
elif state == State.DRIVE_STRAIGHT:
# Vožnja robota naravnost proti ciljni točki.
print("stanje voznje naravnost")
# Vmes bi radi tudi zavijali, zato uporabimo dva regulatorja.
if state_changed:
# Ponastavi regulatorja PID.
PID_frwd_base.reset()
PID_frwd_turn.reset()
timer_near_target = TIMER_NEAR_TARGET
# Ali smo blizu cilja?
robot_near_target = target_dist < DIST_NEAR
if not robot_near_target_old and robot_near_target:
# Vstopili smo v bližino cilja.
# Začnimo odštevati varnostno budilko.
timer_near_target = TIMER_NEAR_TARGET
if robot_near_target:
timer_near_target = timer_near_target - loop_time
robot_near_target_old = robot_near_target
# Ali smo že na cilju?
# Zadnjih nekaj obhodov zanke mora biti razdalja do cilja
# manjša ali enaka DIST_EPS.
err_eps = [d > DIST_EPS for d in robot_dist_hist]
if sum(err_eps) == 0:
print("Smo na cilju")
# Razdalja do cilja je znotraj tolerance, zamenjamo stanje.
speed_right = 0
speed_left = 0
if(gotApple):
state = State.GO_GET_APPLE
gotApple = False
else:
state = State.GO_HOME
gotApple = True
#state = State.LOAD_NEXT_TARGET
elif timer_near_target < 0:
# Smo morda blizu cilja, in je varnostna budilka potekla?
speed_right = 0
speed_left = 0
state = State.TURN
else:
u_turn = PID_frwd_turn.update(measurement=target_angle)
# Ker je napaka izračunana kot setpoint - measurement in
# smo nastavili setpoint na 0, bomo v primeru u_base dobili
# negativne vrednosti takrat, ko se bo robot moral premikati
# naprej. Zato dodamo minus pri izračunu hitrosti motorjev.
u_base = PID_frwd_base.update(measurement=target_dist)
# Omejimo nazivno hitrost, ki je enaka za obe kolesi,
# da imamo še manevrski prostor za zavijanje.
u_base = min(max(u_base, -SPEED_BASE_MAX), SPEED_BASE_MAX)
speed_right = -u_base - u_turn
speed_left = -u_base + u_turn
# Omejimo vrednosti za hitrosti na motorjih.
speed_right = round(
min(
max(speed_right, -SPEED_MAX),
SPEED_MAX)
)
speed_left = round(
min(
max(speed_left, -SPEED_MAX),
SPEED_MAX)
)
# Vrtimo motorje.
motor_right.run_forever(speed_sp=-speed_right)
motor_left.run_forever(speed_sp=-speed_left)
else:
# Robot bodisi ni viden na kameri bodisi tema ne teče.
motor_left.stop(stop_action='brake')
motor_right.stop(stop_action='brake')
# Konec programa
robot_die()