Revision 6:a7379a681adf, committed 2014-10-31

Comitter:

lauradeheus

Date:

Fri Oct 31 12:36:14 2014 +0000

Parent:

5:4842219cb77c

Child:

7:7e3e183bf063

Commit message:

Script motoraansturing (nog geen correcte PID waarden) en emgmeting werkend

Changed in this revision

--- a/main.cpp	Fri Oct 31 08:36:46 2014 +0000
+++ b/main.cpp	Fri Oct 31 12:36:14 2014 +0000
@@ -16,8 +16,9 @@
 #include "TextLCD.h"
 #include "mbed.h"
 #include "encoder.h"
-//#include "HIDScope.h"
+#include "HIDScope.h"
 #include "PwmOut.h"
+#include "arm_math.h"
 
 /*
 #define vaste waarden
@@ -53,11 +54,17 @@
 DigitalOut LEDGREEN(LED_GREEN);
 DigitalOut LEDRED(LED_RED); 
 Serial pc(USBTX,USBRX);
+HIDScope scope(3);
+AnalogIn    emg(PTB1);
 /*
 definieer namen aan var, float, int, static float, uint8_t, uint16_t etc. en geef ze eventueel een waarde
 */
 Ticker statemachine;
 Ticker screen;
+arm_biquad_casd_df1_inst_f32 lowpass_1; //2e orde lowpass biquad butterworthfilter 99Hz
+arm_biquad_casd_df1_inst_f32 lowpass_2; //2e orde lowpass biquad butterworthfilter 3Hz
+arm_biquad_casd_df1_inst_f32 highpass; //2e orde highpass biquad butterworthfilter 20Hz
+arm_biquad_casd_df1_inst_f32 notch; //2e orde lowpass biquad butterworthfilter 50Hz
 int previous_herhalingen = 0;
 int current_herhalingen = 0;
 int PWM2_percentage = 100; 
@@ -72,6 +79,9 @@
 int current_pos_motor1;
 int EMG = 1;
 int delta_pos_motor1_puls;
+int aantal_pieken;
+int doel;
+bool aanspan;
 void clamp(float * in, float min, float max);
 volatile bool looptimerflag;
 int16_t gewenste_snelheid = 2;
@@ -88,7 +98,19 @@
 float Speed_motor1;
 float Speed_motor1rad;
 float setpoint = 0;
-float prev_setpoint = 0; 
+float prev_setpoint = 0;
+float lowpass_1_const[] = {0.978030479206560 , 1.956060958413119 , 0.978030479206560 , -1.955578240315036 , -0.956543676511203};
+float lowpass_1_states[4];
+float lowpass_2_const[] = {0.002080567135492 , 0.004161134270985 , 0.002080567135492 , 1.866892279711715 , -0.875214548253684};
+float lowpass_2_states[4];
+float highpass_const[] = {0.638945525159022 , -1.277891050318045 ,  0.638945525159022 , 1.142980502539901 , -0.412801598096189};
+float highpass_states[4];
+float notch_const[] = {0.978048948305681 , 0.000000000000000 , 0.978048948305681 , 0.000000000000000 , -0.956097896611362};
+float notch_states[4];
+float emg_filtered;
+float emg_max = 0;
+float emg_treshhold_laag = 0;
+float emg_treshhold_hoog = 0;
 
 //HIDScope scope(6);
 
@@ -107,8 +129,58 @@
     motor2.setPosition(0);    
 }
 
+void pieken_tellen(){
+    if (emg_filtered>=emg_treshhold_hoog) 
+    {
+        aanspan=true; //maak een variabele waarin je opslaat dat het signaal hoog is.
+    }
+    if (aanspan==true && emg_filtered<=emg_treshhold_laag)//== ipv =, anders wordt aanspan true gemaakt
+    {
+        aanspan=false;
+        aantal_pieken++;
+        doel = aantal_pieken-((aantal_pieken/3)*3)+1;
+    }
+}
+
+void emg_filtering() {
+    uint16_t emg_value;
+    float emg_value_f32;
+    emg_value = emg.read_u16(); // read direct ADC result, converted to 16 bit integer (0..2^16 = 0..65536 = 0..3.3V)
+    emg_value_f32 = emg.read();
+    
+    arm_biquad_cascade_df1_f32(&highpass, &emg_value_f32, &emg_filtered, 1 );
+    arm_biquad_cascade_df1_f32(&lowpass_1, &emg_filtered, &emg_filtered, 1 );
+    arm_biquad_cascade_df1_f32(&notch, &emg_filtered, &emg_filtered, 1);
+    emg_filtered = fabs(emg_filtered);
+    arm_biquad_cascade_df1_f32(&lowpass_2, &emg_filtered, &emg_filtered, 1 );
+    scope.set(0,emg_value);     //uint value
+    scope.set(1,emg_filtered);  //processed float
+    scope.set(2,doel);
+    scope.send();
+    if(state!=EMG_KALIBRATIE)
+    {
+        pieken_tellen();
+    }
+}
+
+void emg_max_meting(){
+    emg_filtering();
+    if (emg_filtered>=emg_max)
+    {
+        emg_max=emg_filtered;
+    }
+    emg_treshhold_laag = 0.3*emg_max;
+    emg_treshhold_hoog = 0.7*emg_max;
+}
+
 void emg_kalibratie() {
-    current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
+    if(emg_filtered>=0.05){
+        current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
+    }
+    if(current_herhalingen<=1000)
+    {
+        emg_max_meting();
+    } 
 }
 
 void meten_richting() {
@@ -256,7 +328,7 @@
         case EMG_KALIBRATIE: 
         {
             emg_kalibratie();
-            if (current_herhalingen == 100) 
+            if (current_herhalingen >=1000) 
             {
                 state = METEN_RICHTING;
                 current_herhalingen = 0;
@@ -346,14 +418,43 @@
         lcd.locate(0,1);
         lcd.printf("  GROEP 7   ");
     }
+    else if(state==EMG_KALIBRATIE){
+        lcd.cls();
+        lcd.locate(0,0);
+        lcd.printf("Max. aanspannen");
+        if(current_herhalingen<=200){
+            lcd.locate(0,1); 
+            lcd.printf("nog 5 sec.");
+        }
+        else if(current_herhalingen<=400){
+            lcd.locate(0,1); 
+            lcd.printf("nog 4 sec.");
+        }
+        else if(current_herhalingen<=600){
+            lcd.locate(0,1); 
+            lcd.printf("nog 3 sec.");
+        }
+        else if(current_herhalingen<=800){
+            lcd.locate(0,1); 
+            lcd.printf("nog 2 sec.");
+        }
+        else if(current_herhalingen<=1000){
+            lcd.locate(0,1); 
+            lcd.printf("nog 1 sec.");
+        }
+    }
     else{     
         lcd.cls();
         lcd.printf("state %d", state); //hier nog aan toevoegen hoe je de 'waarde', dus eigenlijk tekst, die opgeslagen staat in state kan printen.
     }
 }
 
-int main() {
+int main(){
     pc.baud(115200);
+    arm_biquad_cascade_df1_init_f32(&lowpass_1,1 , lowpass_1_const, lowpass_1_states);
+    arm_biquad_cascade_df1_init_f32(&highpass,1 , highpass_const, highpass_states);
+    arm_biquad_cascade_df1_init_f32(&notch,1 , notch_const, notch_states);
+    arm_biquad_cascade_df1_init_f32(&lowpass_2,1 , lowpass_2_const, lowpass_2_states);
     statemachine.attach(&statemachinefunction, TSAMP); // the address of the function to be attached (flip) and the interval (2 seconds)
     screen.attach(&screenupdate, 0.2);
     while(1);

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-dsp.lib	Fri Oct 31 12:36:14 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/teams/mbed-official/code/mbed-dsp/#7a284390b0ce