-
Notifications
You must be signed in to change notification settings - Fork 1
/
test_sonar_rotate.py
49 lines (40 loc) · 1.1 KB
/
test_sonar_rotate.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
import brickpi
import time
import random
import time
import sys
import random
import numpy as np
from robot_controller import *
from particleDataStructures import *
can = Canvas()
mymap = Map(can);
particle = Particles(can)
mymap.add_wall((0,0,0,168)); # a
mymap.add_wall((0,168,84,168)); # b
mymap.add_wall((84,126,84,210)); # c
mymap.add_wall((84,210,168,210)); # d
mymap.add_wall((168,210,168,84)); # e
mymap.add_wall((168,84,210,84)); # f
mymap.add_wall((210,84,210,0)); # g
mymap.add_wall((210,0,0,0)); # h
mymap.add_wall((84,30,180,30));
mymap.add_wall((180,30,180,54));
mymap.add_wall((180,54,138,54));
mymap.add_wall((138,54,138,168));
mymap.add_wall((138,168,114,168));
mymap.add_wall((114,168,114,84));
mymap.add_wall((114,84,84,84));
mymap.add_wall((84,84,84,30));
mymap.draw();
controller = RobotController(particle)
particleSet = controller.initParticles()
#controller.go2(82)
#controller.turnSonar90()
self.turnSonar(-90)
time.sleep(3)
controller.turnSonar90Neg()
#controller.go2(82)
controller.turnSonar90Neg()
controller.turnSonar90Neg()
controller.turnSonar90Neg()