Revision 36:4d4fc200171b, committed 2015-10-22

Comitter:

Vigilance88

Date:

Thu Oct 22 23:19:50 2015 +0000

Parent:

35:7d9fca0b1545

Child:

37:4d7b7ced20ef

Commit message:

DLS and Trig control loops work.

Changed in this revision

--- a/QEI.lib	Thu Oct 22 14:18:41 2015 +0000
+++ b/QEI.lib	Thu Oct 22 23:19:50 2015 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/aberk/code/QEI/#5c2ad81551aa
+http://mbed.org/users/aberk/code/QEI/#378b29004b9a

--- a/main.cpp	Thu Oct 22 14:18:41 2015 +0000
+++ b/main.cpp	Thu Oct 22 23:19:50 2015 +0000
@@ -23,7 +23,7 @@
 
 MODSERIAL pc(USBTX,USBRX);      //serial communication
 
-//Debug legs
+//Debug LEDs
 DigitalOut red(LED_RED);
 DigitalOut green(LED_GREEN);
 DigitalOut blue(LED_BLUE);
@@ -83,24 +83,25 @@
 ---- DECLARE VARIABLES -----------------------------------------------------------------------------------------------
 --------------------------------------------------------------------------------------------------------------------*/
 
-//EMG variables: raw EMG - filtered EMG - maximum voluntary contraction
-double emg_biceps; double biceps_power; double bicepsMVC = 0; double bicepsmin;
-double emg_triceps; double triceps_power; double tricepsMVC = 0; double tricepsmin;
-double emg_flexor; double flexor_power; double flexorMVC = 0; double flexormin;
-double emg_extens; double extens_power; double extensMVC = 0; double extensmin;
+//EMG variables: raw EMG - filtered EMG - maximum voluntary contraction - minimum amplitude during relaxation. 
+//minimum declared as 1 so the comparison with EMG during calibration works correctly - function emg_min()
+double emg_biceps; double biceps_power; double bicepsMVC = 0; double bicepsmin=1;
+double emg_triceps; double triceps_power; double tricepsMVC = 0; double tricepsmin=1;
+double emg_flexor; double flexor_power; double flexorMVC = 0; double flexormin=1;
+double emg_extens; double extens_power; double extensMVC = 0; double extensmin=1;
 
-int muscle;             //Muscle selector for MVC measurement
+int muscle;             //Muscle selector for MVC measurement, 1 = first emg etc.
 double calibrate_time;  //Elapsed time for each MVC measurement
 
 //PID variables
-double u1; double u2;                                               //Output of PID controller (PWM value for motor 1 and 2)
-double m1_error=0; double m1_e_int=0; double m1_e_prev=0;           //Error, integrated error, previous error
-const double m1_kp=0.1; const double m1_ki=0.0125; const double m1_kd=0.02;   //Proportional, integral and derivative gains.
+double u1; double u2;                                                         //Output of PID controller (PWM value for motor 1 and 2)
+double m1_error=0; double m1_e_int=0; double m1_e_prev=0;                     //Error, integrated error, previous error
+const double m1_kp=8; const double m1_ki=0.125; const double m1_kd=0.5;   //Proportional, integral and derivative gains.
 
-double m2_error=0; double m2_e_int=0; double m2_e_prev=0;           //Error, integrated error, previous error
-const double m2_kp=0.1; const double m2_ki=0.0125; const double m2_kd=0.02;   //Proportional, integral and derivative gains.
+double m2_error=0; double m2_e_int=0; double m2_e_prev=0;                     //Error, integrated error, previous error
+const double m2_kp=8; const double m2_ki=0.125; const double m2_kd=0.5;   //Proportional, integral and derivative gains.
 
-//Calibration, checks if elbow/shoulder are done
+//Calibration bools, checks if elbow/shoulder limits are hit
 bool done1 = false;
 bool done2 = false;
 
@@ -112,10 +113,7 @@
 const double high_a2 = 0.9149758348014341;
  
 //notchfilter 50Hz
-/*  ** Primary Filter (H1)**
-Filter Arithmetic = Floating Point (Double Precision)
-Architecture = IIR
-Response = Bandstop
+/* 
 Method = Butterworth
 Biquad = Yes
 Stable = Yes
@@ -152,19 +150,28 @@
 const double low_a1 = -1.968902268531908;
 const double low_a2 = 0.9693784555043481;
 
-//Forward and Inverse Kinematics
-const double l1 = 0.25; const double l2 = 0.25;
-double current_x; double current_y;
-double theta1; double theta2;
-double dtheta1; double dtheta2;
-double rpc;
+//Forward Kinematics
+const double l1 = 0.25; const double l2 = 0.25;     //Arm lengths
+double current_x; double current_y;                 //Current position
+double theta1; double theta2;                       //Current angles
+double rpc;                                         //Encoder resolution: radians per count
+
+//Reference position
 double x; double y;
-double x_error; double y_error;
-double costheta1; double sintheta1;
-double costheta2; double sintheta2;
-double dls1, dls2, dls3, dls4;
-double q1_error, q2_error;
-double lambda=0.1;
+
+//Inverse Kinematics - Trig / Gonio method. 
+//First convert reference position to angle needed, then compare that angle to current angle.
+double dtheta1; double dtheta2;                     //reference angles
+double costheta1; double sintheta1;                 //helper variables
+double costheta2; double sintheta2;                 //    
+
+//Inverse Kinematics - Damped least squares method. 
+//Angle error is directly calculated from position error: dq = [DLS matrix] * position_error
+                                                    //             |DLS1 DLS2|
+double dls1, dls2, dls3, dls4;                      //DLS matrix:  |DLS3 DLS4|
+double q1_error, q2_error;                          //Angle errors
+double x_error; double y_error;                     //Position errors
+double lambda=0.1;                                  //Damping constant
 
 /*--------------------------------------------------------------------------------------------------------------------
 ---- Filters ---------------------------------------------------------------------------------------------------------
@@ -253,24 +260,17 @@
     // make a menu, user has to initiate next step
     titleBox();
     mainMenu();
-    //caliMenu();            // Menu function
-    //calibrate_arm();        //Start Calibration
-    
-    //calibrate_emg();        
-    
+   
     //set initial reference position
-    x=0.5; y=0;
-    
-    //start_control();        //100 Hz control
+    x = 0.5;
+    y = 0;
+    theta1=0.6980;
+    theta2=0.5200;
     
     //maybe some stop commands when a button is pressed: detach from timers.
     //stop_control();
     //start_sampling();
     
-    //char c;
-    
-    
-    
     char command=0;
     
     while(command != 'Q' && command != 'q')
@@ -425,7 +425,9 @@
     if(c!='q' && c!='Q'){
     pc.printf("Reference position: %f and %f \r\n",x,y);
     pc.printf("Current position: %f and %f \r\n",current_x,current_y);
+    pc.printf("Pos Error: %f and %f \r\n",x_error,y_error);
     pc.printf("Current angles: %f and %f \r\n",theta1,theta2);
+    pc.printf("DLS1: %f and DLS2 %f and DLS3 %f and DLS4: %f \r\n",dls1,dls2,dls3,dls4);
     pc.printf("Error in angles: %f and %f \r\n",q1_error,q2_error);
     pc.printf("PID output: %f and %f \r\n",u1,u2);
     pc.printf("----------------------------------------\r\n\n");
@@ -486,6 +488,9 @@
     Encoder1.reset();
     done1 = true;
     pc.printf("Shoulder Limit hit - shutting down motor 1\r\n");
+    //mechanical angle limits -> pulses. If 40 degrees -> counts = floor( 40 * (4200/360) )
+    //Encoder1.setPulses(467);       //edited QEI library: added setPulses()
+    
 }
 
 void elbow(){
@@ -493,6 +498,9 @@
     Encoder2.reset();
     done2 = true;
     pc.printf("Elbow Limit hit - shutting down motor 2\r\n");
+    //Mechanical limit 30 degrees -> 30*(4200/360) = 350
+    
+    //Encoder2.setPulses(350);       //edited QEI library: added setPulses()
 }
     
 //Send arm to mechanical limits, and set encoder to 0. Then send arm to starting position.
@@ -508,14 +516,17 @@
     pc.printf("To start arm calibration, press any key =>");
     pc.getc();
     pc.printf("\r\n Calibrating... \r\n");
-    dir_motor1=1;   //cw
-    dir_motor2=0;   //cw
-    pwm_motor1.write(0.2);     //move upper arm slowly cw
+    dir_motor1=0;   //cw
+    dir_motor2=1;   //cw
+    
        
     
     while(calibrating){
         shoulder_limit.rise(&shoulder);
         elbow_limit.rise(&elbow);
+        while(!done1){
+        pwm_motor1.write(0.2);     //move upper arm slowly cw
+        }
         if(done1==true){
             pwm_motor2.write(0.2);     //move forearm slowly cw
         }
@@ -528,9 +539,7 @@
     }//end while
 
     //TO DO:
-    //mechanical angle limits -> pulses. If 40 degrees -> counts = floor( 40 * (2*pi/4200) )
-    //Encoder1.setPulses(100);       //edited QEI library: added setPulses()
-    //Encoder2.setPulses(100);       //edited QEI library: added setPulses()
+    
     //pc.printf("Elbow Limit hit - shutting down motor 2. Current pulsecount: %i \r\n",Encoder1.getPulses());
     
     wait(1);
@@ -593,9 +602,7 @@
         
     //}
     calibrate_time = calibrate_time + 0.002;
-    
-    
-   
+
 }
 
 void emg_min()
@@ -628,7 +635,7 @@
    
    pc.printf("Testcode calibration \r\n");
    wait(1);
-   pc.printf("Starting minimum measurement, relax all muscles.");
+   pc.printf("Starting minimum measurement, relax all muscles.\r\n");
    wait(1);
    pc.printf(" Press any key to begin... "); wait(1);
    char input;
@@ -638,6 +645,7 @@
    pc.printf(" \r\n  Starting in 1... \r\n"); wait(1);
    
    start_sampling();
+   
    timer.attach(&emg_min,SAMPLE_RATE);
    wait(5);
    timer.detach();
@@ -1221,12 +1229,14 @@
 }
 
 void emgInstructions(){
-    pc.printf("\r\nPrepare the skin before applying electrodes: \n\r"); wait(1);
+    pc.printf("\r\nPrepare the skin before applying electrodes: \n\r"); 
     pc.printf("-> Shave electrode locations if needed and clean with alcohol \n\r"); wait(1);
-    pc.printf("\n\r Relax for a few minutes after electrodes are placed and check if EMG signal looks normal \n\r "); wait(1);
-    pc.printf("\n\r To calibrate the EMG signals we will measure the Maximum Voluntary Contraction of each muscle \n\r"); wait(1);
-    pc.printf("You will need to flex the mentioned muscle as much as possible for 5 seconds \n\r"); wait(1);
-    pc.printf("The measurement will begin once you confirm you're ready [Hit any key] \n\r \n\r"); wait(1);
+    pc.printf("\n\r Check if EMG signal looks normal \n\r "); wait(1);
+    pc.printf("\n\r To calibrate the EMG signals we will measure: \n\r ");
+    pc.printf("\n\r - Minimum amplitude, while relaxing all muscles. \n\r ");
+    pc.printf("\n\r - Maximum Voluntary Contraction of each muscle. \n\r"); wait(2);
+    pc.printf("For the MVC you need to flex the mentioned muscle as much as possible for 5 seconds \n\r"); wait(1);
+    pc.printf("The measurements will begin once you confirm you're ready [Hit any key] \n\r \n\r"); wait(1);
 }
 
 //keeps input limited between min max