Diff: main.cpp

Revision:

27:fa493551be99

Parent:

26:ac5656aa35c7

Child:

28:5e54cd4525de

diff -r ac5656aa35c7 -r fa493551be99 main.cpp
--- a/main.cpp	Wed Oct 31 10:36:01 2018 +0000
+++ b/main.cpp	Wed Oct 31 12:38:00 2018 +0000
@@ -41,7 +41,7 @@
 Ticker      ticker;                          
 
 //Global variables
-const float T   = 0.002f;                           //Ticker period
+const float T   = 0.002f;                           //Ticker period             Deze wordt ook gebruikt in de PID, moet die niet anders???
 
 //EMG filter
 double emg0_filt, emg1_filt, emg2_filt;                                                          //Variables for filtered EMG data channel 0, 1 and 2
@@ -116,6 +116,8 @@
 double encoder2 = 0;
 double encoder_radians2=0;
 
+double start_control = 0;
+
 
 //--------------Functions----------------------------------------------------------------------------------------------------------------------------//
 
@@ -286,7 +288,6 @@
     movAg0 = sum1/windowsize;                        //calculates an average in the array
     movAg1 = sum2/windowsize;
     movAg2 = sum3/windowsize;
-    //serial getallen sturen, als het 1 getal is gaat hier wat fout, als het een reeks is dan gaat er bij de input naar HIDscope wat fout.
 }
 
 void emg_filtered()             //Call all filter functions
@@ -294,6 +295,7 @@
     EMGFilter0();
     EMGFilter1();
     EMGFilter2();
+    MovAg();
 }
 void switch_to_calibrate()
 {
@@ -379,6 +381,7 @@
         case 3:                                         //EMG is calibrated, robot can be set to Home position.
         {
             emg_cal = 1;                                //This is the setting for which the motors can begin turning in this code (!!)
+            
             wait(0.001f);
             break;
         }
@@ -420,11 +423,13 @@
         
     q1ref = q1_ii;
     q2ref = q2_ii; 
+    
+    start_control = 1;
 }
 
 void v_des_calculate_qref()
 {
-    while(emg_cal==1)                                   //After calibration is finished, emg_cal will be 1. Otherwise 0. 
+    if(emg_cal==1)                                   //After calibration is finished, emg_cal will be 1. Otherwise 0. 
     { 
                 if(movAg1>Threshold1)                   //If the filtered EMG signal of muscle 1 is higher than the threshold, motor 1 will turn
                 {
@@ -457,11 +462,10 @@
                     ledb = 0;
                     ledg = 0;
                 }
-                            
-        break;           
+                
+        inverse_kinematics();                           //Call inverse kinematics function
+        
         }
-        
-        inverse_kinematics();                           //Call inverse kinematics function
 }
 
 //---------PID controller motor 1 + start motor 1 -----------------------------------------------------------//
@@ -475,7 +479,7 @@
   // Proportional part:
   double u_k1 = Kp1 * err1;
 
-  // Integral part
+  // Integral part  
   err_integral1 = err_integral1 + err1 * T;
   double u_i1 = Ki1 * err_integral1;
 
@@ -502,12 +506,17 @@
     }
 }  
 
-    void engine_control1()                                           //Engine 1 is rotational engine, connected with left side pins
+void engine_control1()                                           //Engine 1 is rotational engine, connected with left side pins
 {
-    encoder_radians1 = encoder1*(2*PI)/8400;
-    double err1 = q1ref - encoder_radians1;
-    double u1 = PID_controller1(err1);                               //PID controller function call
-    start_your_engines1(u1);                                         //Call start_your_engines function
+    while(start_control == 1)
+    {
+        encoder_radians1 = encoder1*(2*PI)/8400;
+        double err1 = q1ref - encoder_radians1;
+        double u1 = PID_controller1(err1);                               //PID controller function call
+        start_your_engines1(u1);                                         //Call start_your_engines function
+        
+        break;
+    }
 }
 
 
@@ -539,23 +548,29 @@
 
 void start_your_engines2(double u2)
 {
-    if(encoder2<12600 && encoder2>-1)                              //limits translation in counts
-    {
-         pwmpin2 = fabs(u2);                                       //u_total moet nog geschaald worden om in de motor gevoerd te worden!!!
-         directionpin2.write(u2 < 0.0f);
-    }
+     if(encoder2<12600 && encoder2>-1)                              //limits translation in counts
+     {
+        pwmpin2 = fabs(u2);                                       //u_total moet nog geschaald worden om in de motor gevoerd te worden!!!
+        directionpin2.write(u2 < 0.0f);
+     }
     else
-    {
+     {
         pwmpin2 = 0;
-    }
+     }
+    
 }  
 
 void engine_control2()                                             //Engine 2 is translational engine, connected with right side wires
 {
-    encoder_radians2 = encoder2*(2*PI)/8400;
-    double err2 = q2ref - encoder_radians2;
-    double u2 = PID_controller2(err2);                             //PID controller function call
-    start_your_engines2(u2);                                       //Call start_your_engines function
+    while(start_control == 1)
+    {
+        encoder_radians2 = encoder2*(2*PI)/8400;
+        double err2 = q2ref - encoder_radians2;
+        double u2 = PID_controller2(err2);                             //PID controller function call
+        start_your_engines2(u2);                                       //Call start_your_engines function
+        
+        break;
+    }
 }
 
 //------------------ Start main function --------------------------//
@@ -573,8 +588,7 @@
         
     while(true)
     {
-        ticker.attach(&emg_filtered,T);                         //EMG signals filtered every T sec.
-        ticker.attach(&MovAg,T);                                //Moving average calculation every T sec.
+        ticker.attach(&emg_filtered,T);                         //EMG signals filtered + moving average every T sec.
         ticker.attach(&v_des_calculate_qref,T);                 //v_des determined every T
               
        // HIDScope_tick.attach(&HIDScope_sample,T);             //EMG signals raw + filtered to HIDScope every T sec.