Revision 5:224c1fe73a8f, committed 2018-09-28

Comitter:

s1680897

Date:

Fri Sep 28 09:43:45 2018 +0000

Parent:

4:de0923cf6bcc

Commit message:

Biorobotics 2017

Changed in this revision

--- a/main.cpp	Thu Nov 02 12:11:41 2017 +0000
+++ b/main.cpp	Fri Sep 28 09:43:45 2018 +0000
@@ -35,8 +35,24 @@
 Ticker motor_update2;
 Ticker error_update;
 
+//Define objects EMG filter
+AnalogIn    emg2( A2 );
+AnalogIn    emg3( A3 );
+AnalogIn    emg4( A4 );
+AnalogIn    emg5( A5 );
 
-//-----------------variable decleration----------------
+
+Ticker      sample_timer;
+Ticker      scope_timer;
+
+DigitalOut  ledred(LED_RED);
+DigitalOut  ledblue(LED_BLUE);
+DigitalOut  ledgreen(LED_GREEN);
+DigitalOut  led2(LED_RED);
+DigitalIn   button_cal (SW2);
+
+
+//-----------------variable declaration----------------
 int pwm_freq = 500;
 float set_speed;
 float reference_pos;
@@ -66,14 +82,217 @@
 
 float Ts = 0.002; //500hz sample freq
 
+float gain_factor = 1.5;
+double x_vel;
+double y_vel;
+
 bool M1homflag;
 bool M2homflag;
 bool Homstartflag;
   
+/** Sample function
+ * this function samples the emg and sends it to HIDScope
+ **/
+ int P= 200;
+ double A[200];
+ 
+ double sig_2_abs;
+ double sig_3_abs;
+ double sig_4_abs;
+ double sig_5_abs;
+ double movmean2;
+ double movmean3;
+ double movmean4;
+ double movmean5;
+ double emgsample2;
+ double emgsample3;
+ double emgsample4;
+ double emgsample5;
+ 
+enum        States {cal2,cal3,cal4,cal5};
+int         state;
 
+double      movmean_cal[5000];
+double      movmean_max2=0;
+double      movmean_max3=0;
+double      movmean_max4=0;
+double      movmean_max5=0; 
+
+
+// Filters 
+BiQuadChain bqc;
+BiQuad bq1( 0.6844323315947305,1.368864663189461, 0.6844323315947305,1.2243497755555954,0.5133795508233265); //lp?
+BiQuad bq2( 0.6844323315947306, -1.3688646631894612,  0.6844323315947306,   -1.2243497755555959, 0.5133795508233266); //hp?
+BiQuad bq3(  0.7566897754116633, -1.2243497755555959,  0.7566897754116633,   -1.2243497755555959, 0.5133795508233266); // notch
+    
+    
+//Function emgfilter signal 2
+    double emgfilter2(double emgsample2)
+        {
+        //filter and take abs of input
+        double sig_2_filt = bqc.step(emgsample2);
+        sig_2_abs = abs(sig_2_filt);
+        //calculate moving mean of the filtered signal
+        for(int i = P-1; i >= 0; i--){
+            if (i == 0) {
+            A[i] = sig_2_abs;
+            }
+            else {
+             A[i] = A[i-1];
+             }   
+                                    }
+            double sum2 = 0;
+            // deze for-loop sommeert de array
+            for (int n = 0; n < P-1; n++) 
+                {
+                sum2 = sum2 + A[n];
+                movmean2 = sum2/P; //dit is de moving average waarde  
+                }
+                double set_speed2=movmean2/movmean_max2;
+                return(set_speed2);
+                }
+            
+    
+//Signal 3
+    double emgfilter3(double emgsample3){
+        //filter and take abs of input
+        double sig_3_filt = bqc.step(emgsample3);
+        sig_3_abs = abs(sig_3_filt);
+    
+        //calculate moving mean of the filtered signal
+        for(int i = P-1; i >= 0; i--){
+        if (i == 0) {
+            A[i] = sig_3_abs;
+            }
+         else {
+             A[i] = A[i-1];
+             }   
+            }
+    double sum3 = 0;
+    // deze for-loop sommeert de array
+    for (int n = 0; n < P-1; n++) {
+    sum3 = sum3 + A[n];
+    movmean3 = sum3/P; //dit is de moving average waarde    
+        }
+    double set_speed3=movmean3/movmean_max3;
+                return(set_speed3);
+    }
+//Signal 4
+    double emgfilter4(double emgsample4)
+    {
+        //filter and take abs of input
+        double sig_4_filt = bqc.step(emgsample4);
+        sig_4_abs = abs(sig_4_filt);
+    
+        //calculate moving mean of the filtered signal
+        for(int i = P-1; i >= 0; i--){
+             if (i == 0) {
+                A[i] = sig_4_abs;
+                }
+            else {
+                A[i] = A[i-1];
+                }   
+                                    }
+    double sum4 = 0;
+    // deze for-loop sommeert de array
+    for (int n = 0; n < P-1; n++) {
+    sum4 = sum4 + A[n];
+    movmean4 = sum4/P; //dit is de moving average waarde    
+        }
+    double set_speed4=movmean4/movmean_max4;
+    return(set_speed4);
+    }
+    //Signal 5  
+    double emgfilter5(double emgsample5)
+    {
+        //filter and take abs of input
+        double sig_5_filt = bqc.step(emgsample5);
+        sig_5_abs = abs(sig_5_filt);
+    
+        //calculate moving mean of the filtered signal
+        for(int i = P-1; i >= 0; i--){
+            if (i == 0) {
+                A[i] = sig_5_abs;
+                }
+            else {
+                A[i] = A[i-1];
+             }   
+                }
+    double sum5 = 0;
+    // deze for-loop sommeert de array
+    for (int n = 0; n < P-1; n++) {
+    sum5 = sum5 + A[n];
+    movmean5 = sum5/P; //dit is de moving average waarde    
+    }
+    double set_speed5=movmean5/movmean_max5;
+    return(set_speed5);
+}
+
+void sample(){
+    emgsample2 = emg2.read();
+    emgsample3 = emg3.read();
+    emgsample4 = emg4.read();
+    emgsample5 = emg5.read();
+    double x_vel = emgfilter2(emgsample2)-emgfilter3(emgsample3); 
+    double y_vel  = emgfilter4(emgsample4)-emgfilter5(emgsample5);
+    }
+    
+//Calibration
+void calibration_switch()
+{
+    switch(state) {
+        case cal2:
+            ledred=0;
+            for (int i=0; i<=3500; i++){
+            if (movmean2>movmean_max2) {
+                movmean_max2=movmean2;
+                }}
+                state=cal3;
+        break;
+                
+        case cal3:        
+            ledred=1;
+            ledblue=0;
+            for (int i=0; i<=3500; i++){
+            if (movmean3>movmean_max3) {
+                movmean_max3=movmean3;
+                }}
+                state=cal4;
+        break;
+                
+        case cal4:        
+            ledblue=1;
+            ledgreen=0;
+            for (int i=0; i<=3500; i++){
+            if (movmean4>movmean_max4) {
+                movmean_max4=movmean4;
+                }}
+                state=cal5;
+        break;
+                
+        case cal5:
+            ledgreen=1;
+            ledred=0;
+            ledblue=0;
+            for (int i=0; i<=3500; i++){
+            if (movmean5>movmean_max5) {
+                movmean_max5=movmean5;
+                }}
+        break;
+} //end switch  
+} //end calibration_switch
+
+void calibration () {
+         while(button1==0) {
+                state=cal2;
+                calibration_switch();
+                    }
+                    }
 
  
 void kinemtaica(double q1, double q2, double vdx, double vdy, double &q1_new, double &q2_new){
+  vdx=x_vel*gain_factor;
+  vdy=y_vel*gain_factor;
   double q1_dot = (2.5*cos(q1+q2))/(sin(q2)) *vdx + (2.5*sin(q1+q2))/(sin(q2))*vdy;
   double q2_dot = -(0.3*cos(q1+q2)+0.4*cos(q1))/(0.12*sin(q2))*vdx -(0.3*sin(q1+q2)+0.4*sin(q2))/(0.12*sin(q2))*vdy;
   q1_new = q1 +q1_dot*Ts;
@@ -218,7 +437,15 @@
 }
  
 int main() {
+    calibration();
+    //add biquad sections to the chain
+    bqc.add( &bq1 ).add( &bq2 ).add( &bq3 );
     
+    sample_timer.attach(&sample, 0.002);
+    //scope_timer.attach(&scopesend, 0.002);
+                
+    /*empty loop, sample() is executed periodically*/
+    while(1) {}
     //initialize serial comm and set motor PWM freq
 M1_speed.period(1.0/pwm_freq);
 M2_speed.period(1.0/pwm_freq);