/
HeliosController.py
executable file
·155 lines (127 loc) · 3.99 KB
/
HeliosController.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
#!/usr/bin/env python
import time, sys
from ConfigParser import ConfigParser
from Globals import Globals
try:
import wiringpi
except:
print "wiringpi not found, switching to simulation"
Globals.globSimulate = True
from MotorDriver import MotorDriver
from DistanceSensor import DistanceSensor
from AltitudeControlLoop import AltitudeControlLoop
from Camera import Camera
from Observable import Observable
from subprocess import call
import base64
class HeliosController(Observable):
md = None
ds = None
acl = None
cam = None
status = {}
def __init__(self, conffile):
Observable.__init__(self)
cp = ConfigParser()
cp.read(conffile)
if not Globals.globSimulate:
wiringpi.wiringPiSetupGpio()
self.md = MotorDriver([
cp.getint("MotorDriver", "LEFT_L"),
cp.getint("MotorDriver", "LEFT_R"),
cp.getint("MotorDriver", "RIGHT_L"),
cp.getint("MotorDriver", "RIGHT_R"),
cp.getint("MotorDriver", "VERTICAL_L"),
cp.getint("MotorDriver", "VERTICAL_R"),
])
self.md.start()
self.ds = DistanceSensor(
cp.getint("DistanceSensor", "TRIGGER"),
cp.getint("DistanceSensor", "ECHO")
)
self.ds.start()
self.acl = AltitudeControlLoop(self.md, self.ds)
self.update("MotorDriver", self.md)
self.update("DistanceSensor", self.ds)
self.update("AltitudeControlLoop", self.acl)
self.md.subscribe(self.update)
self.ds.subscribe(self.update)
self.acl.subscribe(self.update)
self.setCamera(False)
#self.acl.debug = True
#self.md.debug = True
def shutdown(self):
self.md.shutdown()
self.md.join()
self.ds.shutdown()
self.ds.join()
self.setCamera(False)
def turnLeft(self):
self.md.turnLeft()
def turnRight(self):
self.md.turnRight()
def forward(self):
self.md.forward()
def backward(self):
self.md.backward()
def up(self):
self.md.up()
def down(self):
self.md.down()
def setSpeed(self, val):
self.md.setSpeed(val)
def setAuto(self, val):
self.acl.setAuto(val)
def setHeight(self, val):
self.acl.setHeight(val)
def setForceDescent(self, val):
self.acl.setForceDescent(val)
def setSingleSteerMode(self, val):
self.md.setSingleSteerMode(val)
def setCamera(self, val):
if self.cam == None:
if val:
print "HeliosController enabling camera"
self.cam = Camera()
self.cam.subscribe(self.updateImage)
self.cam.start()
else:
print "HeliosController tried to disable camera, but is not active"
else:
if val:
print "HeliosController tried to enable camera, but is already active"
else:
print "HeliosController disabling camera"
self.cam.shutdown()
self.cam.join()
self.cam = None
self.status["cameraImage"] = "data:image/jpeg;base64," + base64.b64encode(open("nocam.jpg","rb").read())
self.status["cameraActive"] = True if self.cam != None else False
self.emit("HeliosController", self)
def updateImage(self, id, cam):
self.status["cameraImage"] = "data:image/jpeg;base64," + base64.b64encode(cam.getImage())
def getImage(self):
if self.cam != None:
return self.cam.getImage()
return open("nocam.jpg","rb").read()
def update(self, etype, src):
if etype == "MotorDriver":
self.status["statLeft"] = src.statusLeft()
self.status["statRight"] = src.statusRight()
self.status["statVert"] = src.statusVert()
self.status["pwmValue"] = src.getPWM()
self.status["singleSteeringMode"] = src.getSingleSteerMode()
if etype == "DistanceSensor":
self.status["currentAltitude"] = src.getCurrentDistance()
self.status["altitudeHistory"] = src.getDistanceHistory()
if etype == "AltitudeControlLoop":
self.status["requestedAltitude"] = src.getHeight()
self.status["maintainAltitude"] = src.getAuto()
self.status["forceDescent"] = src.getForceDescent()
self.emit("HeliosController", self)
def getStatus(self):
return self.status
if __name__ == '__main__':
hc = HeliosController("config.ini")
time.sleep(5)
hc.shutdown()