Diff: main.cpp

Revision:

5:867fe891b990

Parent:

4:c371fc59749e

Child:

6:bfd24400e9d0

--- a/main.cpp	Wed Oct 21 14:02:07 2015 +0000
+++ b/main.cpp	Thu Oct 22 11:34:39 2015 +0000
@@ -6,7 +6,6 @@
 
 
 ///// points..... mooi maken calib
-AnalogOut setpoint_analog(DAC0_OUT);
 Serial pc(USBTX,USBRX);
 HIDScope scope(4);          // definieerd het aantal kanalen van de scope
 
@@ -39,9 +38,16 @@
 ///////////////////////////
 // Main control loop transfer variables
 // here all variables that transfer bewtween the primary control functions
+
+// filter to calibration
 double output1;
 double output2;
 
+// filter to x-y
+double output1_amp;
+double output2_amp;
+
+// x-y to motor control
 double phi_one;
 double phi_two;
 
@@ -78,9 +84,6 @@
 
 // REFERENCE SIGNAL SETTINGS
     double input_threshold = 0.25;   // the minimum value the signal must have to change the reference.
-    // Define signal amplification  (needed with EMG, used in control loop, precise amp determination is a work in progress!!!!)    ??
-    double signalamp1 = 1;
-    double signalamp2 = 1;
  // Define storage variables for reference values
     double c_reference1 = 0;
     double c_reference2 = 0;
@@ -120,15 +123,11 @@
     double m2_prev_err = 0;
 
 // EMG calibration variables
-double emg_gain1 = 7;
-double emg_gain2 = 7;
-
-////// MOET NETTER!!!!!
-double output1_1 = 0;
-double output2_1 = 0;
-
+double emg_gain1 = 1;       // set to one for unamplified signal
+double emg_gain2 = 1;
 
 double cal_samples = 25;
+double normalize_emg_value = 1;      // set te desired value to calibrate the signal to
 
 //// FILTER VARIABLES
 // storage variables
@@ -214,7 +213,6 @@
 
 // This function takes the controller outputvalue and ensures it is between -1 and 1
 // this is done to limit the motor input to possible values (the motor takes 0 to 1 and the sign changes the direction).
-// needs more work to use it for the wind-up prevention.
 double outputlimiter (double output, double limit)
 {
     if(output> limit)                                
@@ -248,6 +246,7 @@
     }
 
 // biquadfilters die bij het filteren van signaal 2 horen, copy paste, alle waardes zijn veranderd naar +t (t van two of twee)
+// (niet netjes maar werkt goed)
 double biquadfiltert(double ut, double &v1t, double &v2t, const double a1t, const double a2t, const double b0t, const double b1t, const double b2t)
 {
     double vt = ut- a1t*v1t-a2t*v2t;
@@ -258,7 +257,7 @@
 }
 
 // PID Controller given in sheets
-// aangepast om zelfde filter te gebruiken en om de termen later gesplitst te kunnen limiteren (windup preventie!!)
+// aangepast om zelfde filter te gebruiken en om de termen te splitsen
 double PID(double e, const double Kp, const double Ki, const double Kd, double Ts,double &e_int, double &e_prev)
 {
 // Proportional
@@ -277,39 +276,103 @@
 return output;
 }
 
-void calibrate_amp()
+ double angle_limits(double phi, double limlow, double limhigh)
 {
-    //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);
+    if(phi < limlow)
+    {
+        phi = limlow;
+    }
+    if(phi > limhigh)
+    {
+        phi = limhigh;
+    }
+    return phi;
+}
 
-}
 /////////////////////////////////////////////////////////////////////
 ////////////////// PRIMARY CONTROL FUNCTIONS ///////////////////////
 ///////////////////////////////////////////////////////////////////
 // these functions are called by go-flags and are used to update main variables and send signals to motor
 
+// function that updates the inputs
+void EMG_filter()
+{
+// filteren van EMG signaal 1 (A0) eerst notch(2 biquads), dan highpass, rectify(abs()), lowpass
+    double u1 = EMG_in.read();
+    double y1 = biquadfilter( u1, f1_v1, f1_v2,dennotch50biq1_2, dennotch50biq1_3,numnotch50biq1_1,numnotch50biq1_2,numnotch50biq1_3);
+    double y2 = biquadfilter( y1, f2_v1, f2_v2,dennotch50biq2_2, dennotch50biq2_3,numnotch50biq2_1,numnotch50biq2_2,numnotch50biq2_3);
+    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);
+    // update global variables
+    output1 = y5;
+    output1_amp = y5*emg_gain1;   // update global variable
+
+// filteren van EMG signaal 2 (A2), zelfde proces als signaal 1
+    double u1t = EMG_int.read();
+    double y1t = biquadfiltert( u1t, f1_v1t, f1_v2t,dennotch50biq1_2, dennotch50biq1_3,numnotch50biq1_1,numnotch50biq1_2,numnotch50biq1_3);
+    double y2t = biquadfiltert( y1t, f2_v1t, f2_v2t,dennotch50biq2_2, dennotch50biq2_3,numnotch50biq2_1,numnotch50biq2_2,numnotch50biq2_3);
+    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);
+    // update global variables
+    output2 = y5t;
+    output2_amp = y5t*emg_gain2; 
+    
+    scope.set(0,output1_amp);
+    scope.set(1,output2_amp);
+    scope.send();
+ }
+ 
+ 
+ // function that updates the required motor angles
+ void det_angles()
+{
+    
+    
+    if(output1>1) {output1 = 1;}
+    if(output2>1) {output2 = 1;}
+    
+    output1 = potright.read();
+    output2 = potleft.read();
+    
+    double xx = 50+output1_amp*20;
+    
+    double ymin = - sqrt(4900 - pow(xx,2));
+    double ymax = sqrt(4900 - pow(xx,2));
+    double yy = ymin+output2_amp*(ymax-ymin);
+    double r = sqrt(pow(xx,2)+pow(yy,2)); // vector naar end effector
+    double alfa = acos((2*pow(L,2)-pow(r,2))/(2*pow(L,2))); // alfa is de hoek tussen upper en lower arm
+    double beta = acos((pow(r,2))/(2*L*r)); // beta is de hoek tussen upper arm en r
+    
+    double theta_one = (atan2(yy,xx)+beta); 
+    double theta_two = (-pi + alfa); 
+    
+    double phi1 = 4*(theta_one) + 2.8;
+    double phi2 = 4*(theta_one+theta_two) + 1.85;
+    phi2 = -phi2;
+
+    // check the input angles and apply the limits
+    phi1 = angle_limits(phi1,limlow1,limhigh1);
+    phi2 = angle_limits(phi2,limlow2,limhigh2);
+    
+    // smooth the input signal (lowpass 1Hz)    
+    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);
+    
+    // write into global variables
+    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);
+}
 // MOTOR 1
 void motor1_control()
 {
        
     double reference1 = phi_one;  
     
+    // add smooth start up
     if(rc1 < start_loops)
     {
         rc1++;
@@ -319,7 +382,7 @@
     {
         reference1 = reference1;
     }
-        // m_input1 = biquadfilter(m_input1, r1_f_v1, r1_f_v2, r1_f_a1, r1_f_a2, r1_f_b0, r1_f_b1, r1_f_b2);        //biquad with diff-filter  coefficients to smooth input
+    
     double rads1 = get_radians(motor1_enc.getPosition());    // determine the position of the motor
     double error1 = (reference1 - rads1);                       // determine the error (reference - position)
     double m_output1 = PID(error1, m1_Kp, m1_Ki, m1_Kd, controlstep, m1_err_int, m1_prev_err);
@@ -363,84 +426,26 @@
     }
 }
 
-
-// function that updates the inputs
-void EMG_filter()
-{
-// filteren van EMG signaal 1 (A0) eerst notch(2 biquads), dan highpass, rectify(abs()), lowpass
-    double u1 = EMG_in.read();
-    double y1 = biquadfilter( u1, f1_v1, f1_v2,dennotch50biq1_2, dennotch50biq1_3,numnotch50biq1_1,numnotch50biq1_2,numnotch50biq1_3);
-    double y2 = biquadfilter( y1, f2_v1, f2_v2,dennotch50biq2_2, dennotch50biq2_3,numnotch50biq2_1,numnotch50biq2_2,numnotch50biq2_3);
-    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_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
-    double u1t = EMG_int.read();
-    double y1t = biquadfiltert( u1t, f1_v1t, f1_v2t,dennotch50biq1_2, dennotch50biq1_3,numnotch50biq1_1,numnotch50biq1_2,numnotch50biq1_3);
-    double y2t = biquadfiltert( y1t, f2_v1t, f2_v2t,dennotch50biq2_2, dennotch50biq2_3,numnotch50biq2_1,numnotch50biq2_2,numnotch50biq2_3);
-    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_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
- void det_angles()
+// calibrate the emg-signal
+// works bij taking a certain amount of samples adding them and then normalize to the desired value
+void calibrate_amp()
 {
-    // potmeter debugging
-    // static double time = 0.0;
-    // const double time_inc = 0.005;
-    // setpoint_analog= 0.5+(sin(time)/2.0);
-    // time += time_inc;    
-    
-    //output1 = potright.read();
+    double total1 = 0;
+    double total2 = 0;
 
-    //output2 = potleft.read();
-    
-    if(output1>1) {output1 = 1;}
-    if(output2>1) {output2 = 1;}
-    
-    double xx = 50+output1*20;
-    
-    double ymin = - 16;
-    double ymax = + 16;
-    double yy = ymin+output2*(ymax-ymin);
-    double r = sqrt(pow(xx,2)+pow(yy,2)); // vector naar end effector
-    double alfa = acos((2*pow(L,2)-pow(r,2))/(2*pow(L,2))); // alfa is de hoek tussen upper en lower arm
-    double beta = acos((pow(r,2))/(2*L*r)); // beta is de hoek tussen upper arm en r
-    
-    double theta_one = (atan2(yy,xx)+beta); 
-    double theta_two = (-pi + alfa); 
-    
-    double phi1 = 4*(theta_one) + 2.8;
-    double phi2 = 4*(theta_one+theta_two) + 1.85;
-    phi2 = -phi2;
-
-   
-    if(phi1 < limlow1){phi1 = limlow1;}
-    if(phi1 > limhigh1){phi1 = limhigh1;}
-    
-    if(phi2 < limlow2){phi2 = limlow2;}           
-    if(phi2 > limhigh2){phi2 = limhigh2;}
-    
-    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);
-
-    
+    for(int i = 0; i<cal_samples; i++)
+    {
+        EMG_filter();       // run filter
+        double input1 = output1;
+        total1 = total1 + input1;       // sum inputs
+        
+        double input2 = output2;
+        total2 = total2 + input2;
+        wait(0.1);
+    }
+    emg_gain1 = normalize_emg_value/(total1/cal_samples);  // normalize the amplification so that the maximum signal hits the desired one
+    emg_gain2 = normalize_emg_value/(total2/cal_samples);
+   pc.printf("gain1 = %f, gain2 = %f",emg_gain1,emg_gain2);
 
 }
 //////////////////////////////////////////////////////////////////