Diff: main.cpp

Revision:

46:c8c5c455dd51

Parent:

45:198654304238

Child:

47:c52f9b4c90c4

--- a/main.cpp	Fri Oct 23 10:18:29 2015 +0000
+++ b/main.cpp	Fri Oct 23 14:07:58 2015 +0000
@@ -93,7 +93,10 @@
 double biceps, triceps, flexor, extens;         //Storage for normalized emg
 double xdir, ydir;                              //EMG reference position directions
 double xpower, ypower;                          //EMG reference magnitude
-double dx, dy;                                  //integration of EMG (velocity) to position
+double dx, dy;   
+double emg_control_time;
+
+
 //Threshold moving average
 const int window=100;                           //00 samples
 int i=0;                                        //buffer index 
@@ -101,7 +104,9 @@
 double movavg2[window];
 double movavg3[window];
 double movavg4[window];
-       
+double sum1, sum2, sum3, sum4 ;
+double biceps_avg, triceps_avg,flexor_avg, extens_avg;
+
 int muscle;             //Muscle selector for MVC measurement, 1 = first emg etc.
 double calibrate_time;  //Elapsed time for each MVC measurement
 
@@ -225,8 +230,8 @@
 biquadFilter     lowpass4( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 );                     // EMG envelope    
 
 //PID filter (lowpass ??? Hz)
-biquadFilter     derfilter( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 );   // derivative filter
-
+biquadFilter     derfilter1( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 );   // derivative filter
+biquadFilter     derfilter2( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 );   // derivative filter
 
 /*--------------------------------------------------------------------------------------------------------------------
 ---- DECLARE FUNCTION NAMES ------------------------------------------------------------------------------------------
@@ -274,6 +279,16 @@
     pc.baud(115200);            //serial baudrate
     red=1; green=1; blue=1;     //Make sure debug LEDs are off  
     
+    theta1_cal = floor(theta1_lower * (4200/(2*PI)));
+    Encoder1.setPulses(theta1_cal);       //edited QEI library: added setPulses()
+    
+    //Mechanical limit 43 degrees -> 43*(4200/360) = 350
+    theta2_cal = floor(theta2_lower * (4200/(2*PI)));
+    Encoder2.setPulses(theta2_cal);
+    
+    x = 0.2220;
+    y = 0.4088;
+    
     //Set PwmOut frequency to 10k Hz
     pwm_motor1.period(0.001);    
     pwm_motor2.period(0.001);
@@ -369,7 +384,8 @@
                 pc.printf("=> You chose EMG DLS control \r\n\n");
                 wait(1);
                 start_sampling();
-                x=0; y=0;
+                
+                emg_control_time = 0;
                 wait(1);
                 emg_control=true;
                 control_method=2;
@@ -545,12 +561,14 @@
     while(calibrating){
         shoulder_limit.rise(&shoulder);
         elbow_limit.rise(&elbow);
-        while(!done1){
-        pwm_motor1.write(0.1);     //move upper arm slowly cw
-        }
-        if(done1==true){
-            pwm_motor2.write(0.1);     //move forearm slowly cw
-        }
+        //while(!done1){
+        //pwm_motor1=0.1;     //move upper arm slowly cw
+        
+        //}
+        //if(done1==true){
+           // pwm_motor2=0.1;     //move forearm slowly cw
+            
+        //}
         
         if(done1 && done2){
             calibrating=false;      //Leave while loop when both limits are reached
@@ -789,7 +807,7 @@
 {
     // Derivative
     double e_der = (error-e_prev)/ CONTROL_RATE;
-    e_der = derfilter.step(e_der);
+    e_der = derfilter1.step(e_der);
     e_prev = error;
     // Integral
     e_int = e_int + CONTROL_RATE * error;
@@ -798,14 +816,30 @@
  
 }
 
+//Input error and Kp, Kd, Ki, output control signal
+double pid2(double error, double kp, double ki, double kd,double &e_int, double &e_prev)
+{
+    // Derivative
+    double e_der = (error-e_prev)/ CONTROL_RATE;
+    e_der = derfilter2.step(e_der);
+    e_prev = error;
+    // Integral
+    e_int = e_int + CONTROL_RATE * error;
+    // PID
+    return kp*error + ki*e_int + kd * e_der;
+ 
+}
+
+
 //Analyze filtered EMG, calculate reference position from EMG, compare reference position with current position,convert to angles, send error through pid(), send PWM and DIR to motors 
 void control()
 { 
-    if(emg_control=true){
+    if(emg_control==true){
     //TODO some idle time with static reference before EMG kicks in
-    //emg_control_time += CONTROL_RATE; 
-    // if emg_control_time < 5 seconds, reference is  x=0; y=0;    
-    
+    emg_control_time += CONTROL_RATE; 
+    //if(emg_control_time < 5){
+    //    x=0; y=0;    
+    //}
     //normalize emg to value between 0-1
     biceps = (biceps_power - bicepsmin) / (bicepsMVC - bicepsmin);
     triceps = (triceps_power - tricepsmin) / (tricepsMVC - tricepsmin);
@@ -823,8 +857,8 @@
     scope.send();
     
     //threshold detection! buffer or two thresholds? If average of 100 samples > threshold, then muscle considered on.
-    /* 
-    
+     
+    /*
     movavg1[i]=biceps;   //fill array with 100 normalized samples
     movavg2[i]=triceps;
     movavg3[i]=flexor;
@@ -844,8 +878,8 @@
     triceps_avg = sum2/window;
     flexor_avg = sum3/window;
     extens_avg = sum4/window;
+    
     */
-    
     //analyze emg (= velocity)
     if (biceps>triceps && biceps > 0.2){
         xdir = 0;
@@ -973,7 +1007,7 @@
     //PID controller
     
     u1=pid(m1_error,m1_kp,m1_ki,m1_kd,m1_e_int,m1_e_prev);    //motor 1
-    u2=pid(m2_error,m2_kp,m2_ki,m2_kd,m2_e_int,m2_e_prev);    //motor 2
+    u2=pid2(m2_error,m2_kp,m2_ki,m2_kd,m2_e_int,m2_e_prev);    //motor 2
     
     keep_in_range(&u1,-0.6,0.6);    //keep u between -1 and 1, sign = direction, PWM = 0-1
     keep_in_range(&u2,-0.6,0.6);