Diff: main.cpp

Revision:

19:1fd39a2afc30

Parent:

15:ad065ab92d11

Child:

20:a6a5bdd7d118

--- a/main.cpp	Tue Oct 15 13:17:49 2019 +0000
+++ b/main.cpp	Thu Oct 24 09:25:27 2019 +0000
@@ -21,9 +21,68 @@
 
 AnalogIn EMG_biceps_right_raw (A0);
 AnalogIn EMG_biceps_left_raw (A1);
-Analogin EMG_calf_raw (A2);
+AnalogIn EMG_calf_raw (A2);
 
 Ticker loop_ticker;
+<<<<<<< working copy
+Ticker HIDScope_ticker;
+Ticker emgSampleTicker;
+
+HIDScope scope(3);
+
+BiQuadChain bqc; // Let op !!! Deze coëfficiënten moeten nog worden veranderd
+BiQuad bq1(0.0030, 0.0059, 0.0030, -1.8404,0.8522); //voor low-pass
+BiQuad bq2(0.9737, -1.9474, 0.9737, -1.9467, 0.9481); //voor high-pass
+BiQuad bq3(0.9912, -1.9823,0.9912, -1.9822, 0.9824); //lage piek eruit-> voor coëfficienten, zie matlab
+
+bool calib = false; // MOGELIJK GAAT HET HIER FOUT
+int i_calib = 0;
+
+void emgSampleFilter() // Deze functie wordt aangeroepen dmv een ticker. Het sampled 
+                       // hierdoor het EMG signaal en het haalt er een filter overheen
+{   
+    float filtered_EMG_biceps_right=bqc.step(EMG_biceps_right_raw.read());
+    float filtered_EMG_biceps_left=bqc.step(EMG_biceps_left_raw.read());
+    float filtered_EMG_calf=bqc.step(EMG_calf_raw.read());
+    
+    float filtered_EMG_biceps_right_total=filtered_EMG_biceps_right;
+    float filtered_EMG_biceps_left_total=filtered_EMG_biceps_left;
+    float filtered_EMG_calf_total=filtered_EMG_calf;
+    
+    if (calib)
+        {   
+            if (i_calib < 500)
+                {
+                    filtered_EMG_biceps_right_total=filtered_EMG_biceps_right+filtered_EMG_biceps_right_total;
+                    filtered_EMG_biceps_left_total=filtered_EMG_biceps_left+filtered_EMG_biceps_left_total;
+                    filtered_EMG_calf_total=filtered_EMG_calf+filtered_EMG_calf_total;
+                    i_calib++;
+                }
+            if (i_calib >= 500)
+                {   
+                    mean_EMG_biceps_right=filtered_EMG_biceps_right_total/500;
+                    mean_EMG_biceps_left=filtered_EMG_biceps_left_total/500;
+                    mean_EMG_calf=filtered_EMG_calf_total/500;
+                    calib = false; 
+                }
+         }
+}
+
+void sendHIDScope() // Deze functie geeft de gefilterde EMG-signalen weer in de HIDScope
+                    // Wordt eveneens gerund dmv een ticker
+{
+    /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
+    scope.set(0, filtered_EMG_biceps_right() ); // Werkt dit zo? Of nog met .read?
+    scope.set(1, filtered_EMG_biceps_left() );
+    scope.set(2, filtered_EMG_calf() );
+    /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels) 
+    *  Ensure that enough channels are available (HIDScope scope( 2 ))
+    *  Finally, send all channels to the PC at once */
+    scope.send();
+    // Eventueel nog een ledje laten branden
+}
+=======
+>>>>>>> destination
 
 // Emergency    
 void emergency()
@@ -54,6 +113,25 @@
         motor1_dir.write(1);
         pc.printf("Motoren aan functie\r\n");
     }
+<<<<<<< working copy
+        
+// EMG kalibreren        
+void  emg_calibration()   
+     {
+        // Gedurende bijv. 5 seconden EMG meten, wanneer de spieren maximaal
+        // worden aangespannen -> maximaal potentiaal verkrijgen. Een fractie 
+        // hiervan kan als drempel worden gebruikt voor beweging
+        
+        // *Tijd instellen* 
+        // Iets met DOUBLE_MAX? https://docs.microsoft.com/en-us/cpp/c-language/cpp-integer-limits?view=vs-2019
+        
+        // Ledje van kleur laten veranderen
+        
+        // MOGELIJK NIET MEER NODIG???
+       
+     }
+=======
+>>>>>>> destination
 
 // Finite state machine programming (calibration servo motor?)
 enum states {Motors_off, Calib_motor, Calib_EMG, Homing, Operation_mode};
@@ -107,10 +185,21 @@
             if (stateChanged)
             {
                 // Hierbij wordt een een kalibratie uitgevoerd, waarbij de maximale EMG-amplitude waarde wordt bepaald
+<<<<<<< working copy
+                calib = true;
+                emgSampleFilter() // Gaat dit nu goed? -> moet sws worden toegevoegd bij relevante onderdelen?
+                if (i_calib >= 500) // of wait(10);?
+                {
+                    currentState = Homing;
+                    stateChanged = true;
+                    pc.printf("Moving to Homing state\r\n");
+                }
+=======
                 currentState = Homing;
                 stateChanged = true;
                 pc.printf("Moving to Homing state\r\n");
             }
+>>>>>>> destination
             if  (Emergency_button_pressed.read() == false) 
             { 
                 emergency();