-
Notifications
You must be signed in to change notification settings - Fork 0
/
master.py
539 lines (457 loc) · 18.6 KB
/
master.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
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
"""
Main script which calls other scripts/functions during the rocket's life.
1. Ground Idle -
2. Power Flight - detected when liftoff
Acceleration threshold to measure whether we have taken off. We are above the threshold for 0.1 seconds is also
another prerequisite.
3. Unpowered Flight - burnout
If accelerator is less than a certain amount of m/seconds squared. For greater than 0.1 seconds
4. Ballistic Descent - apogee
To detect apogee: we compare two different readings that are one second apart to see if we are ascending or
descending.
5. Chute Descent - pyro fire
If we fired the parachutes. If we are at a certain altitude fire the chutes and switch to the chute state.
6. Landing - if acceleration is almost or near zero for more than 10 seconds.
Guidance Navigation and Control
More Directions:
Will hold the code to connect a Raspberry Pi and it's connected components to the PID controller to allow for feedback
between the two.
One PID loop will be for the x axis, and one will be for the y axis. These pid loops will execute independently of
each other, with each one controlling one servo motor.
Control Flow: (Happening continuously)
Get input from gyroscope connected to Pi, convert to degrees for X and Y.
Put degrees into two PID objects for X and Y, get the output necessary (degrees).
Take the output and send it to the thrust vector control system.
"""
# IMPORT STATEMENTS
import board
import busio
import adafruit_bmp3xx
from simple_pid import PID
from data import write_data_to_csv
import sys
import logging
from Adafruit_BNO055 import BNO055
import time
from datetime import datetime
import RPi.GPIO as GPIO
import servo_control
import math
# Get the current csv_number and update the csv_number to write to the correct filename for no data loss
def find_update_csv_number():
with open("csv_number.txt", "r") as file: # Get the number
number = file.readlines()
number = int(number[0])
file.close()
with open("csv_number.txt", "w") as file: # Update it so we won't be written over
new_number = number + 1
file.write(str(new_number))
file.close()
return number # The number we will use to write data to
csv_number = find_update_csv_number() # CSV number that will be used later to write to csv
# STAGES OF FLIGHT
ground_idle = True
power_flight = False
unpowered_flight = False
ballistic_descent = False
chute_descent = False
landing = False
# Time: seconds, acceleration/altitude: meters
WAIT = 0.5
SLEEP = 0.01
ACCEL_LEVEL = 1 # Acceleration level
DEPLOY_CHUTE_ALTITUDE = 50
STOPPED_ACCELERATION = 0.05
CHUTE_DESCENT_WAIT = 0.5 # How much to wait until we check the acceleration again to transition to chute descent
acceleration = 0 # Get acceleration from IMU
# CONFIGURATION FOR IMU CONTROLLER
# Raspberry Pi configuration with serial UART
bno = BNO055.BNO055(serial_port='/dev/serial0', rst=18)
# Enable verbose debug logging if -v is passed as a parameter.
if len(sys.argv) == 2 and sys.argv[1].lower() == '-v':
logging.basicConfig(level=logging.DEBUG)
if not bno.begin():
raise RuntimeError('Failed to initialize BNO055! Is the sensor connected?')
status, self_test, error = bno.get_system_status()
print('System status: {0}'.format(status))
print('Self test result (0x0F is normal): 0x{0:02X}'.format(self_test))
# Print out an error if system status is in error mode.
if status == 0x01:
print('System error: {0}'.format(error))
print('See datasheet section 4.3.59 for the meaning.')
# Calibrating the IMU
print("CALIBRATION VALUES BEFORE (sys, gyro, accel, mag): ")
print(bno.get_calibration_status())
bno.set_calibration(bno.get_calibration())
# Will be useful for the PID loop because the BNO does not start at 0 degrees .
x_quat_initial, y_quat_initial, z_quat_initial, w_quat_initial = bno.read_quaternion()
time.sleep(1)
x_quat_initial, y_quat_initial, z_quat_initial, w_quat_initial = bno.read_quaternion()
# CONFIGURATION OF BAROMETRIC CONTROLLER
i2c = busio.I2C(board.SCL, board.SDA)
bmp = adafruit_bmp3xx.BMP3XX_I2C(i2c)
bmp.pressure_oversampling = 8
bmp.temperature_oversampling = 2
# IMU Dict
imu_data = {
# uncommented is currently unused but may be valuable later
"heading": None,
"roll": None,
"pitch": None,
"sys": None,
"gyro": None,
"acceleration": None,
"mag": None,
"x_quaternion": None,
"y_quaternion": None,
"z_quaternion": None,
"w_quaternion": None,
"x_accelerometer": None,
"y_accelerometer": None,
"z_accelerometer": None,
'x_gravity': None,
'y_gravity': None,
'z_gravity': None,
}
# For comparing acceleration
other_variables = {
"average_acceleration": 0,
"past_average_acceleration": 0
}
# Barometric dict
barometric_dict = {
"pressure": None,
"temperature": None,
"altitude": None,
}
# Function to get average acceleration
def get_average_acceleration():
try:
average_acceleration = imu_data['x_accelerometer'] + imu_data['y_accelerometer'] + imu_data['z_accelerometer']
average_acceleration = average_acceleration / 3
other_variables['past_average_acceleration'] = other_variables['average_acceleration']
other_variables['average_acceleration'] = average_acceleration
except Exception as e:
pass
return
# PID loop setup
proportional = 0.0206310292526927
integral = 0.000965837060148333
derivative = 0.108213442401603
setpoint = 0
pid_x = PID(proportional, integral, derivative, setpoint=setpoint)
pid_y = PID(proportional, integral, derivative, setpoint=setpoint)
# Things to change:
# Use of heading, pitch, roll
# Servo1 and servo2
# PID and setpoint values
# Function for interacting with the PID loop
# Heading: z-axis
# Pitch: x-axis
# Roll: y-axis
# Will take in quaternion angles and then create euler angles
# def quaternion_to_euler_X():
# w = imu_data['w_quaternion'] - w_quat_initial
# x = imu_data['x_quaternion'] - x_quat_initial
# y = imu_data['y_quaternion'] - y_quat_initial
# z = imu_data['z_quaternion'] - z_quat_initial
#
# t0 = +2.0 * (w * x + y * z)
# t1 = +1.0 - 2.0 * (x * x + y * y)
# X = math.degrees(math.atan2(t0, t1))
# return X
#
#
# def quaternion_to_euler_Y():
# w = imu_data['w_quaternion'] - w_quat_initial
# x = imu_data['x_quaternion'] - x_quat_initial
# y = imu_data['y_quaternion'] - y_quat_initial
# z = imu_data['z_quaternion'] - z_quat_initial
#
# t2 = +2.0 * (w * y - z * x)
# t2 = +1.0 if t2 > +1.0 else t2
# t2 = -1.0 if t2 < -1.0 else t2
# Y = math.degrees(math.asin(t2))
# return Y
# takes in a list of quaternions (x, y, z, w)
def quaternion_to_euler():
q = [
imu_data['x_quaternion'] - x_quat_initial,
imu_data['y_quaternion'] - y_quat_initial,
imu_data['z_quaternion'] - z_quat_initial,
imu_data['w_quaternion'] - w_quat_initial
]
(x, y, z, w) = (q[0], q[1], q[2], q[3])
t0 = +2.0 * (w * x + y * z)
t1 = +1.0 - 2.0 * (x * x + y * y)
roll = math.atan2(t0, t1)
t2 = +2.0 * (w * y - z * x)
t2 = +1.0 if t2 > +1.0 else t2
t2 = -1.0 if t2 < -1.0 else t2
pitch = math.asin(t2)
t3 = +2.0 * (w * z + x * y)
t4 = +1.0 - 2.0 * (y * y + z * z)
heading = math.atan2(t3, t4)
roll = math.degrees(roll) * 4
pitch = math.degrees(pitch) * 4
heading = math.degrees(heading) * 4
return [heading, pitch, roll]
def pid_interact_x():
heading, pitch, roll = quaternion_to_euler()
output_x = pid_x(pitch)
print("EULER X ANGLE: " + str(pitch))
print("OUTPUT X: " + str(output_x))
servo_control.move_servo_1(output_x)
# input_x = quaternion_to_euler_X() # Is the X euler angle
# output_x = pid_x(input_x) # Is a degree for the motor to move
# print("EULER X ANGLE: " + str(input_x))
# print("OUTPUT_X: " + str(output_x))
# servo_control.move_servo_1(output_x)
def pid_interact_y():
heading, pitch, roll = quaternion_to_euler()
output_y = pid_y(roll)
print("EULER Y ANGLE: " + str(roll))
print("OUTPUT Y: " + str(output_y))
# input_y = quaternion_to_euler_Y() # Is the Y euler angle
# output_y = pid_y(input_y) # Is a degree for the motor to move
# print("EULER Y ANGLE: " + str(input_y))
# print("OUTPUT_Y: " + str(output_y))
# servo_control.move_servo_2(output_y)
# Setup for the button that will arm the rocket
# button_pin = 13
# GPIO.setmode(GPIO.BOARD)
# GPIO.setup(button_pin, GPIO.IN, pull_up_down=GPIO.PUD_DOWN)
# STAGES OF FLIGHT
# Loop before we enter power flight (ground idling at this point)
buzzer_pin = 17 # For BCM
button_pin = 27 # For BCM (13=27)
GPIO.setmode(GPIO.BCM)
GPIO.setup(button_pin, GPIO.IN, pull_up_down=GPIO.PUD_DOWN)
while ground_idle:
# Run the buzzer (set it up, buz, turn off, destroy, then repeat)
# GPIO.setmode(GPIO.BCM)
GPIO.setup(buzzer_pin, GPIO.OUT)
GPIO.output(buzzer_pin, GPIO.HIGH)
time.sleep(1)
GPIO.cleanup(buzzer_pin)
time.sleep(1)
# Check if the button has been pressed, if it has go to the next stage
if GPIO.input(button_pin) == GPIO.HIGH:
power_flight = True
ground_idle = False
print("BUTTON PRESSED - GOING TO POWERED FLIGHT")
# Cleanup the button and setup the buzzer to play a loud sound continuously now
GPIO.cleanup(button_pin)
GPIO.setup(buzzer_pin, GPIO.OUT)
GPIO.setup(buzzer_pin, GPIO.LOW)
while power_flight:
# GPIO.output(buzzer_pin, GPIO.LOW)
get_average_acceleration()
# Check if acceleration has fallen more than the accel level
if other_variables['past_average_acceleration'] - other_variables['average_acceleration'] >= ACCEL_LEVEL:
print("POWER FLIGHT FIRST CHECK SUCCESS, ACCEL FALLEN")
# Wait, then get acceleration and check if it changed again
time.sleep(WAIT)
imu_data['x_accelerometer'], imu_data['y_accelerometer'], imu_data['z_accelerometer'] = bno.read_accelerometer()
get_average_acceleration()
# Check again after waiting to make sure it's not a fluke and that the acceleration has changed
if other_variables['past_average_acceleration'] - other_variables['average_acceleration'] >= ACCEL_LEVEL:
power_flight = False
unpowered_flight = True
print("MOVING TO UNPOWERED FLIGHT FROM POWER FLIGHT")
break
else:
continue
else: # We did not trigger next stage, read/write data
imu_data['heading'], imu_data['roll'], imu_data['pitch'] = bno.read_euler()
imu_data['sys'], imu_data['gyro'], imu_data['acceleration'], imu_data['mag'] = bno.get_calibration_status()
imu_data['x_quaternion'], imu_data['y_quaternion'], imu_data['z_quaternion'], imu_data['w_quaternion'] = bno.read_quaternion()
imu_data['x_accelerometer'], imu_data['y_accelerometer'], imu_data['z_accelerometer'] = bno.read_accelerometer()
imu_data['x_gravity'], imu_data['y_gravity'], imu_data['z_gravity'] = bno.read_gravity()
barometric_dict = {
"pressure": bmp.pressure,
"temperature": bmp.temperature,
"altitude": bmp.altitude,
}
# Get timestamp for CSV file
timestamp = datetime.now()
# Write all the data to the CSV file
write_data_to_csv([
timestamp,
imu_data['heading'],
imu_data['roll'],
imu_data['pitch'],
imu_data['sys'],
imu_data['gyro'],
imu_data['acceleration'],
imu_data['mag'],
imu_data['x_quaternion'],
imu_data['y_quaternion'],
imu_data['z_quaternion'],
imu_data['w_quaternion'],
imu_data['x_accelerometer'],
imu_data['y_accelerometer'],
imu_data['z_accelerometer'],
imu_data['x_gravity'],
imu_data['y_gravity'],
imu_data['z_gravity'],
barometric_dict['pressure'],
barometric_dict['temperature'],
barometric_dict['altitude'],
], csv_number)
# Code to test out the servo
# servo_control.move_servo_2(25)
# servo_control.move_servo_1(-25)
pid_interact_x()
pid_interact_y()
while unpowered_flight:
altitude = 100 # Get altitude from barometer
cur_altitude = 90 # Get altitude from barometer
if cur_altitude < altitude: # We are falling now if true
unpowered_flight = False
ballistic_descent = True
print("MOVING TO BALLISTIC DESCENT FROM UNPOWERED FLIGHT")
break
else: # We did not trigger next stage, read/write data
imu_data['heading'], imu_data['roll'], imu_data['pitch'] = bno.read_euler()
imu_data['sys'], imu_data['gyro'], imu_data['acceleration'], imu_data['mag'] = bno.get_calibration_status()
imu_data['x_quaternion'], imu_data['y_quaternion'], imu_data['z_quaternion'], imu_data[
'w_quaternion'] = bno.read_quaternion()
imu_data['x_accelerometer'], imu_data['y_accelerometer'], imu_data['z_accelerometer'] = bno.read_accelerometer()
imu_data['x_gravity'], imu_data['y_gravity'], imu_data['z_gravity'] = bno.read_gravity()
barometric_dict = {
"pressure": bmp.pressure,
"temperature": bmp.temperature,
"altitude": bmp.altitude,
}
# Get timestamp for CSV file
timestamp = datetime.now()
# Write all the data to the CSV file
write_data_to_csv([
timestamp,
imu_data['heading'],
imu_data['roll'],
imu_data['pitch'],
imu_data['sys'],
imu_data['gyro'],
imu_data['acceleration'],
imu_data['mag'],
imu_data['x_quaternion'],
imu_data['y_quaternion'],
imu_data['z_quaternion'],
imu_data['w_quaternion'],
imu_data['x_accelerometer'],
imu_data['y_accelerometer'],
imu_data['z_accelerometer'],
imu_data['x_gravity'],
imu_data['y_gravity'],
imu_data['z_gravity'],
barometric_dict['pressure'],
barometric_dict['temperature'],
barometric_dict['altitude'],
], csv_number)
pid_interact_x()
pid_interact_y()
while ballistic_descent:
para_altitude = 10 # Get altitude from barometer
if para_altitude <= DEPLOY_CHUTE_ALTITUDE:
ballistic_descent = False
chute_descent = True
print("MOVING TO CHUTE DESCENT FROM BALLISTIC DESCENT")
break
else: # We did not trigger next stage, read/write data
imu_data['heading'], imu_data['roll'], imu_data['pitch'] = bno.read_euler()
imu_data['sys'], imu_data['gyro'], imu_data['acceleration'], imu_data['mag'] = bno.get_calibration_status()
imu_data['x_quaternion'], imu_data['y_quaternion'], imu_data['z_quaternion'], imu_data[
'w_quaternion'] = bno.read_quaternion()
imu_data['x_accelerometer'], imu_data['y_accelerometer'], imu_data['z_accelerometer'] = bno.read_accelerometer()
imu_data['x_gravity'], imu_data['y_gravity'], imu_data['z_gravity'] = bno.read_gravity()
barometric_dict = {
"pressure": bmp.pressure,
"temperature": bmp.temperature,
"altitude": bmp.altitude,
}
# Get timestamp for CSV file
timestamp = datetime.now()
# Write all the data to the CSV file
write_data_to_csv([
timestamp,
imu_data['heading'],
imu_data['roll'],
imu_data['pitch'],
imu_data['sys'],
imu_data['gyro'],
imu_data['acceleration'],
imu_data['mag'],
imu_data['x_quaternion'],
imu_data['y_quaternion'],
imu_data['z_quaternion'],
imu_data['w_quaternion'],
imu_data['x_accelerometer'],
imu_data['y_accelerometer'],
imu_data['z_accelerometer'],
imu_data['x_gravity'],
imu_data['y_gravity'],
imu_data['z_gravity'],
barometric_dict['pressure'],
barometric_dict['temperature'],
barometric_dict['altitude'],
], csv_number)
pid_interact_x()
pid_interact_y()
while chute_descent:
# Get average acceleration from x,y and z accel from IMU
acceleration = (imu_data['x_accelerometer'] + imu_data['y_accelerometer'] + imu_data['z_accelerometer'])/3
if acceleration <= STOPPED_ACCELERATION:
# TODO: Get acceleration level again before comparing
time.sleep(CHUTE_DESCENT_WAIT)
if acceleration <= STOPPED_ACCELERATION:
chute_descent = False
landing = True
print("MOVING TO LANDING :D")
break
else: # We did not trigger next stage, read/write data
imu_data['heading'], imu_data['roll'], imu_data['pitch'] = bno.read_euler()
imu_data['sys'], imu_data['gyro'], imu_data['acceleration'], imu_data['mag'] = bno.get_calibration_status()
imu_data['x_quaternion'], imu_data['y_quaternion'], imu_data['z_quaternion'], imu_data[
'w_quaternion'] = bno.read_quaternion()
imu_data['x_accelerometer'], imu_data['y_accelerometer'], imu_data['z_accelerometer'] = bno.read_accelerometer()
imu_data['x_gravity'], imu_data['y_gravity'], imu_data['z_gravity'] = bno.read_gravity()
barometric_dict = {
"pressure": bmp.pressure,
"temperature": bmp.temperature,
"altitude": bmp.altitude,
}
# Get timestamp for CSV file
timestamp = datetime.now()
# Write all the data to the CSV file
write_data_to_csv([
timestamp,
imu_data['heading'],
imu_data['roll'],
imu_data['pitch'],
imu_data['sys'],
imu_data['gyro'],
imu_data['acceleration'],
imu_data['mag'],
imu_data['x_quaternion'],
imu_data['y_quaternion'],
imu_data['z_quaternion'],
imu_data['w_quaternion'],
imu_data['x_accelerometer'],
imu_data['y_accelerometer'],
imu_data['z_accelerometer'],
imu_data['x_gravity'],
imu_data['y_gravity'],
imu_data['z_gravity'],
barometric_dict['pressure'],
barometric_dict['temperature'],
barometric_dict['altitude'],
], csv_number)
pid_interact_x()
pid_interact_y()
while landing:
landing = False # Remove
# Setting the servos back to 0 degrees
servo_control.move_servo_2(0)
servo_control.move_servo_1(0)