forked from erlerobot/erle_control
/
stabilize.py
271 lines (216 loc) · 7.34 KB
/
stabilize.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
# -*- encoding: utf-8 -*-
'''
_____ _ _____ _ _
| __|___| |___ | __ |___| |_ ___| |_
| __| _| | -_| | -| . | . | . | _|
|_____|_| |_|___| |__|__|___|___|___|_|
@author: Víctor Mayoral Vilches <victor@erlerobot.com>
@description: Code for stabilizing the quadrotor. It runs and infinite
loop that adjust the 4 motors according to the IMU readings.
'''
import os
from subprocess import call
from imu import IMU
from motors import Motor
from pid import PID
from time import sleep
import datetime as dt
from dynamical_model import Dynamical_Model
import signal
import sys
from bt_controller import BT_Controller
"""
When testing is important to power off the motors when the
script is stoped through SIGINT
"""
def signal_handler(signal, frame):
print 'Setting all motors to 0...'
motor1.setSpeedBrushless(0)
motor2.setSpeedBrushless(0)
motor3.setSpeedBrushless(0)
motor4.setSpeedBrushless(0)
for mot in motors:
mot.go()
# calculate the frequency of the main loop
sum = 0
for f in frequencies:
sum+=f
print "average frequency (Hz): "+str(sum/len(frequencies))
print "minimum frequency (Hz): "+str(min(frequencies))
print "killing the controller thread (bt thread)..."
# # stop bt-controller NOT WORKING
# bt.stop()
sys.exit(0)
""" Limits the thrust passed to the motors
in the range (-100,100)
"""
def limitThrust(thrust, upperLimit = 100, lowerLimit = 0):
if thrust > upperLimit:
thrust = upperLimit
elif thrust < lowerLimit:
thrust = lowerLimit
return thrust
###############################
# INIT
###############################
# Set the handler for SIGINT
signal.signal(signal.SIGINT, signal_handler)
# Activate I2C-2
retvalue = os.system("/bin/echo BB-I2C1 > $SLOTS")
# Activate the motors with the init script
retvalue = os.system("/usr/bin/python /root/erle_control/init_motors.py")
# First time the IMU sensor raises an error (probably an error with the firmware or so). The following program readies the sensor to work properly:
#retvalue = os.system("/root/erle_control/imu/imu -b 2")
call(["/root/erle_control/imu/imu", "-b2"])
############################
# variables
############################
logging = 0
limit_thrust = 70
roll_d = 0
pitch_d = 0
yaw_d = 0
z_d = 0
#xpos = 0
#ypos = 0
# attitude constants
if len(sys.argv) > 1:
Kp = float(sys.argv[1])
Kd = float(sys.argv[2])
Ki = float(sys.argv[3])
else:
Kp = 0.9
Kd = 0.2
Ki = 0
############################
#instantiate the BT controller
bt = BT_Controller(z_d)
bt.run()
#instantiate IMU
#TODO see how to import C interface to python
imu=IMU()
#MyKalman=KalmanFilter(....)
# dynamical model instance
dyn_model = Dynamical_Model()
#instantiate motors and put them together in a list
motor1=Motor(1)
motor2=Motor(2)
motor3=Motor(3)
motor4=Motor(4)
motors=[motor1,motor2,motor3,motor4]
# instantiate PID controllers
rollPID=PID(Kp, Kd, Ki) # Kp, Kd, Ki
pitchPID=PID(Kp, Kd, Ki)
yawPID=PID(0, 0, 0)
zPID=PID(1, 0, 0)
#xposPID=PID(-0.09, -0.1, 0)
#yposPID=PID(-0.09, -0.1, 0)
# test variables
frequencies = []
if logging:
print "------------------------"
print " stabilize loop "
print "------------------------"
############################
#loop
############################
while 1:
start = dt.datetime.now()
# process the input from the controller
z_d = bt.getThrust()
# print "z_d: "+str(z_d)
# pitch, roll and yaw DESIRED, get FROM THE TELEOPERATOR (for now, global vars are used)
# roll_d = 0
# pitch_d = 0
# yaw_d = 0
# z_d = 10
# #xpos = 0
# #ypos = 0
#Measure angles
#roll_m, pitch_m, yaw_m = imu.read_fusedEuler()
roll_m, pitch_m, yaw_m = imu.read_fusedEuler(0)
#MyKalman.measure([roll,pitch, yaw])
z_m = 0 # putting this is the same as telling it to always increase the thrust (go up)
#Run the PIDs
roll = rollPID.update(roll_d - roll_m, 0)
pitch = pitchPID.update(pitch_d - pitch_m, 0)
yaw = yawPID.update(yaw_d - yaw_m, 0)
z = zPID.update(z_d - z_m, 0)
#xpos = xposPID.update(xpos_d - xpos_m)
#ypos = yposPID.update(ypos_d - ypos_m)
#TODO change this parameter and see the behaviour
#thrust is provided by the controller (NOTE: this is also treated as "z" and it should use the zPID controller)
# the point of hovering is 35% duty cycle in the motors
if logging:
#Log the values:
print "**************************"
print "Desired angles:"
print " pitch:" + str(pitch_d)
print " roll:" + str(roll_d)
print " yaw:" + str(yaw_d)
print " z:" + str(z_d)
print "Measured angles:"
print " pitch:" + str(pitch_m)
print " roll:" + str(roll_m)
print " yaw:" + str(yaw_m)
# print " thrust (z):" + str(z_d) # maybe using some sensor?
print "PID output angles:"
print " pitch:" + str(pitch)
print " roll:" + str(roll)
print " yaw:" + str(yaw)
print " z:" + str(z)
# using M. Wieremma's thesis
#u = dyn_model.motor_inversion1(z, roll, pitch, yaw, logging)
# using Crazyflie's implementation
#u = dyn_model.motor_inversion2(z, roll, pitch, yaw, logging)
# using Crazyflie's HACKED implementation
#u = dyn_model.motor_inversion3(z, roll, pitch, yaw, logging)
# using M. Wieremma's thesis HACKED impl
u = dyn_model.motor_inversion4(z, roll, pitch, yaw, logging)
#if the controller says that the thrust is 0, all motors to 0
if z_d == 0:
motorPowerM1 = 0;
motorPowerM2 = 0;
motorPowerM3 = 0;
motorPowerM4 = 0;
else:
motorPowerM1 = limitThrust(u[0], limit_thrust);
motorPowerM2 = limitThrust(u[1], limit_thrust);
motorPowerM3 = limitThrust(u[2], limit_thrust);
motorPowerM4 = limitThrust(u[3], limit_thrust);
if logging:
#Log the motor powers:
print "------------------------"
print "motorPowerM1 (limited):" + str(motorPowerM1)
print "motorPowerM2 (limited):" + str(motorPowerM2)
print "motorPowerM3 (limited):" + str(motorPowerM3)
print "motorPowerM4 (limited):" + str(motorPowerM4)
print "**************************"
#Set motor speeds
motor1.setSpeedBrushless(motorPowerM1)
motor2.setSpeedBrushless(motorPowerM2)
motor3.setSpeedBrushless(motorPowerM3)
motor4.setSpeedBrushless(motorPowerM4)
#Start Motors
for mot in motors:
mot.go()
#Kalman Prediction
#MyKalman.predict()
# calculate the time each iteration takes
time_u = (dt.datetime.now() - start).microseconds
time_s = time_u/1e6
frequency = 1e6/time_u
# force 50 Hz loop
if time_s < 20e-3: #50 Hz
sleep(20e-3 - time_s)
# # force 60 Hz loop
# if time_s < 16.66e-3: #50 Hz
# sleep(16.66e-3 - time_s)
# # force 70 Hz loop
# if time_s < 14.28e-3: #50 Hz
# sleep(14.28e-3 - time_s)
time_u = (dt.datetime.now() - start).microseconds
frequency = 1e6/time_u
frequencies.append(frequency)
############################
############################