def __init__(self): self.adc = adc.adc(0) self.GAIN = 1 # o co2 xreiazetai pwm gia tin lampa tou GPIO.setmode(GPIO.BCM) GPIO.setup(19, GPIO.OUT) self.p1 = GPIO.PWM(19, 4) GPIO.setup(13, GPIO.OUT) self.p2 = GPIO.PWM(13, 4) self.p1.start(50) self.p2.start(50)
def subf16_model(x, u, model, adjust_cy=True, multipliers=None): '''output aircraft state vector derivative for a given input The reference for the model is Appendix A of Stevens & Lewis if multipliers is not None, it should be a 7-tuple: xcg_mult, cxt_mult, cyt_mult, czt_mult, clt_mult, cmt_mult, cnt_mult xcg is x component of center of gravity (between 0.0 and 1.0, default 0.35) cxt is the x-axis aerodynamic force coefficient cyt is the sideforce coefficient czt is the z-axis force coefficient clt is a sum of the rolling moment coefficients cmt is the pitching moment coefficient cnt is a sum of the yawing moment coefficients ''' assert model == 'stevens' or model == 'morelli' assert len(x) == 13 assert len(u) == 4 assert multipliers is None or len(multipliers) == 7 xcg = 0.35 if multipliers is not None: xcg *= multipliers[0] thtlc, el, ail, rdr = u s = 300 b = 30 cbar = 11.32 rm = 1.57e-3 xcgr = .35 he = 160.0 c1 = -.770 c2 = .02755 c3 = 1.055e-4 c4 = 1.642e-6 c5 = .9604 c6 = 1.759e-2 c7 = 1.792e-5 c8 = -.7336 c9 = 1.587e-5 rtod = 57.29578 g = 32.17 xd = x.copy() vt = x[0] alpha = x[1] * rtod beta = x[2] * rtod phi = x[3] theta = x[4] psi = x[5] p = x[6] q = x[7] r = x[8] alt = x[11] power = x[12] # air data computer and engine model amach, qbar = adc(vt, alt) cpow = tgear(thtlc) xd[12] = pdot(power, cpow) t = thrust(power, alt, amach) dail = ail / 20 drdr = rdr / 30 # component build up if model == 'stevens': # stevens & lewis (look up table version) cxt = cx(alpha, el) cyt = cy(beta, ail, rdr) czt = cz(alpha, beta, el) clt = cl(alpha, beta) + dlda(alpha, beta) * dail + dldr(alpha, beta) * drdr cmt = cm(alpha, el) cnt = cn(alpha, beta) + dnda(alpha, beta) * dail + dndr(alpha, beta) * drdr else: # morelli model (polynomial version) cxt, cyt, czt, clt, cmt, cnt = Morellif16(alpha*pi/180, beta*pi/180, el*pi/180, ail*pi/180, rdr*pi/180, \ p, q, r, cbar, b, vt, xcg, xcgr) # multipliers adjustement if multipliers is not None: cxt *= multipliers[1] cyt *= multipliers[2] czt *= multipliers[3] clt *= multipliers[4] cmt *= multipliers[5] cnt *= multipliers[6] # add damping derivatives tvt = .5 / vt b2v = b * tvt cq = cbar * q * tvt # get ready for state equations d = dampp(alpha) cxt = cxt + cq * d[0] cyt = cyt + b2v * (d[1] * r + d[2] * p) czt = czt + cq * d[3] clt = clt + b2v * (d[4] * r + d[5] * p) cmt = cmt + cq * d[6] + czt * (xcgr - xcg) cnt = cnt + b2v * (d[7] * r + d[8] * p) - cyt * (xcgr - xcg) * cbar / b cbta = cos(x[2]) u = vt * cos(x[1]) * cbta v = vt * sin(x[2]) w = vt * sin(x[1]) * cbta sth = sin(theta) cth = cos(theta) sph = sin(phi) cph = cos(phi) spsi = sin(psi) cpsi = cos(psi) qs = qbar * s qsb = qs * b rmqs = rm * qs gcth = g * cth qsph = q * sph ay = rmqs * cyt az = rmqs * czt # force equations udot = r * v - q * w - g * sth + rm * (qs * cxt + t) vdot = p * w - r * u + gcth * sph + ay wdot = q * u - p * v + gcth * cph + az dum = (u * u + w * w) xd[0] = (u * udot + v * vdot + w * wdot) / vt xd[1] = (u * wdot - w * udot) / dum xd[2] = (vt * vdot - v * xd[0]) * cbta / dum # kinematics xd[3] = p + (sth / cth) * (qsph + r * cph) xd[4] = q * cph - r * sph xd[5] = (qsph + r * cph) / cth # moments xd[6] = (c2 * p + c1 * r + c4 * he) * q + qsb * (c3 * clt + c4 * cnt) xd[7] = (c5 * p - c7 * he) * r + c6 * (r * r - p * p) + qs * cbar * c7 * cmt xd[8] = (c8 * p - c2 * r + c9 * he) * q + qsb * (c4 * clt + c9 * cnt) # navigation t1 = sph * cpsi t2 = cph * sth t3 = sph * spsi s1 = cth * cpsi s2 = cth * spsi s3 = t1 * sth - cph * spsi s4 = t3 * sth + cph * cpsi s5 = sph * cth s6 = t2 * cpsi + t3 s7 = t2 * spsi - t1 s8 = cph * cth xd[9] = u * s1 + v * s3 + w * s6 # north speed xd[10] = u * s2 + v * s4 + w * s7 # east speed xd[11] = u * sth - v * s5 - w * s8 # vertical speed # outputs xa = 15.0 # sets distance normal accel is in front of the c.g. (xa = 15.0 at pilot) az = az - xa * xd[7] # moves normal accel in front of c.g. #################################### ###### peter additionls below ###### if adjust_cy: ay = ay + xa * xd[8] # moves side accel in front of c.g. # For extraction of Nz Nz = (-az / g) - 1 # zeroed at 1 g, positive g = pulling up Ny = ay / g return xd, Nz, Ny, az, ay
def compute(file, fs=0, time_res=0, amp_res=0, fmin=0, fmax=0, fcs=[], nb_filters=0, q=0, n=0, filters=[], filters_fq=[], ax=None, plotd=True, dbfs=False, spec_only=False, spec_xlim=False, drc_tl=False, drc_th=False, drc_r=False, adc_res=16, formants=[]): """ Exécute la chaîne de traitement dans sa totalité avec le fichier audio en paramètre. :param file: Nom du fichier audio à traiter ou liste d'amplitude :type file: string ou number :param fs: Fréquence d'échantillonage (uniquement si une liste d'amplitude est donnée pour le paramètre file) :type fs: number :param adc_res: Résolution du convertisseur analogique numérique :type adc_res: number :param drc_tl: Seuil bas du compresseur audio :type drc_tl: number :param drc_th: Seuil haut du compresseur audio :type drc_th: number :param drc_r: Taux de compression du compresseur audio :type drc_r: number :param fmin: Fréquence minimum :type fmin: number :param fmax: Fréquence maximum :type fmax: number :param fcs: Liste de fréquences centrales personnalisées (dans ce cas, les paramètres fmin, fmax et nb_filters sont ignorés) :type fcs: number[] :param nb_filters: Nombre de filtres :type nb_filters: number :param q: Facteur de qualité :type q: number :param n: Ordre du filtre :type n: number :param filters: Banque de filtre déjà générée (dans ce cas, les paramètres de génération de filtres sont ignorés) :type filters: filtre[] :param filters_fq: Listes d'objets contenant "fc", "fl" et "fh" indiquant les fréquences caractéristiques du filtre associé :type filters_fq: object("fc", "fl", "fh")[] :param time_res: Résolution temporelle :type time_res: number :param amp_res: Résolution en amplitude :type amp_res: number :param formants: Liste des formants à tracer sur la figure ("a", "e", "i", "o", "u") :type formants: string[] :param ax: Surface de dessin existante (une nouvelle figure sera crée si aucune n'est donnée en paramètre) :type ax: figure :param plot_d: Si actif, affiche le spectrogramme de chaque fichier traité :type plot_d: bool :param spec_only: Si actif, affiche uniquement le spectrogramme sur mesure (dans ce cas, précisez un titre) :type spec_only: string :param spec_xlim: Modifie la limite supérieure de l'axe des abscisses du spectrogramme :type spec_xlim: number :param dbfs: Affiche le spectre db FS :type dbfs: boolean :return: Liste des segments temporels, liste des fréquences et liste des séquences d'énergies :rtype: number[], number[], number[][] """ # Récupération du fichier audio et génération du bruit (si précisé) if type(file) == list: if (type(file[0]) == str): fs, y = sw.read(file[0]) y = np.array(y) for i in range(1, len(file)): d, noise = sw.read(file[i]) for j in range(0, min(len(y), len(noise))): y[j] = y[j] + noise[j] else: y = file # Récupération du fichier audio elif type(file) == str: fs, y = sw.read(file) else: y = file N = len(y) t = np.linspace(0, N / fs, N) # Compresseur audio if drc_r != False: y = drc(y, tl=drc_tl, th=drc_th, ratio=drc_r) # Convertisseur analogique numérique if adc_res < 16: y = adc(y, adc_res) # Filtrage if ((nb_filters > 0) or (len(fcs) > 0)): filters, filters_fq = gen_filters(q, n, fs, nb_filters=nb_filters, fmin=fmin, fmax=fmax, fcs=fcs) filtered = gen_filtered(y, fs, filters) # Spectrogramme rsegs, rfreqs, rseqs = gen_data(filtered, fs, time_res, amp_res, filters_fq) # Suppression du silence au début de l'échantillon rsum = np.sum(rseqs, axis=0) for i in range(len(rsum)): if rsum[i] != 0: break if spec_only: rsegs = np.delete(rsegs, range(len(rsegs) - i, len(rsegs))) else: rsegs = np.delete(rsegs, range(0, i)) rseqs = np.delete(rseqs, range(0, i), 1) # Affichage if plotd: if spec_only: plot_datagram(rsegs, rfreqs, rseqs, title=spec_only, xlim=spec_xlim, formants=formants) else: plot_data(y, t, rsegs, rfreqs, rseqs, ax=ax, xlim=spec_xlim, dbfs=dbfs, formants=formants) return rsegs, rfreqs, rseqs
def main(): global pylab #LCD dimensions width = 128 height = 160 lcdDPI = 80 # intermediate file used to draw the graph imageFileName = "/tmp/lcdSonarGraph.png" # set up the range finder # ultrasound #s = srf02.srf02() title = "Pi sonar" # IR s = adc.adc() title = "Pi iRanger" # set up pygame to use the framebuffer os.environ["SDL_FBDEV"] = "/dev/fb1" pygame.init() pygame.font.init() pygame.mouse.set_visible(0) screen = pygame.display.set_mode((width, height)) bigFont = pygame.font.Font(None, 50) littleFont = pygame.font.Font(None, 20) # set up matplotlib. size is in inches and dpi. # the dimensions are reversed so we can rotate the image through 90 degrees to show it landscape myFig = pylab.figure(figsize=(float(height) / lcdDPI, float(width) / lcdDPI), dpi=lcdDPI) axes = myFig.add_subplot(111) axes.yaxis.set_ticks((0, 30, 60, 90, 120, 150, 180)) axes.xaxis.set_visible(False) valueHistory = [] timeHistory = [] for i in range(100): # clear the screen screen.fill([0, 0, 0]) # single pings for now, the SRF doesn't need smoothing values = s.getValues(1) # add the new value to the chart's arrays valueHistory.append(values[0]["distance"]) timeHistory.append(i) if (i % 8 == 0): # update the chart with the newest values. this takes a couple of seconds so we only do it every few pings axes.plot(timeHistory, valueHistory, linewidth=1.0, color='red') myFig.savefig(imageFileName, dpi=80, bbox_inches='tight', format='png') chartImg = pygame.image.load(imageFileName) chartImg = pygame.transform.rotate(chartImg, 90) # otherwise just draw the old image screen.blit(chartImg, (0, 0)) # we should probably only draw this label once and only blank the bit of the screen that changes # this should be yellow but my LCD's blue and red signals are swapped labelText = littleFont.render(title, True, (0, 215, 255, 0)) labelText = pygame.transform.rotate(labelText, 90) labelTextPosition = labelText.get_rect() labelTextPosition.centery = 40 labelTextPosition.centerx = 123 screen.blit(labelText, labelTextPosition) # draw the result to the screen in jumbo text rangeText = bigFont.render( str(values[0]["distance"]) + "cm", True, (0, 140, 0, 0)) rangeText = pygame.transform.rotate(rangeText, 90) rangeTextPosition = rangeText.get_rect() rangeTextPosition.centery = 75 rangeTextPosition.centerx = 40 screen.blit(rangeText, rangeTextPosition) # maybe track dirty rectangles? or try double buffering and use .flip()? pygame.display.update() time.sleep(0.25) raw_input('any key?') #myImage = pygame.image.load('pi.png') #for j in range(0, 720, 10): # rotated = pygame.transform.rotate(myImage, j) # screen.blit(blank, (0,0)) # screen.blit(rotated, (0,0)) # pygame.display.update() pygame.quit()
def main(): global pylab #LCD dimensions width = 128 height = 160 lcdDPI = 80 # intermediate file used to draw the graph imageFileName = "/tmp/lcdSonarGraph.png" # set up the range finder # ultrasound #s = srf02.srf02() title = "Pi sonar" # IR s = adc.adc() title = "Pi iRanger" # set up pygame to use the framebuffer os.environ["SDL_FBDEV"] = "/dev/fb1" pygame.init() pygame.font.init() pygame.mouse.set_visible(0) screen = pygame.display.set_mode((width, height)) bigFont = pygame.font.Font(None, 50) littleFont = pygame.font.Font(None, 20) # set up matplotlib. size is in inches and dpi. # the dimensions are reversed so we can rotate the image through 90 degrees to show it landscape myFig = pylab.figure(figsize = (float(height) / lcdDPI, float(width) / lcdDPI), dpi = lcdDPI) axes = myFig.add_subplot(111) axes.yaxis.set_ticks((0, 30, 60, 90, 120, 150, 180)) axes.xaxis.set_visible(False) valueHistory = [] timeHistory = [] for i in range(100): # clear the screen screen.fill([0, 0, 0]) # single pings for now, the SRF doesn't need smoothing values = s.getValues(1) # add the new value to the chart's arrays valueHistory.append(values[0]["distance"]) timeHistory.append(i) if (i % 8 == 0): # update the chart with the newest values. this takes a couple of seconds so we only do it every few pings axes.plot(timeHistory, valueHistory, linewidth=1.0, color='red') myFig.savefig(imageFileName, dpi=80, bbox_inches = 'tight', format = 'png') chartImg = pygame.image.load(imageFileName) chartImg = pygame.transform.rotate(chartImg, 90) # otherwise just draw the old image screen.blit(chartImg, (0,0)) # we should probably only draw this label once and only blank the bit of the screen that changes # this should be yellow but my LCD's blue and red signals are swapped labelText = littleFont.render(title, True, (0, 215, 255, 0)) labelText = pygame.transform.rotate(labelText, 90) labelTextPosition = labelText.get_rect() labelTextPosition.centery = 40 labelTextPosition.centerx = 123 screen.blit(labelText, labelTextPosition) # draw the result to the screen in jumbo text rangeText = bigFont.render(str(values[0]["distance"]) + "cm", True, (0, 140, 0, 0)) rangeText = pygame.transform.rotate(rangeText, 90) rangeTextPosition = rangeText.get_rect() rangeTextPosition.centery = 75 rangeTextPosition.centerx = 40 screen.blit(rangeText, rangeTextPosition) # maybe track dirty rectangles? or try double buffering and use .flip()? pygame.display.update() time.sleep(0.25) raw_input('any key?') #myImage = pygame.image.load('pi.png') #for j in range(0, 720, 10): # rotated = pygame.transform.rotate(myImage, j) # screen.blit(blank, (0,0)) # screen.blit(rotated, (0,0)) # pygame.display.update() pygame.quit()
def trimmerFun(orient, inputs, printOn, Xcg=0.35, model='stevens', adjust_cy=False): # def trimmerFun(Xguess, Uguess, orient, inputs, printOn, Xcg=0.35, model='stevens', adjust_cy=True): 'calculate equilibrium state' # assert isinstance(Xguess, np.ndarray) # assert isinstance(Uguess, np.ndarray) # assert isinstance(inputs, np.ndarray) Xguess = np.array( [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) Uguess = np.array([0.0, 0.0, 0.0, 0.0]) x = Xguess.copy() u = Uguess.copy() xcg = Xcg if printOn: print('------------------------------------------------------------') print('Running trimmerFun.py') # gamma singam rr pr tr phi cphi sphi thetadot coord stab orient const = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1] rtod = 57.29577951 # orient: 'Wings Level (gamma = 0)','Wings Level (gamma <> 0)','Steady Turn','Steady Pull Up' const[11] = orient # inputs: [Vt, h, gamm, psidot, thetadot] x[0] = inputs[0] x[11] = inputs[1] if orient == 2: gamm = inputs[2] const[0] = gamm / rtod const[1] = sin(const[0]) elif orient == 3: psidot = inputs[3] const[4] = psidot / rtod # tr = turn tr = const[4] gamm = inputs[2] const[0] = gamm phi = turn_coord_cons(tr, x[1], x[2], x[0], gamma=gamm) const[5] = phi const[6] = sin(phi) const[7] = cos(phi) x[4] = rate_of_climb_cons(gamm, x[1], x[2], phi) elif orient == 4: thetadot = inputs[4] const[8] = thetadot / rtod if orient == 3: s = np.zeros(shape=(5, )) s[0] = x[1] s[1] = u[0] s[2] = u[1] s[3] = u[2] s[4] = u[3] else: # for orient 1, 2, 4 s = np.zeros(shape=(3, )) s[0] = u[0] s[1] = u[1] s[2] = x[1] if printOn: print(f"initial cost = {clf16(s, x, u, xcg, const, model, adjust_cy)}") # #=== MINIMIZE Algorithm ============================================================= # maxiter = 1000 # tol = 1e-7 # minimize_tol = 1e-9 #1e-9 # res = minimize(clf16, s, args=(x, u, xcg, const, model, adjust_cy), method='Nelder-Mead', tol=minimize_tol, \ # options={'maxiter': maxiter}) # cost = res.fun # #=================================================================================== ##=== FMIN Algorithm ================================================================ s = fmin(clf16, s, args=(x, u, xcg, const, model, adjust_cy), xtol=1e-12, maxiter=2000) J = clf16(s, x, u, xcg, const, model, adjust_cy) if printOn: print(f"cost = {J}") if orient != 3: x[1] = s[2] u[0] = s[0] u[1] = s[1] else: x[1] = s[0] u[0] = s[1] u[1] = s[2] u[2] = s[3] u[3] = s[4] ##=================================================================================== if printOn: print(f'Throttle (percent): {u[0]}') print(f'Elevator (deg): {u[1]}') print(f'Ailerons (deg): {u[2]}') print(f'Rudder (deg): {u[3]}') print(f'Angle of Attack (deg): {rtod*x[1]}') print(f'Sideslip Angle (deg): {rtod*x[2]}') print(f'Pitch Angle (deg): {rtod*x[4]}') print(f'Bank Angle (deg): {rtod*x[3]}') amach, qbar = adc(x[0], x[11]) print(f'Dynamic Pressure (psf): {qbar}') print(f'Mach Number: {amach}') # print('') # print(f'Cost Function: {cost}') # assert cost < tol, "trimmerFun did not converge" return x, u
def trimmerFun(Xguess, Uguess, orient, inputs, printOn, model='stevens', adjust_cy=True): 'calculate equilibrium state' assert isinstance(Xguess, np.ndarray) assert isinstance(Uguess, np.ndarray) assert isinstance(inputs, np.ndarray) x = Xguess.copy() u = Uguess.copy() if printOn: print '------------------------------------------------------------' print 'Running trimmerFun.m' # gamma singam rr pr tr phi cphi sphi thetadot coord stab orient const = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1] rtod = 57.29577951 # orient: 'Wings Level (gamma = 0)','Wings Level (gamma <> 0)','Steady Turn','Steady Pull Up' const[11] = orient # inputs: [Vt, h, gamm, psidot, thetadot] x[0] = inputs[0] x[11] = inputs[1] if orient == 2: gamm = inputs[2] const[0] = gamm / rtod const[1] = sin(const[0]) elif orient == 3: psidot = inputs[3] const[4] = psidot / rtod elif orient == 4: thetadot = inputs[4] const[8] = thetadot / rtod if orient == 3: s = np.zeros(shape=(7, )) s[0] = u[0] s[1] = u[1] s[2] = u[2] s[3] = u[3] s[4] = x[1] s[5] = x[3] s[6] = x[4] else: s = np.zeros(shape=(3, )) s[0] = u[0] s[1] = u[1] s[2] = x[1] maxiter = 1000 tol = 1e-7 minimize_tol = 1e-9 res = minimize(clf16, s, args=(x, u, const, model, adjust_cy), method='Nelder-Mead', tol=minimize_tol, \ options={'maxiter': maxiter}) cost = res.fun if printOn: print 'Throttle (percent): {}'.format(u[0]) print 'Elevator (deg): {}'.format(u[1]) print 'Ailerons (deg): {}'.format(u[2]) print 'Rudder (deg): {}'.format(u[3]) print 'Angle of Attack (deg): {}'.format(rtod * x[1]) print 'Sideslip Angle (deg): {}'.format(rtod * x[2]) print 'Pitch Angle (deg): {}'.format(rtod * x[4]) print 'Bank Angle (deg): {}'.format(rtod * x[3]) amach, qbar = adc(x[0], x[11]) print 'Dynamic Pressure (psf): {}'.format(qbar) print 'Mach Number: {}'.format(amach) print '' print 'Cost Function: {}'.format(cost) assert cost < tol, "trimmerFun did not converge" return x, u
def plot_nspecgram(file, filters_fq, ax=None, title="Spectrogramme", xlim=False, formants=[], drc_tl=False, drc_th=False, drc_r=False, adc_res=16): """ Affiche le spectrogramme natif. :param file: Nom du fichier :type file: string :param filters_fq: Listes d'objets contenant "fc", "fl" et "fh" indiquant les fréquences caractéristiques du filtre associé :type filters_fq: object("fc", "fl", "fh")[] :param ax: Surface de dessin existante (une nouvelle figure sera crée si aucune n'est donnée en paramètre) :type ax: figure :param title: Titre :type title: string :param xlim: Modifie la limite supérieure de l'axe des abscisses du spectrogramme :type xlim: number :param formants: Liste des formants à tracer sur la figure ("a", "e", "i", "o", "u") :type formants: string[] :param adc_res: Résolution du convertisseur analogique numérique :type adc_res: number :param drc_tl: Seuil bas du compresseur audio :type drc_tl: number :param drc_th: Seuil haut du compresseur audio :type drc_th: number :param drc_r: Taux de compression du compresseur audio :type drc_r: number """ # Initialisation fs, y = sw.read(file) if type(ax) == type(None): plt.figure(figsize=(12, 4), dpi=80, facecolor="w", edgecolor="k") ax = plt.subplot(111) # Compresseur audio if drc_r != False: y = drc(y, tl=drc_tl, th=drc_th, ratio=drc_r) # Convertisseur analogique numérique y = adc(y, adc_res) # Spectrogramme (fonction native) F, T, Sxx = signal.spectrogram(y, fs) ax.set_title(title) ax.set_xlabel("Time [s]") ax.set_ylabel("Frequency [Hz]") ax.pcolormesh(T, F, Sxx, cmap="magma") ax.set_facecolor("black") # Fréquences rfreqs = [] for i in range(len(filters_fq)): rfreqs.append(filters_fq[i]["fc"]) ax.set_ylim(rfreqs[0], rfreqs[-1]) ax.set_yscale("log") ax.set_yticks(rfreqs) ax.get_yaxis().set_major_formatter(ticker.ScalarFormatter()) # Formants if len(formants) > 0: plot_formants(formants, rfreqs, ax, F[0]) plt.show()
def __init__(self): self.adc = adc.adc(1) self.GAIN = 1
# Imports from adc import adc import numpy as np # Problem size N = 1000 # Initialise random input vecD0 = np.random.rand(N) vecD1 = np.random.rand(N - 1) x = np.random.rand(N) # Run adc and print output print(adc(vecD0, vecD1, x))