Diff: main.cpp

Revision:

21:456acc79726c

Parent:

20:a6a5bdd7d118

Child:

22:8585d41a670b

--- a/main.cpp	Thu Oct 24 11:46:17 2019 +0000
+++ b/main.cpp	Mon Oct 28 14:29:13 2019 +0000
@@ -4,8 +4,10 @@
 #include "MODSERIAL.h"
 #include "BiQuad.h"
 #include "FastPWM.h"
+#define M_PI 3.14159265358979323846  /* pi */
 #include <math.h>
 #include "Servo.h"
+#include <cmath>
 
 // Definieer objecten
 Serial pc(USBTX, USBRX);
@@ -26,90 +28,151 @@
 Ticker loop_ticker;
 Ticker HIDScope_ticker;
 Ticker emgSampleTicker;
+Ticker motorTicker;
+
+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
+
+// Definities variabelen encoder -> motorhoek
+int counts1;
+int counts2;
+const int CPR = 64; // Counts per revolution
+const int full_degrees = 360;
+const int half_degrees = 180;
+double theta_h_1_deg;
+double theta_h_2_deg;
+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
-int i_calib = 0;
+static int i_calib = 0;
 
 HIDScope scope(3);
 
-float mean_EMG_biceps_right;
-float mean_EMG_biceps_left;
-float mean_EMG_calf;
-float normalized_EMG_biceps_right;
-float filtered_EMG_biceps_right;
-float normalized_EMG_biceps_left;
-float filtered_EMG_biceps_left;
-float normalized_EMG_calf;
-float filtered_EMG_calf;
+// 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 eerste BiQuadChain (High-pass en Notch)
-BiQuadChain bqc; 
-BiQuad bq1(0.8006, -1.6012, 0.8006, -1.5610, 0.6414); // High-pass 
-BiQuad bq2(1, -1.6180, 1, -1.6019, 0.9801); // Notch
+// BICEPS-RECHTS
+// Definities voor eerste BiQuadChain (High-pass en Notch) 
+BiQuadChain bqcbr; 
+BiQuad bqbr1(0.8006, -1.6012, 0.8006, -1.5610, 0.6414); // High-pass 
+BiQuad bqbr2(1, -1.6180, 1, -1.6019, 0.9801); // Notch
+// Na het nemen van de absolute waarde moet de tweede BiQuadChain worden toegepast.
+// Definieer (twee Low-pass -> vierde orde verkrijgen): 
+BiQuadChain bqcbr2;
+BiQuad bqbr3(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass
+BiQuad bqbr4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass
 
+// BICEPS-LINKS
+// Definities voor eerste BiQuadChain (High-pass en Notch) 
+BiQuadChain bqcbl; 
+BiQuad bqbl1(0.8006, -1.6012, 0.8006, -1.5610, 0.6414); // High-pass 
+BiQuad bqbl2(1, -1.6180, 1, -1.6019, 0.9801); // Notch
 // Na het nemen van de absolute waarde moet de tweede BiQuadChain worden toegepast.
-// Definieer (twee Low-pass -> vierde orde verkrijgen):
-BiQuadChain bqc2;
-BiQuad bq3(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass
-BiQuad bq4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass
+// Definieer (twee Low-pass -> vierde orde verkrijgen): 
+BiQuadChain bqcbl2;
+BiQuad bqbl3(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass
+BiQuad bqbl4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass
+
+// KUIT
+// Definities voor eerste BiQuadChain (High-pass en Notch) 
+BiQuadChain bqck; 
+BiQuad bqk1(0.8006, -1.6012, 0.8006, -1.5610, 0.6414); // High-pass 
+BiQuad bqk2(1, -1.6180, 1, -1.6019, 0.9801); // Notch
+// Na het nemen van de absolute waarde moet de tweede BiQuadChain worden toegepast.
+// Definieer (twee Low-pass -> vierde orde verkrijgen): 
+BiQuadChain bqck2;
+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.
-                       // Het signaal kan tevens via de HIDScope worden gevolgd. 
-                       // Tenslotte wordt er een stuk script gerund, wanneer de robot
+                       // Tevens wordt er een stuk script gerund, wanneer de robot
                        // zich in de kalibratie toestand bevindt. 
 {   
-    float filtered_EMG_biceps_right_1=bqc.step(EMG_biceps_right_raw.read());
-    float filtered_EMG_biceps_left_1=bqc.step(EMG_biceps_left_raw.read());
-    float filtered_EMG_calf_1=bqc.step(EMG_calf_raw.read());
+    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());
     
-    float filtered_EMG_biceps_right_abs=abs(filtered_EMG_biceps_right_1);
-    float filtered_EMG_biceps_left_abs=abs(filtered_EMG_biceps_left_1);
-    float filtered_EMG_calf_abs=abs(filtered_EMG_calf_1);
-    
-    float filtered_EMG_biceps_right=bqc2.step(filtered_EMG_biceps_right_abs);
-    float filtered_EMG_biceps_left=bqc2.step(filtered_EMG_biceps_left_abs);
-    float filtered_EMG_calf=bqc2.step(filtered_EMG_calf_abs);
+    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);
     
-    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;
+    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);
     
-    //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
-//}
+    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;
     
     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.
+               // worden bij elkaar opgeteld, waarna het gemiddelde wordt bepaald.     
         {   
-            if (i_calib < 500)
+            if (i_calib < 2500)
                 {
-                    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;
+                    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 >= 500)
+            if (i_calib >= 2500 && calib)
                 {   
-                    float mean_EMG_biceps_right=filtered_EMG_biceps_right_total/500;
-                    float mean_EMG_biceps_left=filtered_EMG_biceps_left_total/500;
-                    float mean_EMG_calf=filtered_EMG_calf_total/500;
+                    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; 
                 }
          }
 }
 
+    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
+    }
+
 // Emergency    
 void emergency()
     {
@@ -192,6 +255,10 @@
             if (stateChanged)
             {       
                 // Hier wordt een kalibratie uitgevoerd, waarbij de motorhoeken worden bepaald
+                pc.printf("Huidige hoek in radialen motor 1:%f en motor 2: %f\r\n", theta_h_1_rad, theta_h_2_rad);
+                theta_h_1_rad = 0;
+                theta_h_2_rad = 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);
                 currentState = Calib_EMG;
                 stateChanged = true;
                 pc.printf("Moving to Calib_EMG state\r\n");
@@ -204,25 +271,30 @@
             
          case Calib_EMG:
             
-            motors_off();
             if (stateChanged)
-            {
-                // Hierbij wordt een een kalibratie uitgevoerd, waarbij de maximale EMG-amplitude waarde wordt bepaald
-
-                calib = true;
-                emgSampleFilter(); // Gaat dit nu goed? -> moet sws worden toegevoegd bij relevante onderdelen?
-                if (i_calib >= 500) // of wait(10);?
                 {
+                    // Hierbij wordt een een kalibratie uitgevoerd, waarbij de maximale EMG-amplitude waarde wordt bepaald
+                
+                    motors_off();
+                    // 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);?
+                {
+                    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");
                 }
-
-                currentState = Homing;
-                stateChanged = true;
-                pc.printf("Moving to Homing state\r\n");
-            }
-
+            
             if  (Emergency_button_pressed.read() == false) 
             { 
                 emergency();
@@ -231,11 +303,11 @@
             
          case Homing:
             
-            motors_on();
             if (stateChanged)
             {   
                 // Ervoor zorgen dat de motoren zo bewegen dat de robotarm 
                 // (inclusief de end-effector) in de juiste home positie wordt gezet
+                motors_on();
                 currentState = Operation_mode;
                 stateChanged = true;
                 pc.printf("Moving to operation mode \r\n");
@@ -248,22 +320,25 @@
             
          case Operation_mode: // Overgaan tot emergency wanneer referentie niet 
                               // overeenkomt met werkelijkheid
-        
+            
             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   
                 
-                {
-                    float normalized_EMG_biceps_right=filtered_EMG_biceps_right/mean_EMG_biceps_right;
-                    float normalized_EMG_biceps_left=filtered_EMG_biceps_left/mean_EMG_biceps_left;
-                    float normalized_EMG_calf=filtered_EMG_calf/mean_EMG_calf;
-                    
+                {                  
                     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;
+                                    // motor2_dir = !motor2_dir;
+                                }  
                         }
                     if (normalized_EMG_biceps_right < 0.3)
                         {       
@@ -273,35 +348,40 @@
                     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;
+                                    // motor2_dir = !motor2_dir;
+                                }  
                         }
                     if (normalized_EMG_biceps_left < 0.3)
                         {
                             motor2.write(0);
                             motor1.write(0);
                         }              
-                    
-                    if (Power_button_pressed.read() == false) // Normaal waarde 1 bij indrukken, nu nul -> false
-                        {
-                            motors_off();
-                            currentState = Motors_off;
-                            stateChanged = true;
-                            pc.printf("Terug naar de state Motors_off\r\n");
-                        }
-                    if  (Emergency_button_pressed.read() == false) 
-                        { 
-                            emergency();
-                        } 
-                    // wait(5);
-                    else 
-                        { 
-                            currentState = Homing; 
-                            stateChanged = true; 
-                            pc.printf("Terug naar de state Homing\r\n");
-                        }
-                    break;
-                  
-                }   
+                 }   
+            if (Power_button_pressed.read() == false) // Normaal waarde 1 bij indrukken, nu nul -> false
+                {
+                    motors_off();
+                    currentState = Motors_off;
+                    stateChanged = true;
+                    pc.printf("Terug naar de state Motors_off\r\n");
+                }
+            if  (Emergency_button_pressed.read() == false) 
+                { 
+                    emergency();
+                } 
+            // wait(25);
+            // else 
+                // { 
+                    // currentState = Homing; 
+                    // stateChanged = true; 
+                    // pc.printf("Terug naar de state Homing\r\n");
+                // }
+            break;
         
         default:
             // Zelfde functie als die eerder is toegepast om motoren uit te schakelen -> safety!
@@ -314,10 +394,25 @@
 int main(void)
     {
         pc.printf("Opstarten\r\n");
-        bqc.add(&bq1).add(&bq2);
-        bqc2.add(&bq3).add(&bq4);
-        emgSampleTicker.attach(&emgSampleFilter, 0.01f);
+        
+        // Chain voor rechter biceps
+        bqcbr.add(&bqbr1).add(&bqbr2);
+        bqcbr2.add(&bqbr3).add(&bqbr4);
+        // Chain voor linker biceps
+        bqcbl.add(&bqbl1).add(&bqbl2);
+        bqcbl2.add(&bqbl3).add(&bqbl4);  
+        // 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);  
+        
         while(true) 
-        { /* do nothing */}
+            { 
+                // wait(0.2);
+                /* do nothing */
+            }
     }
\ No newline at end of file