Diff: main.cpp

Revision:

41:e9d6fdf02074

Parent:

40:e217056584be

Child:

42:bcd496523c66

--- a/main.cpp	Wed Oct 31 14:24:58 2018 +0000
+++ b/main.cpp	Wed Oct 31 15:05:04 2018 +0000
@@ -65,6 +65,7 @@
 
 
 // Variables for calibration
+int n;
 double calib_q1 = 3.1415926535f;
 double calib_q2 = 1.5f*3.1415926535f;
 double off_set_q1 = 0; // This variable is used to determine the off set from our definition from q1 and q2.
@@ -101,7 +102,7 @@
     forwardkinematics_function(q1,q2,x,y);  
     raw_emg_0 = emg0.read(); //sample analog voltages (all sampling theory applies, you might get aliasing etc.)
     raw_emg_1 = emg1.read();
-    processing_chain_emg(raw_emg_0, raw_emg_1, process_emg_0, process_emg_1);  // processes the emg signals
+    processing_chain_emg(raw_emg_0, raw_emg_1, raw_emg_2, raw_emg_3, process_emg_0, process_emg_1, process_emg_2, process_emg_3);  // processes the emg signals
 }
 
 void output_all() {
@@ -304,7 +305,12 @@
             else if(y_norm <= -0.4) { des_vy = -0.2; }
             else if(y_norm <= -0.2) { des_vy = -0.1; }
             else { des_vy = 0; }
-                        
+            
+            if( n % 100 == 0){
+                pc.printf("%f",des_vx);
+                pc.printf("%f /r/n",des_vy);
+            }
+            
             if (button.read() == true && button_suppressed == false ) {
                 current_state = demo; 
                 state_changed = true;
@@ -391,8 +397,10 @@
     current_state = waiting; //we start in state ‘waiting’ and current_state can be accessed by all functions
     u1 = 0.0f; //initial output to motors is 0.
     u2 = 0.0f;
-    bqc0.add(&bq0high).add(&bq0notch);        // filter cascade for emg
-    bqc1.add(&bq1high).add(&bq1notch);        // filter cascade for emg
+    bqc0.add(&bq0high).add(&bq0notch).add(&bq0notch2).add(&bq0notch3).add(&bq0notch4);        // filter cascade for emg 0
+    bqc1.add(&bq1high).add(&bq1notch).add(&bq1notch2).add(&bq1notch3).add(&bq1notch4);        // filter cascade for emg 1
+    bqc2.add(&bq2high).add(&bq2notch).add(&bq2notch2).add(&bq2notch3).add(&bq2notch4);        // filter cascade for emg 2
+    bqc3.add(&bq3high).add(&bq3notch).add(&bq3notch2).add(&bq3notch3).add(&bq3notch4);        // filter cascade for emg 3
     loop_ticker.attach(&loop_function, T); //Run the function loop_function 1000 times per second
     led_R = 1;
     led_B = 1;