intergreren van de twee scriptjes (emgfilters en PID controller). kijken of de motor aan te sturen is met emg-signalen
Dependencies: Encoder HIDScope MODSERIAL biquadFilter mbed
main.cpp
00001 //libaries 00002 #include "mbed.h" 00003 #include "BiQuad.h" 00004 #include "HIDScope.h" 00005 #include "encoder.h" 00006 #include "MODSERIAL.h" 00007 00008 00009 //globale variabelen FILTERS 00010 00011 //Hidscope aanmaken 00012 HIDScope scope(2); 00013 double maxi = 0.12; // max signal after filtering, 0.1-0.12 00014 00015 // Biquad filters van respectievelijk Notch, High-pass en Low-pass filter 00016 BiQuad N1( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 ); 00017 BiQuadChain NF; 00018 BiQuad HP1( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 ); 00019 BiQuad HP2( 1.00000e+00, -2.00001e+00, 1.00001e+00, -1.96161e+00, 9.63007e-01 ); 00020 BiQuadChain HPF; 00021 BiQuad LP1( 2.56971e-06, 2.56968e-06, 0.00000e+00, -9.72729e-01, 0.00000e+00 ); 00022 BiQuad LP2( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 ); 00023 BiQuadChain LPF; 00024 00025 float f = 500; // frequency 00026 float dt = 1/f; // sample frequency 00027 Ticker emgverwerkticker; 00028 AnalogIn emg(A0); // EMG lezen 00029 00030 // globale variabelen PID controller 00031 00032 Ticker AInTicker; //We make a ticker named AIn (use for HIDScope) 00033 00034 Ticker Treecko; //We make a awesome ticker for our control system 00035 //AnalogIn potMeter2(A1); //Analoge input of potmeter 2 (will be use for te reference position) --> emgFiltered 00036 PwmOut M1E(D6); //Biorobotics Motor 1 PWM control of the speed 00037 DigitalOut M1D(D7); //Biorobotics Motor 1 diraction control 00038 00039 Encoder motor1(D13,D12,true); 00040 MODSERIAL pc(USBTX,USBRX); 00041 00042 float PwmPeriod = 1.0/5000.0; //set up of PWM periode (5000 Hz, want 5000 periodes in 1 seconde) 00043 const float Ts = 0.1; // tickettijd/ sample time 00044 float e_prev = 0; 00045 float e_int = 0; 00046 float refP_prev = 0; 00047 00048 //FILTERS 00049 /*void emgverwerk () 00050 { 00051 double emgNotch = NF.step(emg.read() ); // Notch filter 00052 double emgHP = HPF.step(emgNotch); // High-pass filter: also normalises around 0. 00053 double emgAbsHP = abs(emgHP); // Take absolute value 00054 double emgLP = LPF.step(emgAbsHP); // Low-pass filter: creates envelope 00055 double emgMax = maxi; //(emgLP); // moet waarde 'schatten' voor de max, want je leest de data live. voorbeeld: 0.1, maar mogelijk 0.2 kiezen voor veiligheidsfactor. Dan gaat motor alleen maximaal op 1/2 vermogen. 00056 double emgFiltered = emgLP/emgMax; // Scale to maximum signal: useful for motor 00057 if (emgFiltered >1) 00058 { 00059 emgFiltered=1.00; 00060 } 00061 scope.set(0,emgFiltered); 00062 scope.set(1,emg.read()); 00063 scope.send(); 00064 } 00065 */ 00066 00067 00068 // PID CONTROLLER 00069 float GetReferencePosition(double emgFilt, double &refP_prev ) // anders met emg 00070 { 00071 double refP; 00072 if (.45<emgFilt<.80) 00073 { 00074 refP= refP_prev + 0.001; 00075 } 00076 else if (emgFilt>=.80) 00077 { 00078 refP= refP_prev + 0.04; 00079 } 00080 else 00081 { } 00082 00083 int maxwaarde = 4096; // = 64x64 aantal posities die de motor kan hebben 00084 refP = refP*maxwaarde; 00085 return refP; // value between 0 and 4096 00086 } 00087 00088 float FeedBackControl(float error, float &e_prev, float &e_int) // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking) 00089 { 00090 float kp = 0.001; // kind of scaled. 00091 float Proportional= kp*error; 00092 00093 float kd = 0.0004; // kind of scaled. 00094 float VelocityError = (error - e_prev)/Ts; 00095 float Derivative = kd*VelocityError; 00096 e_prev = error; 00097 00098 float ki = 0.00005; // kind of scaled. 00099 e_int = e_int+Ts*error; 00100 float Integrator = ki*e_int; 00101 00102 00103 float motorValue = Proportional + Integrator + Derivative; 00104 return motorValue; 00105 } 00106 00107 void SetMotor1(float motorValue) 00108 { 00109 if (motorValue >= 0) 00110 { 00111 M1D = 0; 00112 } 00113 else 00114 { 00115 M1D = 1; 00116 } 00117 00118 if (fabs(motorValue) > 1) 00119 { 00120 M1E = 1; //de snelheid wordt teruggeschaald naar 8.4 rad/s (maximale snelheid, dus waarde 1) 00121 } 00122 else 00123 { 00124 M1E = fabs(motorValue); //de absolute snelheid wordt bepaald, de motor staat uit bij een waarde 0 00125 } 00126 } 00127 00128 float Encoder () 00129 { 00130 float Huidigepositie = motor1.getPosition (); 00131 return Huidigepositie; // huidige positie = current position 00132 } 00133 00134 void MeasureAndControl(void) 00135 { 00136 //emgverwerken 00137 double emgNotch = NF.step(emg.read() ); // Notch filter 00138 double emgHP = HPF.step(emgNotch); // High-pass filter: also normalises around 0. 00139 double emgAbsHP = abs(emgHP); // Take absolute value 00140 double emgLP = LPF.step(emgAbsHP); // Low-pass filter: creates envelope 00141 double emgMax = maxi; //(emgLP); // moet waarde 'schatten' voor de max, want je leest de data live. voorbeeld: 0.1, maar mogelijk 0.2 kiezen voor veiligheidsfactor. Dan gaat motor alleen maximaal op 1/2 vermogen. 00142 double emgFiltered = emgLP/emgMax; // Scale to maximum signal: useful for motor 00143 if (emgFiltered >1) 00144 { 00145 emgFiltered=1.00; 00146 } 00147 scope.set(0,emgFiltered); 00148 scope.set(1,emg.read()); 00149 scope.send(); 00150 00151 // hier the control of the control system 00152 float refP = GetReferencePosition(emgFiltered, refP_prev); 00153 float Huidigepositie = Encoder(); 00154 float error = (refP - Huidigepositie);// make an error 00155 float motorValue = FeedBackControl(error, e_prev, e_int); 00156 SetMotor1(motorValue); 00157 } 00158 00159 00160 int main() 00161 { 00162 NF.add( &N1 ); 00163 HPF.add( &HP1 ).add( &HP2 ); 00164 LPF.add( &LP1 ).add( &LP2 ); 00165 //emgverwerkticker.attach(&emgverwerk, dt); 00166 00167 M1E.period(PwmPeriod); 00168 Treecko.attach(MeasureAndControl, dt); //Ts); //Elke 1 seconde zorgt de ticker voor het runnen en uitlezen van de verschillende 00169 //functies en analoge signalen. Veranderingen worden elke 1 seconde doorgevoerd. 00170 00171 while(1) 00172 { 00173 wait(0.2); 00174 pc.baud(115200); 00175 float B = motor1.getPosition(); 00176 //float Potmeterwaarde = potMeter2.read(); 00177 //float positie = B%4096; 00178 //pc.printf("pos: %d, speed %f, potmeter = %f V, \r\n",motor1.getPosition(), motor1.getSpeed(),(potMeter2.read()*3.3)); //potmeter uitlezen. tussen 0-1. voltage, dus *3.3V 00179 } 00180 }
Generated on Fri Jul 15 2022 15:43:20 by 1.7.2