Diff: main.cpp

Revision:

4:c371fc59749e

Parent:

3:a73502236647

Child:

5:867fe891b990

--- a/main.cpp	Wed Oct 21 12:27:45 2015 +0000
+++ b/main.cpp	Wed Oct 21 14:02:07 2015 +0000
@@ -4,6 +4,8 @@
 #include "HIDScope.h"
 #include "math.h"
 
+
+///// points..... mooi maken calib
 AnalogOut setpoint_analog(DAC0_OUT);
 Serial pc(USBTX,USBRX);
 HIDScope scope(4);          // definieerd het aantal kanalen van de scope
@@ -50,8 +52,8 @@
     AnalogIn potleft(A5);
     
 // Analoge input signalen defineren
-AnalogIn    EMG_in(A2);                 // EMG_in.read kan je nu gebruiken om het analoge signaal A2 uit te lezen
-AnalogIn    EMG_int(A3);                // deze leest A3 uit
+AnalogIn    EMG_in(A0);                 // EMG_in.read kan je nu gebruiken om het analoge signaal A2 uit te lezen
+AnalogIn    EMG_int(A2);                // deze leest A3 uit
 
 // BUTTONS 
     // control flow             
@@ -118,8 +120,14 @@
     double m2_prev_err = 0;
 
 // EMG calibration variables
-double emg_gain1 = 0;
-double emg_gain2 = 0;
+double emg_gain1 = 7;
+double emg_gain2 = 7;
+
+////// MOET NETTER!!!!!
+double output1_1 = 0;
+double output2_1 = 0;
+
+
 double cal_samples = 25;
 
 //// FILTER VARIABLES
@@ -271,25 +279,24 @@
 
 void calibrate_amp()
 {
-    double total1 = 0;
-    for(int i = 0; i<cal_samples; i++)
-    {
-        double input1 = EMG_in.read();
-        total1 = total1 + input1;
-        wait(0.1);
-    }
-    emg_gain1 = 1/(total1/cal_samples);
-   // pc.printf("gain1 = %f",emg_gain1);
-    
-    double total2 = 0;
-    for(int i = 0; i<cal_samples; i++)
-    {
-        double input2 = EMG_int.read();
-        total2 = total2 + input2;
-        wait(0.1);
-    }
-    emg_gain2 = 1/(total2/cal_samples);
-   // pc.printf("gain2 = %f",emg_gain2);
+    //double total1 = 0;
+    //for(int i = 0; i<cal_samples; i++)
+    //{
+    //    double input1 = EMG_in.read();
+    //    total1 = total1 + input1;
+    //    wait(0.1);
+    //}
+    emg_gain1 = 1/output1_1;  //1/(total1/cal_samples);
+       
+    //double total2 = 0;
+    //for(int i = 0; i<cal_samples; i++)
+    //{
+    //    double input2 = output2_1;
+    //    total2 = total2 + input2;
+    //    wait(0.1);
+    //}
+    emg_gain2 = 1/output2_1;
+   pc.printf("gain1 = %f, gain2 = %f",emg_gain1,emg_gain2);
 
 }
 /////////////////////////////////////////////////////////////////////
@@ -367,7 +374,8 @@
     double y3 = biquadfilter( y2, f3_v1, f3_v2, denhigh20_2,denhigh20_3,numhigh20_1, numhigh20_2, numhigh20_3);
     double y4 = abs(y3);
     double y5 = biquadfilter( y4, f4_v1, f4_v2, denlow5_2,denlow5_3,numlow5_1, numlow5_2, numlow5_3);
-    output1 = y5;    // update global variable
+    output1_1 = y5;
+    output1 = y5*emg_gain1;   // update global variable
 // versturen van het input signaal u1 en het gefilterde signaal y5 naar HIDScope channel 0 en 1
 
 // filteren van EMG signaal 2 (A2), zelfde proces als signaal 1
@@ -377,7 +385,12 @@
     double y3t = biquadfiltert( y2t, f3_v1t, f3_v2t, denhigh20_2,denhigh20_3,numhigh20_1, numhigh20_2, numhigh20_3);
     double y4t = abs(y3t);
     double y5t = biquadfiltert( y4t, f4_v1t, f4_v2t, denlow5_2,denlow5_3,numlow5_1, numlow5_2, numlow5_3);
-    output2 = y5t; // update global variable
+    output2_1 = y5t;
+    output2 = y5t*emg_gain2; // update global variable
+    
+    scope.set(0,output1);
+    scope.set(1,output2);
+    scope.send();
  }
  
  // function that updates the required motor angles
@@ -389,9 +402,13 @@
     // setpoint_analog= 0.5+(sin(time)/2.0);
     // time += time_inc;    
     
-    output1 = potright.read();
+    //output1 = potright.read();
 
-    output2 = potleft.read();
+    //output2 = potleft.read();
+    
+    if(output1>1) {output1 = 1;}
+    if(output2>1) {output2 = 1;}
+    
     double xx = 50+output1*20;
     
     double ymin = - 16;
@@ -407,11 +424,8 @@
     double phi1 = 4*(theta_one) + 2.8;
     double phi2 = 4*(theta_one+theta_two) + 1.85;
     phi2 = -phi2;
-    
-    scope.set(0,output1);
-    scope.set(1,output2);
-    scope.send();
-    
+
+   
     if(phi1 < limlow1){phi1 = limlow1;}
     if(phi1 > limhigh1){phi1 = limhigh1;}
     
@@ -420,11 +434,11 @@
     
     phi1 = biquadfilter(phi1, r1_f_v1, r1_f_v2, r1_f_a1, r1_f_a2, r1_f_b0, r1_f_b1, r1_f_b2);
     phi2 = biquadfilter(phi2, r2_f_v1, r2_f_v2, r1_f_a1, r1_f_a2, r1_f_b0, r1_f_b1, r1_f_b2);
-        
+    
     phi_one = phi1;
     phi_two = phi2;
     
-    //pc.printf("x = %f, y = %f, phi_one = %f, phi_two = %f \n",xx,yy,phi_one,phi_two);
+    pc.printf("x = %f, y = %f, phi_one = %f, phi_two = %f \n",xx,yy,phi_one,phi_two);
 
     
 
@@ -454,7 +468,7 @@
 int main()
 {
     pc.baud(115200);
-    // main_filter.attach(&EMG_activate, controlstep);
+    main_filter.attach(&EMG_activate, controlstep);
     cartesian.attach(&angle_activate, controlstep);
     controller1.attach(&motor1_activate, controlstep);      // call a go-flag
     controller2.attach(&motor2_activate, controlstep);