Diff: main.cpp

Revision:

47:c52f9b4c90c4

Parent:

46:c8c5c455dd51

Child:

48:a1f97eb8c020

diff -r c8c5c455dd51 -r c52f9b4c90c4 main.cpp
--- a/main.cpp	Fri Oct 23 14:07:58 2015 +0000
+++ b/main.cpp	Fri Oct 23 20:13:06 2015 +0000
@@ -83,44 +83,44 @@
 ---- DECLARE VARIABLES -----------------------------------------------------------------------------------------------
 --------------------------------------------------------------------------------------------------------------------*/
 
-//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()
+//EMG variables: raw EMG - filtered EMG - maximum voluntary contraction - max amplitude during relaxation. 
 double emg_biceps; double biceps_power; double bicepsMVC = 0; double bicepsmin=0;  
 double emg_triceps; double triceps_power; double tricepsMVC = 0; double tricepsmin=0;
 double emg_flexor; double flexor_power; double flexorMVC = 0; double flexormin=0;
 double emg_extens; double extens_power; double extensMVC = 0; double extensmin=0;
+
 //Normalize and compare variables
 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;   
-double emg_control_time;
-
+double dx, dy;                                  //Integral
+double emg_control_time;                        //Elapsed time in EMG control
 
 //Threshold moving average
-const int window=100;                           //00 samples
-int i=0;                                        //buffer index 
-double movavg1[window];                         //moving average arrays
+const int window=100;                           //100 samples
+int i=0;                                        //movavg array index 
+double movavg1[window];                         //moving average arrays with size of window
 double movavg2[window];
 double movavg3[window];
 double movavg4[window];
-double sum1, sum2, sum3, sum4 ;
-double biceps_avg, triceps_avg,flexor_avg, extens_avg;
+double sum1, sum2, sum3, sum4;                  //sum of the entire window
+double biceps_avg, triceps_avg,flexor_avg, extens_avg;  //sum divided by window size
 
 int muscle;             //Muscle selector for MVC measurement, 1 = first emg etc.
-double calibrate_time;  //Elapsed time for each MVC measurement
+double calibrate_time;  //Elapsed time for each 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=8; const double m1_ki=0.125; const double m1_kd=0.5;   //Proportional, integral and derivative gains.
+double m1_error=0; double m1_e_int=0; double m1_e_prev=0;                     //Error, integrated error, previous error motor 1
+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=8; const double m2_ki=0.125; const double m2_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 motor 2
+const double m2_kp=8; const double m2_ki=0.125; const double m2_kd=0.5;       //Proportional, integral and derivative gains.
 
 //Calibration bools, checks if elbow/shoulder limits are hit
-bool done1 = false;
-bool done2 = false;
+volatile bool done1 = false;
+volatile bool done2 = false;
+volatile bool calibrating = false;
 
 //highpass filter 20 Hz
 const double high_b0 = 0.956543225556877;
@@ -191,7 +191,7 @@
                                                     //             |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 x_error, y_error;                            //Position errors
 double lambda=0.1;                                  //Damping constant
 
 //Mechanical Limits
@@ -247,15 +247,14 @@
 void start_sampling(void);  //Attaches the sample_filter function to a 500Hz ticker
 void stop_sampling(void);   //Stops sample_filter
 void start_control(void);   //Attaches the control function to a 100Hz ticker
-void start_dlscontrol(void);//Attaches DLS control function to a 100Hz ticker    
 void stop_control(void);    //Stops control function
 
-
 //Keeps the input between min and max value
 void keep_in_range(double * in, double min, double max);
 
 //Reusable PID controller, previous and integral error need to be passed by reference
 double pid(double error, double kp, double ki, double kd,double &e_int, double &e_prev);
+double pid2(double error, double kp, double ki, double kd,double &e_int, double &e_prev);
 
 //Menu functions
 void mainMenu();
@@ -267,6 +266,7 @@
 void titleBox();
 
 //Limit switches - power off motors if switches hit (rising edge interrupt)
+void calibrate(void);
 void shoulder();
 void elbow();
 
@@ -298,15 +298,7 @@
     // make a menu, user has to initiate next step
     titleBox();
     mainMenu();
-   
-    //set initial reference position
-    //x = 0.5;
-    //y = 0;
-    
-    //maybe some stop commands when a button is pressed: detach from timers.
-    //stop_control();
-    //start_sampling();
-    
+       
     char command=0;
     
     while(command != 'Q' && command != 'q')
@@ -317,7 +309,7 @@
             switch(command){
             
             case 'c':
-            case 'C':
+            case 'C': {
                 pc.printf("\n\r => You chose calibration.\r\n\n");
                 caliMenu();
                 
@@ -355,6 +347,7 @@
                 
                 }//end while
                 break;
+            }//end case c C
             case 't':
             case 'T':
                 pc.printf("=> You chose TRIG button control \r\n\n");
@@ -384,11 +377,11 @@
                 pc.printf("=> You chose EMG DLS control \r\n\n");
                 wait(1);
                 start_sampling();
-                
+                wait(1);
                 emg_control_time = 0;
-                wait(1);
                 emg_control=true;
                 control_method=2;
+                
                 start_control();
                 wait(1);   
                 controlButtons();
@@ -419,7 +412,7 @@
     
 
     
-    //When end of while loop reached (user chose quit program) - detach all timers and stop motors.
+    //When end of while loop reached (user chose quit program).
 
     pc.printf("\r\n------------------------------ \n\r");
     pc.printf("Program Offline \n\r");
@@ -432,6 +425,7 @@
 ---- FUNCTIONS -------------------------------------------------------------------------------------------------------
 --------------------------------------------------------------------------------------------------------------------*/
 
+//Use WASD keys to change reference position, x is a and d, y is w and s.
 void controlButtons()
 {
     controlMenu();
@@ -472,10 +466,10 @@
                       pc.printf("Sampling and Control detached \n\r"); wait(1);
                       pc.printf("Returning to Main Menu \r\n\n"); wait(1);
                       mainMenu();
-                      
-                      //running = false;                    
+                                        
                       break;
     }//end switch
+    
     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);
@@ -485,9 +479,9 @@
     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");
-    }
+    }//end if
     }
-    //end if
+    //end if pc readable
     }
     //end of while loop
 }
@@ -517,91 +511,91 @@
        
 }
 
-
+//Limit switch - if hit: set pulsecount of encoder to angle of lower mechanical limit
 void shoulder(){
     pwm_motor1=0;
-    //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) )
     theta1_cal = floor(theta1_lower * (4200/(2*PI)));
-    Encoder1.setPulses(theta1_cal);       //edited QEI library: added setPulses()
+    Encoder1.setPulses(theta1_cal);       //edited QEI library: added setPulses(int p)
     
 }
 
 void elbow(){
     pwm_motor2=0;
-    //Encoder2.reset();
     done2 = true;
     pc.printf("Elbow Limit hit - shutting down motor 2\r\n");
+    
     //Mechanical limit 43 degrees -> 43*(4200/360) = 350
     theta2_cal = floor(theta2_lower * (4200/(2*PI)));
-    Encoder2.setPulses(theta2_cal);       //edited QEI library: added setPulses()
+    Encoder2.setPulses(theta2_cal);       //edited QEI library: added setPulses(int p)
     
 }
+
+//Run motors slowly clockwise to mechanical limit. Attached to 100Hz ticker
+void calibrate(void){
+    if(done1==false){                   //if motor 1 limit has not been hit yet
+            pwm_motor1=0.1;             //move upper arm slowly cw
+            pc.printf("Motor 1 running %f \r\n");
+        }
+    if(done1==true && done2==false){    //if limit motor 1 has been hit
+            pwm_motor1=0;               //stop motor1
+            pwm_motor2=0.1;             //start moving forearm slowly cw
+            pc.printf("Motor 2 running %f \r\n");    
+        }   
+  //  if(done1==true && done2==true)      //if both limits are hit
+   //      pwm_motor2=0;                  //stop motor2
+    //     calibrating=false;             //stop calibrating
+}
     
 //Send arm to mechanical limits, and set encoder to 0. Then send arm to starting position.
 void calibrate_arm(void)
 {
     pc.printf("Calibrate_arm() entered\r\n");
-    red=0; blue=0;              //Debug light is purple during arm calibration
-    
+
+    calibrating = true;
     done1 = false;
     done2 = false;
-    bool calibrating = true;
     
     pc.printf("To start arm calibration, press any key =>");
-    pc.getc();
+    pc.getc(); 
     pc.printf("\r\n Calibrating... \r\n");
+    red=0; blue=0;              //Debug light is purple during arm calibration
+    
     dir_motor1=0;   //cw
     dir_motor2=1;   //cw
     
-       
+    control_timer.attach(&calibrate,CONTROL_RATE);
     
     while(calibrating){
-        shoulder_limit.rise(&shoulder);
-        elbow_limit.rise(&elbow);
-        //while(!done1){
-        //pwm_motor1=0.1;     //move upper arm slowly cw
-        
-        //}
-        //if(done1==true){
-           // pwm_motor2=0.1;     //move forearm slowly cw
-            
-        //}
+       shoulder_limit.fall(&shoulder);
+       elbow_limit.fall(&elbow);
+       if(done1 && done2){ 
+        pwm_motor2=0; 
+        control_timer.detach();      //Leave while loop when both limits are reached
+        red=1; blue=1;          //Turn debug light off when calibration complete
+        //set reference position to mechanical limits
+        calibrating=false;
         
-        if(done1 && done2){
-            calibrating=false;      //Leave while loop when both limits are reached
-            red=1; blue=1;          //Turn debug light off when calibration complete
-        }
-   
-    }//end while
-
-    //TO DO:
-    
-    //pc.printf("Elbow Limit hit - shutting down motor 2. Current pulsecount: %i \r\n",Encoder1.getPulses());
-    
-    //set reference position to mechanical limits
-    x = 0.2220;
-    y = 0.4088;
-
-    wait(1);
-    pc.printf("\n\r ------------------------ \n\r"); 
-    pc.printf("Arm Calibration Complete\r\n");
-    pc.printf(" ------------------------ \n\r"); 
-
+        x = l1 * cos(theta1_lower) + l2 * cos(theta1_lower + theta2_lower);
+        y = l1 * sin(theta1_lower) + l2 * sin(theta1_lower + theta2_lower);
+        //x = 0.2220;
+        //y = 0.4088;
+       }
+    }
+        pc.printf("Current pulsecount motor 1: %i, motor 2: %i \r\n",Encoder1.getPulses(),Encoder2.getPulses());
+        pc.printf("Current reference. X: %f, Y: %f \r\n",x,y);
+        wait(1);
+        pc.printf("\n\r-------------------------- \n\r"); 
+        pc.printf(" Arm Calibration Complete\r\n");
+        pc.printf("-------------------------- \n\r"); 
+  
 }
 
 //EMG Maximum Voluntary Contraction measurement
 void emg_mvc()
 {  
-    //double sampletime=0;
-    //sampletime=+SAMPLE_RATE;
-    //
-    // if(sampletime<5)
-    //int muscle=1;
-    //for(int index=0; index<2500;index++){   //measure 5 seconds@500hz = 2500 samples
-     
         if(muscle==1){
             
             if(biceps_power>bicepsMVC){