Diff: main.cpp

Revision:

23:4572750a5c59

Parent:

22:8585d41a670b

Child:

24:764b71885785

Child:

26:7ae60739b310

Child:

28:7c7508bdb21f

diff -r 8585d41a670b -r 4572750a5c59 main.cpp
--- a/main.cpp	Mon Oct 28 15:40:31 2019 +0000
+++ b/main.cpp	Mon Oct 28 18:55:22 2019 +0000
@@ -9,15 +9,17 @@
 #include "Servo.h"
 #include <cmath>
 
-// Definieer objecten
 Serial pc(USBTX, USBRX);
 
-PwmOut motor1(D6);  // Misschien moeten we hiervoor DigitalOut gebruiken, moet 
-PwmOut motor2(D5);  // samen kunnen gaan met de servo motor
+// TICKERS
+Ticker loop_ticker;
 
-DigitalOut motor1_dir(D7);
-DigitalOut motor2_dir(D4);
-  
+// BENODIGD VOOR PROCESS STATE MACHINE
+enum states {Motors_off, Calib_motor, Calib_EMG, Homing, Operation_mode};
+states currentState = Motors_off; 
+bool stateChanged = true; // Make sure the initialization of first state is executed
+
+// INPUTS
 DigitalIn Power_button_pressed(D1); // Geen InterruptIn gebruiken!
 DigitalIn Emergency_button_pressed(D2);
 DigitalIn Motor_calib_button_pressed(SW2);
@@ -26,16 +28,17 @@
 AnalogIn EMG_biceps_left_raw (A1);
 AnalogIn EMG_calf_raw (A2);
 
-Ticker loop_ticker;
-Ticker HIDScope_ticker;
-Ticker emgSampleTicker;
-Ticker motorTicker;
+QEI Encoder1(D12, D13, NC, 8400, QEI::X4_ENCODING); //Checken of die D12, D9 etc wel kloppen, 8400= gear ratio x 64
+QEI Encoder2(D9, D10, NC, 8400, QEI::X4_ENCODING);  
 
-QEI Encoder1(D12, D13, NC, 8400, QEI::X4_ENCODING); //Checken of die D12 etc wel kloppen. 
-QEI Encoder2(D9, D10, NC, 8400, QEI::X4_ENCODING);  //Checken of die D9 etc wel kloppen. 
-// 8400= gear ratio x 64
+// OUTPUTS
+PwmOut motor1(D6);  // Misschien moeten we hiervoor DigitalOut gebruiken, moet 
+PwmOut motor2(D5);  // samen kunnen gaan met de servo motor
 
-// Definities variabelen encoder -> motorhoek
+DigitalOut motor1_dir(D7);
+DigitalOut motor2_dir(D4);
+
+// VARIABELEN VOOR ENCODER, MOTORHOEK ETC.
 int counts1;
 int counts2;
 const int CPR = 64; // Counts per revolution
@@ -46,40 +49,7 @@
 double theta_h_1_rad;
 double theta_h_2_rad;
 
-void Calculate_motor_angle()
-    {
-        counts1 = Encoder1.getPulses(); 
-        counts2 = Encoder2.getPulses(); 
-        theta_h_1_deg=(counts1/(double)CPR)*(double)full_degrees;
-        theta_h_2_deg=(counts2/(double)CPR)*(double)full_degrees;
-        theta_h_1_rad=(theta_h_1_deg/half_degrees)*M_PI;
-        theta_h_2_rad=(theta_h_2_deg/half_degrees)*M_PI;
-    }
-
-bool calib = false; // MOGELIJK GAAT HET HIER FOUT
-static int i_calib = 0;
-
-HIDScope scope(3);
-
-// Defining global variables
-double mean_EMG_biceps_right;
-double mean_EMG_biceps_left;
-double mean_EMG_calf;
-double normalized_EMG_biceps_right;
-double normalized_EMG_biceps_left;
-double normalized_EMG_calf;
-static double filtered_EMG_biceps_right;
-double filtered_EMG_biceps_left;
-double filtered_EMG_calf;
-double filtered_EMG_biceps_left_1;
-double filtered_EMG_biceps_right_1;
-double filtered_EMG_calf_1;
-double filtered_EMG_biceps_right_abs;
-double filtered_EMG_biceps_left_abs;
-double filtered_EMG_calf_abs;
-static double filtered_EMG_biceps_right_total; 
-double filtered_EMG_biceps_left_total;
-double filtered_EMG_calf_total;
+// DEFINITIES VOOR FILTERS
 
 // BICEPS-RECHTS
 // Definities voor eerste BiQuadChain (High-pass en Notch) 
@@ -114,78 +84,49 @@
 BiQuad bqk3(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass
 BiQuad bqk4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass
 
-void emgSampleFilter() // Deze functie wordt aangeroepen dmv een ticker. Het sampled 
-                       // hierdoor het EMG signaal en het haalt er een filter overheen.
-                       // Tevens wordt er een stuk script gerund, wanneer de robot
-                       // zich in de kalibratie toestand bevindt. 
-{   
-    filtered_EMG_biceps_right_1=bqbr1.step(EMG_biceps_right_raw.read());
-    filtered_EMG_biceps_left_1=bqcbl.step(EMG_biceps_left_raw.read());
-    filtered_EMG_calf_1=bqck.step(EMG_calf_raw.read());
-    
-    filtered_EMG_biceps_right_abs=abs(filtered_EMG_biceps_right_1);
-    filtered_EMG_biceps_left_abs=abs(filtered_EMG_biceps_left_1);
-    filtered_EMG_calf_abs=abs(filtered_EMG_calf_1);
-    
-    filtered_EMG_biceps_right=bqcbr2.step(filtered_EMG_biceps_right_abs);
-    filtered_EMG_biceps_left=bqcbl2.step(filtered_EMG_biceps_left_abs);
-    filtered_EMG_calf=bqck2.step(filtered_EMG_calf_abs);
-    
-    if (calib) // In de kalibratie staat treedt deze loop in werking. De spier wordt
-               // dan maximaal aangespannen (gedurende 5 seconden). De EMG waarden
-               // worden bij elkaar opgeteld, waarna het gemiddelde wordt bepaald.     
-        {   
-            if (i_calib == 0)
-                {
-                    filtered_EMG_biceps_right_total=0;
-                    filtered_EMG_biceps_left_total=0;
-                    filtered_EMG_calf_total=0;
-                }
-            if (i_calib <= 2500)
-                {
-                    filtered_EMG_biceps_right_total+=filtered_EMG_biceps_right;
-                    // pc.printf("%f\r\n", filtered_EMG_biceps_right_total);
-                    filtered_EMG_biceps_left_total+=filtered_EMG_biceps_left;
-                    filtered_EMG_calf_total+=filtered_EMG_calf;
-                    i_calib++;
-                }
-            if (i_calib > 2500 && calib)
-                {   
-                    mean_EMG_biceps_right=filtered_EMG_biceps_right_total/2500.0;
-                    mean_EMG_biceps_left=filtered_EMG_biceps_left_total/2500.0;
-                    mean_EMG_calf=filtered_EMG_calf_total/2500.0;
-                    pc.printf("Ontspan spieren\r\n");
-                    pc.printf("Rechterbiceps_max = %f, Linkerbiceps_max = %f, Kuit_max = %f\r\n", mean_EMG_biceps_right, mean_EMG_biceps_left, mean_EMG_calf);
-                    pc.printf("Rechterbiceps_max = %f\r\n", filtered_EMG_biceps_right_total);
-                    calib = false; 
-                }
-         }
-}
+// VARIABELEN VOOR EMG + FILTEREN 
+double filtered_EMG_biceps_right;
+double filtered_EMG_biceps_left;
+double filtered_EMG_calf;
+
+double filtered_EMG_biceps_right_1;
+double filtered_EMG_biceps_left_1;
+double filtered_EMG_calf_1;
+
+double filtered_EMG_biceps_right_abs;
+double filtered_EMG_biceps_left_abs;
+double filtered_EMG_calf_abs;
+
+double filtered_EMG_biceps_right_total; 
+double filtered_EMG_biceps_left_total;
+double filtered_EMG_calf_total;
 
-    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); 
-     scope.set(1, EMG_biceps_right_raw);
-     scope.set(2, normalized_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
-    }
+// Variabelen voor HIDScope
+HIDScope scope(3);
+
+// VARIABELEN VOOR (INITIATIE VAN) EMG KALIBRATIE LOOP
+bool calib = false; 
+static int i_calib = 0;
 
-// Emergency    
+double mean_EMG_biceps_right;
+double mean_EMG_biceps_left;
+double mean_EMG_calf;
+
+// VARIABELEN VOOR OPERATION MODE
+double normalized_EMG_biceps_right;
+double normalized_EMG_biceps_left;
+double normalized_EMG_calf;
+
+// VOIDS
+
+// Noodfunctie waarbij alles uitgaat (evt. nog een rood LEDje laten branden).
+// Enige optie is resetten, dan wordt het script opnieuw opgestart.    
 void emergency()
     {
         loop_ticker.detach(); 
         motor1.write(0);
         motor2.write(0);
         pc.printf("Ik ga exploderen!!!\r\n");
-        // Alles moet uitgaan (evt. een rood LEDje laten branden), moet 
-        // opnieuw worden opgestart. Mogelijk kan dit door de ticker te 
-        // detachen
     }    
 
 // Motoren uitzetten    
@@ -205,31 +146,76 @@
         motor1_dir.write(1);
         pc.printf("Motoren aan functie\r\n");
     }
-        
-// 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???
-       
-     }
 
 // Finite state machine programming (calibration servo motor?)
-enum states {Motors_off, Calib_motor, Calib_EMG, Homing, Operation_mode};
-
-states currentState = Motors_off; 
-bool stateChanged = true; // Make sure the initialization of first state is executed
-
 void ProcessStateMachine(void) 
 {   
+    // Berekenen van de motorhoeken (in radialen)
+    counts1 = Encoder1.getPulses(); 
+    counts2 = Encoder2.getPulses(); 
+    theta_h_1_deg=(counts1/(double)CPR)*(double)full_degrees;
+    theta_h_2_deg=(counts2/(double)CPR)*(double)full_degrees;
+    theta_h_1_rad=(theta_h_1_deg/half_degrees)*M_PI;
+    theta_h_2_rad=(theta_h_2_deg/half_degrees)*M_PI;
+    
+    // Eerste deel van de filters (High-pass + Notch) over het ruwe EMG signaal
+    // doen. Het ruwe signaal wordt gelezen binnen een ticker en wordt daardoor 'gesampled' 
+    filtered_EMG_biceps_right_1=bqbr1.step(EMG_biceps_right_raw.read());
+    filtered_EMG_biceps_left_1=bqcbl.step(EMG_biceps_left_raw.read());
+    filtered_EMG_calf_1=bqck.step(EMG_calf_raw.read());
+    
+    // Vervolgens wordt de absolute waarde hiervan genomen
+    filtered_EMG_biceps_right_abs=abs(filtered_EMG_biceps_right_1);
+    filtered_EMG_biceps_left_abs=abs(filtered_EMG_biceps_left_1);
+    filtered_EMG_calf_abs=abs(filtered_EMG_calf_1);
+    
+    // Tenslotte wordt het tweede deel van de filters (twee low-pass, voor 4e orde filter)
+    // over het signaal gedaan
+    filtered_EMG_biceps_right=bqcbr2.step(filtered_EMG_biceps_right_abs);
+    filtered_EMG_biceps_left=bqcbl2.step(filtered_EMG_biceps_left_abs);
+    filtered_EMG_calf=bqck2.step(filtered_EMG_calf_abs);
+    
+    // De gefilterde EMG-signalen kunnen tevens visueel worden weergegeven in de HIDScope  
+    scope.set(0, filtered_EMG_biceps_right); 
+    scope.set(1, normalized_EMG_biceps_right);
+    scope.set(2, filtered_EMG_calf);
+    scope.send();
+    
+    // Tijdens de kalibratie moet vervolgens een maximale spierspanning worden bepaald, die 
+    // later kan worden gebruikt voor een normalisatie. De spieren worden hiertoe gedurende 
+    // 5 seconden maximaal aangespannen. De EMG waarden worden bij elkaar opgeteld,
+    // waarna het gemiddelde wordt bepaald. 
+    if (calib) 
+        {   
+            if (i_calib == 0)
+                {
+                    filtered_EMG_biceps_right_total=0;
+                    filtered_EMG_biceps_left_total=0;
+                    filtered_EMG_calf_total=0;
+                }
+            if (i_calib <= 2500)
+                {
+                    filtered_EMG_biceps_right_total+=filtered_EMG_biceps_right;
+                    filtered_EMG_biceps_left_total+=filtered_EMG_biceps_left;
+                    filtered_EMG_calf_total+=filtered_EMG_calf;
+                    i_calib++;
+                }
+            if (i_calib > 2500)
+                {   
+                    mean_EMG_biceps_right=filtered_EMG_biceps_right_total/2500.0;
+                    mean_EMG_biceps_left=filtered_EMG_biceps_left_total/2500.0;
+                    mean_EMG_calf=filtered_EMG_calf_total/2500.0;
+                    pc.printf("Ontspan spieren\r\n");
+                    pc.printf("Rechterbiceps_max = %f, Linkerbiceps_max = %f, Kuit_max = %f\r\n", mean_EMG_biceps_right, mean_EMG_biceps_left, mean_EMG_calf);
+                    calib = false; 
+                }
+         }
+    
+    // Genormaliseerde EMG's berekenen
+    normalized_EMG_biceps_right=filtered_EMG_biceps_right/mean_EMG_biceps_right;
+    normalized_EMG_biceps_left=filtered_EMG_biceps_left/mean_EMG_biceps_left;
+    normalized_EMG_calf=filtered_EMG_calf/mean_EMG_calf;
+         
     switch (currentState) 
     { 
         case Motors_off:
@@ -274,26 +260,18 @@
             
             if (stateChanged)
                 {
-                    // Hierbij wordt een een kalibratie uitgevoerd, waarbij de maximale EMG-amplitude waarde wordt bepaald
-                
                     motors_off();
                     i_calib = 0;
-                    // pc.printf("Huidige hoek in radialen motor 1:%f en motor 2: %f (moet 0 zijn) \r\n", theta_h_1_rad, theta_h_2_rad);
                     calib = true;
                     pc.printf("Span spieren aan\r\n");
-                    // emgSampleFilter(); // Gaat dit nu goed? -> moet sws worden toegevoegd bij relevante onderdelen?
                     stateChanged = false; 
                 }    
                 
-            if (i_calib > 2500) // of wait(10);?
+            if (i_calib > 2500) 
                 {
                     calib = false;
                     currentState = Homing;
                     stateChanged = true;
-                    normalized_EMG_biceps_right=filtered_EMG_biceps_right/mean_EMG_biceps_right;
-                    normalized_EMG_biceps_left=filtered_EMG_biceps_left/mean_EMG_biceps_left;
-                    normalized_EMG_calf=filtered_EMG_calf/mean_EMG_calf;
-                    pc.printf("normalized_EMG_biceps_right= %f, mean_EMG_biceps_right = %f, filtered_EMG_biceps_right = %f\r\n", normalized_EMG_biceps_right, mean_EMG_biceps_right, filtered_EMG_biceps_right);
                     pc.printf("Moving to Homing state\r\n");
                 }
             
@@ -303,7 +281,7 @@
             } 
             break; 
             
-         case Homing:
+         case Homing: // NOG NAAR KIJKEN
             
             if (stateChanged)
             {   
@@ -312,6 +290,7 @@
                 motors_on();
                 currentState = Operation_mode;
                 stateChanged = true;
+                motors_off(); 
                 pc.printf("Moving to operation mode \r\n");
             }
             if  (Emergency_button_pressed.read() == false) 
@@ -323,11 +302,12 @@
          case Operation_mode: // Overgaan tot emergency wanneer referentie niet 
                               // overeenkomt met werkelijkheid
             
+            // pc.printf("normalized_EMG_biceps_right= %f, mean_EMG_biceps_right = %f, filtered_EMG_biceps_right = %f\r\n", normalized_EMG_biceps_right, mean_EMG_biceps_right, filtered_EMG_biceps_right);
             if (stateChanged)
              
                 // Hier moet een functie worden aangeroepen die ervoor zorgt dat 
                 // aan de hand van EMG-signalen de motoren kunnen worden aangestuurd, 
-                // zodat de robotarm kan bewegen   
+                // zodat de robotarm kan bewegen 
                 
                 {                  
                     if (normalized_EMG_biceps_right >= 0.3)
@@ -338,14 +318,45 @@
                             motor2_dir.write(1);
                             if (normalized_EMG_calf >= 0.3)
                                 {
-                                    // motor1_dir = !motor1_dir;
-                                    // motor2_dir = !motor2_dir;
+                                    motor1.write(0.1);
+                                    motor1_dir = !motor1_dir;
                                 }  
+                            if (normalized_EMG_biceps_left >= 0.3)
+                                {
+                                    motor2.write(0.9);
+                                    motor2_dir.write(1);
+                                    motor1.write(0);
+                                    motor1_dir.write(1);
+                                    if (normalized_EMG_calf >= 0.3)
+                                        {
+                                            motor2.write(0.1);
+                                            motor2_dir = !motor2_dir;
+                                        }  
+                                }
                         }
                     if (normalized_EMG_biceps_right < 0.3)
                         {       
                             motor1.write(0);
                             motor2.write(0);
+                            if (normalized_EMG_calf >= 0.3)
+                                {
+                                    // motor1_dir = !motor1_dir;
+                                    // pc.printf("Richting zou om moeten draaien");
+                                    // motor2_dir = !motor2_dir;
+                                }  
+                            if (normalized_EMG_biceps_left >= 0.3)
+                                {
+                                    motor2.write(0.9);
+                                    motor2_dir.write(1);
+                                    motor1.write(0);
+                                    motor1_dir.write(1);
+                                    if (normalized_EMG_calf >= 0.3)
+                                        {
+                                            // motor1_dir = !motor1_dir;
+                                            // pc.printf("Richting zou om moeten draaien");
+                                            // motor2_dir = !motor2_dir;
+                                        }  
+                                }
                         }
                     if (normalized_EMG_biceps_left >= 0.3)
                         {
@@ -356,15 +367,43 @@
                             if (normalized_EMG_calf >= 0.3)
                                 {
                                     // motor1_dir = !motor1_dir;
+                                    // pc.printf("Richting zou om moeten draaien");
                                     // motor2_dir = !motor2_dir;
                                 }  
+                            if (normalized_EMG_biceps_right >= 0.3)
+                                {
+                                    motor1.write(0.5);
+                                    motor1_dir.write(1);
+                                    motor2.write(0);
+                                    motor2_dir.write(1);
+                                    if (normalized_EMG_calf >= 0.3)
+                                        {
+                                            // motor1_dir = !motor1_dir;
+                                            // pc.printf("Richting zou om moeten draaien");
+                                            // motor2_dir = !motor2_dir;
+                                        }  
+                                }
                         }
                     if (normalized_EMG_biceps_left < 0.3)
                         {
                             motor2.write(0);
                             motor1.write(0);
-                        }              
-                 }   
+                            if (normalized_EMG_biceps_right >= 0.3)
+                                {
+                                    motor1.write(0.5);
+                                    motor1_dir.write(1);
+                                    motor2.write(0);
+                                    motor2_dir.write(1);
+                                    if (normalized_EMG_calf >= 0.3)
+                                        {
+                                            // motor1_dir = !motor1_dir;
+                                            // pc.printf("Richting zou om moeten draaien");
+                                            // motor2_dir = !motor2_dir;
+                                        }  
+                                }
+                        }
+                }
+ 
             if (Power_button_pressed.read() == false) // Normaal waarde 1 bij indrukken, nu nul -> false
                 {
                     motors_off();
@@ -406,11 +445,8 @@
         // Chain voor kuit
         bqck.add(&bqk1).add(&bqk2);
         bqck2.add(&bqk3).add(&bqk4); 
-           
-        emgSampleTicker.attach(&emgSampleFilter, 0.002f);
-        HIDScope_ticker.attach(&sendHIDScope, 0.002f);
-        motorTicker.attach(&Calculate_motor_angle, 0.002f);
-        loop_ticker.attach(&ProcessStateMachine, 5.0f);  
+
+        loop_ticker.attach(&ProcessStateMachine, 0.002f);  
         
         while(true) 
             {