Revision 7:defb5001b787, committed 2016-11-01

Comitter:

meikefrok

Date:

Tue Nov 01 13:02:00 2016 +0000

Parent:

6:803234ba7071

Commit message:

New restrictions;

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HIDScope.lib	Tue Nov 01 13:02:00 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/tomlankhorst/code/HIDScope/#188304906687

--- a/main.cpp	Tue Nov 01 09:15:33 2016 +0000
+++ b/main.cpp	Tue Nov 01 13:02:00 2016 +0000
@@ -6,6 +6,7 @@
 #include "Servo.h"
 #include "BiQuad.h"
 #include <math.h>
+#include "HIDScope.h"
 
 //======== Serial Communication ================================================
 MODSERIAL pc(USBTX,USBRX);
@@ -25,6 +26,8 @@
 // servo
 Servo servo(D9);
 
+HIDScope    scope(2); //scope has two ports for the two EMG signals
+
 //======== Miscellaneous =======================================================
 // button
 InterruptIn btn(SW2);
@@ -79,7 +82,7 @@
 // miscellaneous
 const float kTimeToggle = 0.25f;        // period with which to toggle the parts
 const int LedOn = 0;                    // LED on if 0
-volatile int part_id = 2;               // ID of what part should move, begins with the cart
+volatile int part_id = 1;               // ID of what part should move, begins with the cart
 volatile int servo_id = 1;              // ID to the side the servo should move, begins in center position
 
 
@@ -102,6 +105,17 @@
 double rno_a1 = -1.603;    
 double rno_a2 = 0.9822;    
 
+double lno2_b0 = 0.9824;
+double lno2_b1 = -0.6071;
+double lno2_b2 = 0.9824;
+double lno2_a1 = -0.6071;
+double lno2_a2 = 0.9647;
+   
+double rno2_b0 = 0.9824;
+double rno2_b1 = -0.6071;
+double rno2_b2 = 0.9824;
+double rno2_a1 = -0.6071;
+double rno2_a2 = 0.9647;
 
 double lhf_b0 = 0.9355;    
 double lhf_b1 = -1.8711;   
@@ -131,10 +145,12 @@
 
 //starting values of the biquads of the corresponding filters
 double lno_v1 = 0, lno_v2 = 0;
+double lno2_v1 = 0, lno2_v2 = 0;
 double lhf_v1 = 0, lhf_v2 = 0;
 double llf_v1 = 0, llf_v2 = 0;
 
 double rno_v1 = 0, rno_v2 = 0;
+double rno2_v1 = 0, rno2_v2 = 0;
 double rhf_v1 = 0, rhf_v2 = 0;
 double rlf_v1 = 0, rlf_v2 = 0;
 
@@ -143,17 +159,21 @@
 so lno_y goes into lhf_y etc.
 */ 
 double lno_y;
+double lno2_y;
 double lhf_y;
 double llf_y;
 double lrect_y;
 double rno_y;
+double rno2_y;
 double rhf_y;
 double rlf_y;
 double rrect_y;
 
 // set the threshold value for the filtered signal
 //if the signal exceeds this value the motors will start to rotate 
-const double threshold_value = 0.13;
+const double threshold_value_left= 0.13;
+const double threshold_value= 0.13;
+
 
 /* declaration of each biquad
 The coefficients will be filled in later on in void scopeSend
@@ -171,6 +191,16 @@
     return y;
 }
 
+double biquad_lno2(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
+    const double b1 , const double b2 )
+{
+    double v = u - a1*v1 - a2*v2;
+    double y = b0*v + b1*v1 + b2*v2;
+    v2 = v1;
+    v1 = v;
+    return y;
+}
+
 double biquad_lhf(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
     const double b1 , const double b2 )
 {
@@ -200,6 +230,16 @@
     return y;
 }
 
+double biquad_rno2(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
+    const double b1 , const double b2 )
+{
+    double v = u - a1*v1 - a2*v2;
+    double y = b0*v + b1*v1 + b2*v2;
+    v2 = v1;
+    v1 = v;
+    return y;
+}
+
 double biquad_rhf(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
     const double b1 , const double b2 )
 {
@@ -234,13 +274,18 @@
 The filtered left signal (llf_y) is shown in channel 1, the filtered right signal (rlf_y)is shown in channel 0 (scope.set)*/
 void scopeSend(void){
     lno_y = biquad_lno(emgl.read(), lno_v1, lno_v2, lno_a1, lno_a2, lno_b0, lno_b1, lno_b2);
-    lhf_y = biquad_lhf(lno_y, lhf_v1, lhf_v2, lhf_a1, lhf_a2, lhf_b0, lhf_b1, lhf_b2);
+    lno2_y = biquad_lno2(lno_y, lno2_v1, lno2_v2, lno2_a1, lno2_a2, lno2_b0, lno2_b1, lno2_b2);
+    lhf_y = biquad_lhf(lno2_y, lhf_v1, lhf_v2, lhf_a1, lhf_a2, lhf_b0, lhf_b1, lhf_b2);
     lrect_y = fabs(lhf_y);
     llf_y = biquad_llf(lrect_y, llf_v1, llf_v2, llf_a1, llf_a2, llf_b0, llf_b1, llf_b2)/0.2;
     rno_y = biquad_rno(emgr.read(), rno_v1, rno_v2, rno_a1, rno_a2, rno_b0, rno_b1, rno_b2);
-    rhf_y = biquad_rhf(rno_y, rhf_v1, rhf_v2, rhf_a1, rhf_a2, rhf_b0, rhf_b1, rhf_b2);
+    rno2_y = biquad_rno2(rno_y, rno2_v1, rno2_v2, rno2_a1, rno2_a2, rno2_b0, rno2_b1, rno2_b2);
+    rhf_y = biquad_rhf(rno2_y, rhf_v1, rhf_v2, rhf_a1, rhf_a2, rhf_b0, rhf_b1, rhf_b2);
     rrect_y = fabs(rhf_y);
     rlf_y = biquad_rlf(rrect_y, rlf_v1, rlf_v2, rlf_a1, rlf_a2, rlf_b0, rlf_b1, rlf_b2)/0.2;  
+       scope.set(1, llf_y);
+    scope.set(0, rlf_y);
+    scope.send();
     }
 
 
@@ -254,7 +299,7 @@
             if(led_r == LedOn){
                 num_turned_on_0++;
             
-            if (rlf_y > threshold_value) {
+            if (rlf_y > threshold_value) {            
                     if(position_cart <= -95){                                  //If the cart is at the right side, it stops
                     Cart.stop(1)==1;
                     
@@ -270,8 +315,12 @@
                     }else{
                     Cart.speed(cart_speed)==cart_speed;
                     }
+                    
+                    if (llf_y > threshold_value){
+                    Cart.stop(1)==1;
+                    }
          
-                }else if (llf_y > threshold_value) {
+                }else if (llf_y > threshold_value) {                
                     if(position_cart >= 95){                                   //If the cart is at the left side, it stops
                     Cart.stop(1)==1;
                     
@@ -287,6 +336,10 @@
                     }else{
                     Cart.speed(-cart_speed)==-cart_speed;
                     }
+                    
+                    if (rlf_y > threshold_value){
+                    Cart.stop(1)==1;
+                    }
                                             
                 }else {
                 Cart.stop(1)==1;
@@ -308,8 +361,8 @@
         
             wait(0.1);
             pc.baud(115200);
-            pc.printf("Distance in mm: %i\n", position_cart);
-            
+            pc.printf("Distance in mm: %i\n\c", position_cart);
+                        
             break;
         }
         
@@ -326,21 +379,21 @@
                     }else if(position_cart > -95 && position_arm >= 25 && position_claw == 27){
                     Arm.stop(1)==1;
                         
-                    }else if(position_cart<= -95 && position_arm>=80){         //If the cart is at the right end, the arm can't move any further than 70 degrees
+                    }else if(position_cart<= -95 && position_arm>=50){         //If the cart is at the right end, the arm can't move any further than 70 degrees
                     Arm.stop(1)==1;
     
                     }else{
                     Arm.speed(arm_speed)==arm_speed;
                     } 
          
-                }else if (llf_y > threshold_value) {
+                }else if (llf_y > threshold_value_left) {
                     if(position_cart < 95 && position_arm <= -30){             //If the cart is not at the end, the arm can't move any further than 45 degrees 
                     Arm.stop(1)==1;
                     
                     }else if(position_cart < 95 && position_arm <= -25 && position_claw == -18){
                     Arm.stop(1)==1;
                     
-                    }else if(position_cart>=95 && position_arm<=-80){          //If the cart is at the left end, the arm can't move any further than 70 degrees
+                    }else if(position_cart>=95 && position_arm<=-50){          //If the cart is at the left end, the arm can't move any further than 70 degrees
                     Arm.stop(1)==1;
     
                     }else{
@@ -422,7 +475,7 @@
                         }
                     }
                  
-                }else if(llf_y > threshold_value){
+                }else if(llf_y > threshold_value_left){
                 servo_id --;
                 
                 switch (servo_id) {