Delen met projectgroep, Geen eindversie met comments

Dependencies:   QEI mbed

Fork of Project_Program_bijna_af by Yorick Fredrix

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers functions.h Source File

functions.h

00001 double buff0,buff1,buff2,buff3,buff4,buff5,buff6,buff7,buff8,buff9,buff10,buff11,buff12,buff13,buff14,buff15,buff16,buff17,buff18,buff19,buff20,buff21,buff22,buff23,buff24,buff25,buff26,buff27,buff28,buff29,buff30,buff31;
00002 double buffa0,buffa1,buffa2,buffa3,buffa4,buffa5,buffa6,buffa7,buffa8,buffa9,buffa10,buffa11,buffa12,buffa13,buffa14,buffa15,buffa16,buffa17,buffa18,buffa19,buffa20,buffa21,buffa22,buffa23,buffa24,buffa25,buffa26,buffa27,buffa28,buffa29,buffa30,buffa31;
00003 const double a1 = -1.73450428481, a2 = 0.76583463326, b0 = 1, b1 = -2, b2 = 1;
00004 const double aNF1 = -0.00000000587,aNF2 = 0.47335041769,aNF3 = -0.00000000293,aNF4 = 0.39273928249,aNF5 = -0.00000000089,aNF6 = 0.04513191993,bNF0 = 1 ,bNF1 = -0.00000001015,bNF2 = 3,bNF3 = -0.00000002029,bNF4 = 3,bNF5 = -0.00000001015,bNF6 = 1;
00005 double y=0,y2=0,yNF=0,yNF2=0;
00006 
00007 double MovingAverage1(double buff0)
00008 {
00009     buff31 = buff30;
00010     buff30 = buff29;
00011     buff29 = buff28;
00012     buff28 = buff27;
00013     buff27 = buff26;
00014     buff26 = buff25;
00015     buff25 = buff24;
00016     buff24 = buff23;
00017     buff23 = buff22;
00018     buff22 = buff21;
00019     buff21 = buff20;
00020     buff20 = buff19;
00021     buff19 = buff18;
00022     buff18 = buff17;
00023     buff17 = buff16;
00024     buff16 = buff15;
00025     buff15 = buff14;
00026     buff14 = buff13;
00027     buff13 = buff12;
00028     buff12 = buff11;
00029     buff11 = buff10;
00030     buff10 = buff9;
00031     buff9 = buff8;
00032     buff8 = buff7;
00033     buff7 = buff6;
00034     buff6 = buff5;
00035     buff5 = buff4;
00036     buff4 = buff3;
00037     buff3 = buff2;
00038     buff2 = buff1;
00039     buff1 = buff0;
00040     double buffTot = (buff0+buff1+buff2+buff3+buff4+buff5+buff6+buff7+buff8+buff9+buff10+buff11+buff12+buff13+buff14+buff15+buff16+buff17+buff18+buff19+buff20+buff21+buff22+buff23+buff24+buff25+buff26+buff27+buff28+buff29+buff30+buff31)/32.0;
00041     return buffTot;
00042 }
00043 double MovingAverage2(double buffa0)
00044 {
00045     buffa31 = buffa30;
00046     buffa30 = buffa29;
00047     buffa29 = buffa28;
00048     buffa28 = buffa27;
00049     buffa27 = buffa26;
00050     buffa26 = buffa25;
00051     buffa25 = buffa24;
00052     buffa24 = buffa23;
00053     buffa23 = buffa22;
00054     buffa22 = buffa21;
00055     buffa21 = buffa20;
00056     buffa20 = buffa19;
00057     buffa19 = buffa18;
00058     buffa18 = buffa17;
00059     buffa17 = buffa16;
00060     buffa16 = buffa15;
00061     buffa15 = buffa14;
00062     buffa14 = buffa13;
00063     buffa13 = buffa12;
00064     buffa12 = buffa11;
00065     buffa11 = buffa10;
00066     buffa10 = buffa9;
00067     buffa9 = buffa8;
00068     buffa8 = buffa7;
00069     buffa7 = buffa6;
00070     buffa6 = buffa5;
00071     buffa5 = buffa4;
00072     buffa4 = buffa3;
00073     buffa3 = buffa2;
00074     buffa2 = buffa1;
00075     buffa1 = buffa0;
00076     double buffaTot = (buffa0+buffa1+buffa2+buffa3+buffa4+buffa5+buffa6+buffa7+buffa8+buffa9+buffa10+buffa11+buffa12+buffa13+buffa14+buffa15+buffa16+buffa17+buffa18+buffa19+buffa20+buffa21+buffa22+buffa23+buffa24+buffa25+buffa26+buffa27+buffa28+buffa29+buffa30+buffa31)/32.0;
00077     return buffaTot;
00078 }
00079 double computeBiquad(double &u, double &v1, double &v2)
00080 {
00081     double v = u-a1*v1-a2*v2;
00082     y = b0*v+b1*v1+b2*v2;
00083     v2 = v1;
00084     v1 = v;
00085     return y;
00086 }
00087 
00088 double computeBiquadNF(double &y, double &vNF1, double &vNF2, double &vNF3, double &vNF4, double &vNF5, double &vNF6)
00089 {
00090     double v = y-aNF1*vNF1-aNF2*vNF2-aNF3*vNF3 -aNF4*vNF4 -aNF5*vNF5 -aNF6*vNF6 ;
00091     yNF = bNF0*v+bNF1*vNF1+bNF2*vNF2+bNF3*vNF3+bNF4*vNF4+bNF5*vNF5+bNF6*vNF6;
00092     vNF6 = vNF5;
00093     vNF5 = vNF4;
00094     vNF4 = vNF3;
00095     vNF3 = vNF2;
00096     vNF2 = vNF1;
00097     vNF1 = v;
00098     return yNF;
00099 }
00100 double computeBiquad2(double &u2, double &va1, double &va2)
00101 {
00102     double va = u2-a1*va1-a2*va2;
00103     y2 = b0*va+b1*va1+b2*va2;
00104     va2 = va1;
00105     va1 = va;
00106     return y2;
00107 }
00108 double computeBiquadNF2(double &y2, double &vaNF1, double &vaNF2, double &vaNF3, double &vaNF4, double &vaNF5, double &vaNF6)
00109 {
00110     double va = y2-aNF1*vaNF1-aNF2*vaNF2-aNF3*vaNF3 -aNF4*vaNF4 -aNF5*vaNF5 -aNF6*vaNF6 ;
00111     yNF2 = bNF0*va+bNF1*vaNF1+bNF2*vaNF2+bNF3*vaNF3+bNF4*vaNF4+bNF5*vaNF5+bNF6*vaNF6;
00112     vaNF6 = vaNF5;
00113     vaNF5 = vaNF4;
00114     vaNF4 = vaNF3;
00115     vaNF3 = vaNF2;
00116     vaNF2 = vaNF1;
00117     vaNF1 = va;
00118     return yNF2;
00119 }