forked from zeetwii/combee
-
Notifications
You must be signed in to change notification settings - Fork 0
/
motorController.py
144 lines (109 loc) · 5.12 KB
/
motorController.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
import pika # needed to send messages out via RabbitMQ
import threading # needed for multi threads
import gpiozero # needed for motor control
import time # needed for sleep
class MotorController:
"""
Class that controls the motors for the rover
"""
def __init__(self, maxSpeed = 1):
"""
Initialization method
Args:
maxSpeed (float): the max speed of the motors, a decimal value from zero to one
"""
self.maxSpeed = maxSpeed
self.movementList = []
self.turnRate = 120.0 # turns 360 degrees in 3 seconds
#Set up the four motors
self.frontRight = gpiozero.Motor(11, 9)
self.backRight = gpiozero.Motor(19, 26)
self.frontLeft = gpiozero.Motor(12, 16)
self.backLeft = gpiozero.Motor (21, 20)
# zero out all speed
self.frontLeft.forward(0)
self.frontRight.forward(0)
self.backLeft.forward(0)
self.backRight.forward(0)
def processMessage(self, message):
"""
Processes a command message generated by the LLM and adds it to the movement list
Args:
message (str): The movement command
"""
print("Adding message")
self.movementList.append(message)
def moveIt(self):
"""
Thread that drives the rover
"""
print("moveIt started")
while True:
# zero out all speed
self.frontLeft.forward(0)
self.frontRight.forward(0)
self.backLeft.forward(0)
self.backRight.forward(0)
time.sleep(1)
while len(self.movementList) > 0:
print("Running movement command")
move = str(self.movementList[0]).split()
if len(move) != 3: # incorrect format
print(f"Error, {self.movementList[0]} formatted incorrectly")
else:
if move[0] == "Forward":
try:
runTime = float(move[2])
if runTime > 10:
runTime = 10
self.frontLeft.forward(self.maxSpeed)
self.frontRight.forward(self.maxSpeed)
self.backLeft.forward(self.maxSpeed)
self.backRight.forward(self.maxSpeed)
time.sleep(runTime)
self.frontLeft.forward(0)
self.frontRight.forward(0)
self.backLeft.forward(0)
self.backRight.forward(0)
except ValueError:
print("Time was not represented as a float in forward message")
elif move[0] == "Reverse":
try:
runTime = float(move[2])
if runTime > 10:
runTime = 10
self.frontLeft.backward(self.maxSpeed)
self.frontRight.backward(self.maxSpeed)
self.backLeft.backward(self.maxSpeed)
self.backRight.backward(self.maxSpeed)
time.sleep(runTime)
self.frontLeft.forward(0)
self.frontRight.forward(0)
self.backLeft.forward(0)
self.backRight.forward(0)
except ValueError:
print("Time was not represented as a float in backward message")
elif move[0] == "Turn":
try:
angle = float(move[1])
movementTime = abs(angle/self.turnRate)
if angle > 0: # Turn right
self.frontLeft.forward(self.maxSpeed)
self.frontRight.backward(self.maxSpeed)
self.backLeft.forward(self.maxSpeed)
self.backRight.backward(self.maxSpeed)
else: # Turn left
self.frontLeft.backward(self.maxSpeed)
self.frontRight.forward(self.maxSpeed)
self.backLeft.backward(self.maxSpeed)
self.backRight.forward(self.maxSpeed)
time.sleep(movementTime)
self.frontLeft.forward(0)
self.frontRight.forward(0)
self.backLeft.forward(0)
self.backRight.forward(0)
except ValueError:
print("Angle was not represented as a float in turn message")
else:
print(f"Error, {str(move[0])} unknown")
self.movementList.pop(0)