/
robot.py
71 lines (51 loc) · 1.65 KB
/
robot.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
import drive
import nxt
class Robot:
def __init__(self):
self.b = nxt.find_one_brick()
self.leftWheel = drive.Drive(self.b, nxt.PORT_B)
self.rightWheel = drive.Drive(self.b, nxt.PORT_C)
self.leftWheel.SetParam(1, 1, 1)
self.rightWheel.SetParam(1, 1, 1)
self.leftWheel.start()
self.rightWheel.start()
def resetData(self):
self.leftWheel.stop()
self.leftWheel.join()
self.rightWheel.stop()
self.rightWheel.join()
self.leftWheel = drive.Drive(self.b, nxt.PORT_B)
self.rightWheel = drive.Drive(self.b, nxt.PORT_C)
def __turnLeft(self):
self.resetData()
self.leftWheel.SetParam(-1, degree=10)
self.rightWheel.SetParam(1, degree=10)
self.leftWheel.start()
self.rightWheel.start()
def __turnRight(self):
self.resetData()
self.leftWheel.SetParam(1, degree=10)
self.rightWheel.SetParam(-1, degree=10)
self.leftWheel.start()
self.rightWheel.start()
def __moveForward(self):
self.resetData()
self.leftWheel.SetParam()
self.rightWheel.SetParam()
self.leftWheel.start()
self.rightWheel.start()
def __stop(self):
self.resetData()
self.leftWheel.SetParam(1, 1, 1)
self.rightWheel.SetParam(1, 1, 1)
self.leftWheel.start()
self.rightWheel.start()
def doCommand(self, cmd):
if cmd == "2":
self.__turnLeft()
elif cmd == "3":
self.__turnRight()
elif cmd == "4":
self.__moveForward()
elif cmd == "5":
self.__stop()