-
Notifications
You must be signed in to change notification settings - Fork 4
/
rollsim.py
executable file
·99 lines (80 loc) · 2.26 KB
/
rollsim.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
#!/usr/bin/env python
import lv2
import numpy
from scipy.signal import resample
import matplotlib.pyplot as plt
import random
from PIDcontroller import PIDController
# define things
or_sample_rate = 820.0 # Hz
adis_sample_rate = 819.2 # Hz
# Read in from open rocket
columns = numpy.loadtxt('./openrocket/launch-12.csv', delimiter=',', unpack=True)
time = columns[0]
altitude = columns[1]*1000 # km to meters
velocity = columns[4]
# only care about launch to apogee
begin = 500
apogee = 25000
time = time[begin:apogee]
altitude = altitude[begin:apogee]
velocity = velocity[begin:apogee]
# Set up PID
pid = PIDController(p=50, i=0.009, d=0)
pid.setTarget(0.0)
# simulate roll
angular_accel = []
roll = [0]
angle = [0]
alpha = []
pids =[]
rand_stuff = []
for i, t in enumerate(time):
# current time step:
x = altitude[i]
v = velocity[i]
r = roll[-1]
#correction = pid.step(angle[-1])
correction = pid.step(r)
a = lv2.estimate_alpha(correction, x, v, t)
a = lv2.servo(a, t)
aa = lv2.angular_accel(a, x, v, t)
# step response:
#if t > 7 and t < 7.1:
# aa += 100
# random acceleration:
#aa_offset = 80 # ang acc offset in degs/s^2
aa_offset = v/2.0
aa_rand = random.gauss(aa_offset, 20) # random ang acc on the rocket
rand_stuff.append(aa_rand)
aa += aa_rand # add random torque to rocket
# next time step:
r += aa/or_sample_rate
roll.append(r)
ang = angle[-1] + r/or_sample_rate
angle.append(ang)
alpha.append(a)
angular_accel.append(aa)
pids.append(correction)
# chart
fig, (ax0, ax1, ax2, ax3, ax4) = plt.subplots(nrows=5, sharex=True, figsize=(16,9))
ax0.plot(time, angle[1:], 'g-')
ax0.set_title('Rocket Angle')
ax0.set_ylabel(r'Angle [${}^0$]')
ax1.plot(time, roll[1:], 'r-')
ax1.set_title('Roll Rate')
ax1.set_ylabel(r'Roll Rate [${}^0/s$]')
ax1.set_ylim([-5,10])
ax2.plot(time, angular_accel, 'r-')
ax2.set_title('Roll Accel')
ax2.set_ylabel(r'$\omega$ [${}^0/s/s$]')
ax2.set_ylim([-100,100])
ax3.plot(time, pids)
ax3.set_title('Correction')
ax3.set_ylabel(r'Correction')
ax4.plot(time, alpha)
ax4.set_title('Alpha')
ax4.set_ylabel(r'Alpha [${}^0$]')
ax4.set_xlabel(r'Time [$s$]')
plt.savefig('out.png', dpi=200)
plt.show()