Revision 32:76c4d7bb2022, committed 2015-10-22

Comitter:

Vigilance88

Date:

Thu Oct 22 10:57:50 2015 +0000

Parent:

31:7b8b8459bddc

Child:

33:daa6ec305441

Commit message:

started on DLS control code

Changed in this revision

--- a/main.cpp	Wed Oct 21 10:41:59 2015 +0000
+++ b/main.cpp	Thu Oct 22 10:57:50 2015 +0000
@@ -100,6 +100,10 @@
 double m2_error=0; double m2_e_int=0; double m2_e_prev=0;           //Error, integrated error, previous error
 const double m2_kp=0.001; const double m2_ki=0.0125; const double m2_kd=0.1;   //Proportional, integral and derivative gains.
 
+//Calibration, checks if elbow/shoulder are done
+bool done1 = false;
+bool done2 = false;
+
 //highpass filter 20 Hz
 const double high_b0 = 0.956543225556877;
 const double high_b1 = -1.91308645113754;
@@ -158,6 +162,9 @@
 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;
 
 /*--------------------------------------------------------------------------------------------------------------------
 ---- Filters ---------------------------------------------------------------------------------------------------------
@@ -198,6 +205,7 @@
 
 void sample_filter(void);   //Sampling and filtering
 void control();             //Control - reference -> error -> pid -> motor output
+void dlscontrol();
 void calibrate_emg();       //Instructions + measurement of MVC of each muscle 
 void emg_mvc();             //Helper funcion for storing MVC value
 void calibrate_arm(void);   //Calibration of the arm with limit switches
@@ -221,6 +229,9 @@
 void emgInstructions();
 void titleBox();
 
+//Limit switches - power off motors if switches hit (rising edge interrupt)
+void shoulder();
+void elbow();
 
 /*--------------------------------------------------------------------------------------------------------------------
 ---- MAIN LOOP -------------------------------------------------------------------------------------------------------
@@ -253,12 +264,12 @@
     //maybe some stop commands when a button is pressed: detach from timers.
     //stop_control();
     start_sampling();
-    wait(60);
+    
     //char c;
     
     
     
-    char command;
+    char command=0;
     
     while(command != 'Q' && command != 'q')
     {
@@ -291,9 +302,9 @@
                    wait(1);
                    emgInstructions();
                    calibrate_emg(); 
-                   pc.printf("\n\r ------------------------ \n\r"); 
+                   pc.printf("\n\r------------------------- \n\r"); 
                    pc.printf("\n\r EMG Calibration complete \n\r"); 
-                   pc.printf("\n\r ------------------------ \n\r"); 
+                   pc.printf("\n\r------------------------- \n\r"); 
                    caliMenu(); 
                   break;
                   
@@ -367,27 +378,23 @@
             {
             case '1' :  
                      x = x + 0.01; 
-                     //controlMenu();
-                     //running=false;
+                    
                      break;
           
             case '2' :
                      x-=0.01;
-                     //controlMenu();
-                     //running=false;
+
                      break;
                     
             case '3' :
                      y+=0.01;
-                     //controlMenu();
-                     //running=false;
+
                      break;
                      
           
             case '4' :
                      y-=0.01;
-                     //controlMenu();                    
-                     //running=false;
+
                      break;
                      
             case 'q' :
@@ -420,10 +427,10 @@
 //Sample and Filter  
 void sample_filter(void)
 {
-    double emg_biceps = emg1.read();    //Biceps
-    double emg_triceps = emg2.read();    //Triceps
-    double emg_flexor = emg3.read();    //Flexor
-    double emg_extens = emg4.read();    //Extensor
+    emg_biceps = emg1.read();    //Biceps
+    emg_triceps = emg2.read();    //Triceps
+    emg_flexor = emg3.read();    //Flexor
+    emg_extens = emg4.read();    //Extensor
     
     //Filter: high-pass -> notch -> rectify -> lowpass or moving average
     // Can we use same biquadFilter (eg. highpass) for other muscles or does each muscle need its own biquad?
@@ -432,12 +439,17 @@
     biceps_power = notch2.step(biceps_power); triceps_power = notch2_2.step(triceps_power); flexor_power = notch2_3.step(flexor_power); extens_power = notch2_4.step(extens_power);
     biceps_power = abs(biceps_power); triceps_power = abs(triceps_power); flexor_power = abs(flexor_power); extens_power = abs(extens_power);
     biceps_power = lowpass.step(biceps_power); triceps_power = lowpass2.step(triceps_power); flexor_power = lowpass3.step(flexor_power); extens_power = lowpass4.step(extens_power);
+    biceps_power = biceps_power+0.1; triceps_power = triceps_power+0.1; flexor_power = flexor_power+1; extens_power = extens_power+1;
     
-    scope.set(0,emg_biceps);
-    scope.set(1,biceps_power);
-    scope.set(2,biceps_power);
-    scope.set(3,biceps_power);
+    scope.set(0,biceps_power);
+    scope.set(1,triceps_power);
+    scope.set(2,flexor_power);
+    scope.set(3,extens_power);
     scope.send();
+    
+    // on - offset. If above a value it is on. 
+    
+    
     /* alternative for lowpass filter: moving average
     window=30;                      //30 samples
     int i=0;                        //buffer index
@@ -457,13 +469,30 @@
 }
 
 
+void shoulder(){
+    pwm_motor1.write(0);
+    Encoder1.reset();
+    done1 = true;
+    pc.printf("Shoulder Limit hit - shutting down motor 1\r\n");
+}
+
+void elbow(){
+    pwm_motor2.write(0);
+    Encoder2.reset();
+    done2 = true;
+    pc.printf("Elbow Limit hit - shutting down motor 2\r\n");
+}
+    
 //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
+    
+    done1 = false;
+    done2 = false;
     bool calibrating = true;
-    bool done1 = false;
-    bool done2 = false;
+    
     pc.printf("To start arm calibration, press any key =>");
     pc.getc();
     pc.printf("\r\n Calibrating... \r\n");
@@ -473,34 +502,21 @@
        
     
     while(calibrating){
-        red=0; blue=0;              //Debug light is purple during arm calibration
-        
+        shoulder_limit.rise(&shoulder);
+        elbow_limit.rise(&elbow);
         if(done1==true){
             pwm_motor2.write(0.2);     //move forearm slowly cw
         }
         
-        //when limit switches are hit, stop motor and reset encoder
-        if(shoulder_limit.read() < 0.5){   //polling 
-            pwm_motor1.write(0);
-            Encoder1.reset();
-            done1 = true;
-            pc.printf("Shoulder Limit hit - shutting down motor 1\r\n");
-        }
-        if(elbow_limit.read() < 0.5){     //polling
-            pwm_motor2.write(0);
-            Encoder2.reset();
-            done2 = true;
-            pc.printf("Elbow Limit hit - shutting down motor 2. \r\n");
-        }    
         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:
-    //mechanical angle limits -> pulses. If 40 degrees -> counts = floor( 40 / (2*pi/4200) )
+    //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());
@@ -664,6 +680,8 @@
     biceps = (biceps - bicepsmin) / (bicepsmvc - bicepsmin)
     biceps = (biceps - bicepsmin) / (bicepsmvc - bicepsmin)
     
+    //threshold detection! buffer?
+    
     //analyze emg (= velocity)
     if (biceps>triceps && biceps > 0.1)
         xdir = 0;
@@ -745,11 +763,17 @@
     
     dtheta2 = atan2(sintheta2,costheta2);
     
+    double k1 = l1 + l2*costheta2;
+    double k2 = l2*sintheta2;
+    
+    dtheta1 = atan2(y, x) - atan2(k2, k1);
+    
+    /* alternative:
     costheta1 = ( x * (l1 + l2 * costheta2) + y * l2 * sintheta2 ) / ( pow(x,2) + pow(y,2) );
     sintheta1 = sqrt( abs( 1 - pow(costheta1,2) ) );
     
     dtheta1 = atan2(sintheta1,costheta1);
-    
+    */
     
     //Angle error
     m1_error = dtheta1-theta1;
@@ -793,6 +817,142 @@
     
 }
 
+void dlscontrol()
+{
+   /* 
+   
+    //normalize emg to value between 0-1
+    biceps = (biceps - bicepsmin) / (bicepsmvc - bicepsmin)
+    biceps = (biceps - bicepsmin) / (bicepsmvc - bicepsmin)
+    biceps = (biceps - bicepsmin) / (bicepsmvc - bicepsmin)
+    biceps = (biceps - bicepsmin) / (bicepsmvc - bicepsmin)
+    
+    //analyze emg (= velocity)
+    if (biceps>triceps && biceps > 0.1)
+        xdir = 0;
+        xpower = biceps;
+    else if (triceps>biceps && triceps>0.1)
+        xdir = 1;
+        xpower = triceps;
+    else 
+        xpower=0;
+        
+    if (flexor>extensor && flexor > 0.1){
+        ydir = 0;
+        ypower = flexor;
+        }
+    else if (extensor>flexor && extensor > 0.1){
+        ydir = 1;
+        ypower = extensor;
+        }
+    else
+        ypower = 0;
+            
+    //We have power: the longer a signal is active, the further you go. So integrate to determine reference position
+    dx = xpower * CONTROL_RATE;
+    dy = ypower * CONTROL_RATE; 
+    
+    //But: direction! Sum dx and dy but make sure xdir and ydir are considered.
+    if (xdir>0)
+        x += dx;
+    else 
+        x += -dx;
+        
+    if (ydir>0)
+        y += dy;
+    else
+        y += -dy;
+        
+   //now we have x and y -> reference position.     
+   
+   //Set limits to the error! 
+   //lower limit:   Negative error not allowed to go further.
+   if (theta1 < limitangle)
+        if (error1 > 0)
+            error1 = error1;
+        else 
+            error1 = 0; 
+   if (theta2 < limitangle)
+    same as above
+   
+   //upper limit: Positive error not allowed to go further
+   if (theta1 > limitangle)
+        if (error1 < 0 )
+            error1 = error1;
+        else
+            error1 = 0;
+   if (theta2 > limitangle)
+    same as above
+    
+             
+    */
+        
+    //Current position - Encoder counts -> current angle -> Forward Kinematics 
+    rpc=(2*PI)/ENCODER1_CPR;               //radians per count (resolution) - 2pi divided by 4200
+    theta1 = Encoder1.getPulses() * rpc;   //multiply resolution with number of counts
+    theta2 = Encoder2.getPulses() * rpc;
+    current_x = l1 * cos(theta1) + l2 * cos(theta1 + theta2);
+    current_y = l1 * sin(theta1) + l2 * sin(theta1 + theta2);
+    
+        
+    //calculate error (refpos-currentpos) currentpos = forward kinematics
+    x_error = x - current_x;
+    y_error = y - current_y;
+    
+    
+    //inverse kinematics (error in position to error in angles)
+    dls1= -(l2*pow(lambda,2)*sin(theta1 + theta2) + l1*pow(lambda,2)*sin(theta1) + l1*pow(l2,2)*pow(cos(theta1 + theta2),2)*sin(theta1) - l1*pow(l2,2)*cos(theta1 + theta2)*sin(theta1 + theta2)*cos(theta1))/(pow(lambda,4) + 2*pow(l2,2)*pow(lambda,2)*pow(cos(theta1 + theta2),2) + 2*pow(l2,2)*pow(lambda,2)*pow(sin(theta1 + theta2),2) + pow(l1,2)*pow(lambda,2)*pow(cos(theta1),2) + pow(l1,2)*pow(lambda,2)*pow(sin(theta1),2) + pow(l1,2)*pow(l2,2)*pow(cos(theta1 + theta2),2)*pow(sin(theta1),2) + pow(l1,2)*pow(l2,2)*pow(sin(theta1 + theta2),2)*pow(cos(theta1),2) + 2*l1*l2*pow(lambda,2)*cos(theta1 + theta2)*cos(theta1) + 2*l1*l2*pow(lambda,2)*sin(theta1 + theta2)*sin(theta1) - 2*pow(l1,2)*pow(l2,2)*cos(theta1 + theta2)*sin(theta1 + theta2)*cos(theta1)*sin(theta1));
+    /*
+    dls2= (l2*pow(lambda,2)*cos(theta1 + theta2) + l1*pow(lambda,2)*cos(theta1) + l1*pow(l2,2)*sin(theta1 + theta2)^2*cos(theta1) - l1*pow(l2,2)*cos(theta1 + theta2)*sin(theta1 + theta2)*sin(theta1))/(pow(lambda,4) + 2*pow(l2,2)*pow(lambda,2)*pow(cos(theta1 + theta2),2) + 2*l2^2*pow(lambda,2)*pow(sin(theta1 + theta2),2) + pow(l1,2)*pow(lambda,2)*cos(theta1)^2 + pow(l1,2)*pow(lambda,2)*pow(sin(theta1),2) + pow(l1,2)*pow(l2,2)*pow(cos(theta1 + theta2),2)*sin(theta1)^2 + l1^2*l2^2*sin(theta1 + theta2)^2*cos(theta1)^2 + 2*l1*l2*pow(lambda,2)*cos(theta1 + theta2)*cos(theta1) + 2*l1*l2*pow(lambda,2)*sin(theta1 + theta2)*sin(theta1) - 2*l1^2*l2^2*cos(theta1 + theta2)*sin(theta1 + theta2)*cos(theta1)*sin(theta1));
+    dls3= -(l2*pow(lambda,2)*sin(theta1 + theta2) - l1*pow(l2,2)*cos(theta1 + theta2)^2*sin(theta1) + pow(l1,2)*l2*sin(theta1 + theta2)*pow(cos(theta1),2) - pow(l1,2)*l2*cos(theta1 + theta2)*cos(theta1)*sin(theta1) + l1*pow(l2,2)*cos(theta1 + theta2)*sin(theta1 + theta2)*cos(theta1))/(pow(lambda,4) + 2*pow(l2,2)*pow(lambda,2)*pow(cos(theta1 + theta2),2) + 2*pow(l2,2)*pow(lambda,2)*pow(sin(theta1 + theta2),2) + pow(l1,2)*pow(lambda,2)*cos(theta1)^2 + l1^2*lambda^2*sin(theta1)^2 + l1^2*l2^2*cos(theta1 + theta2)^2*sin(theta1)^2 + l1^2*l2^2*sin(theta1 + theta2)^2*cos(theta1)^2 + 2*l1*l2*pow(lambda,2)*cos(theta1 + theta2)*cos(theta1) + 2*l1*l2*pow(lambda,2)*sin(theta1 + theta2)*sin(theta1) - 2*l1^2*l2^2*cos(theta1 + theta2)*sin(theta1 + theta2)*cos(theta1)*sin(theta1));
+    dls4= (l2*pow(lambda,2)*cos(theta1 + theta2) - l1*pow(l2,2)*sin(theta1 + theta2)^2*cos(theta1) + pow(l1,2)*l2*cos(theta1 + theta2)*pow(sin(theta1),2) - pow(l1,2)*l2*sin(theta1 + theta2)*cos(theta1)*sin(theta1) + l1*pow(l2,2)*cos(theta1 + theta2)*sin(theta1 + theta2)*sin(theta1))/(pow(lambda,4) + 2*pow(l2,2)*pow(lambda,2)*pow(cos(theta1 + theta2),2) + 2*pow(l2,2)*pow(lambda,2)*pow(sin(theta1 + theta2),2) + pow(l1,2)*pow(lambda,2)*cos(theta1)^2 + l1^2*lambda^2*sin(theta1)^2 + l1^2*l2^2*cos(theta1 + theta2)^2*sin(theta1)^2 + l1^2*l2^2*sin(theta1 + theta2)^2*cos(theta1)^2 + 2*l1*l2*pow(lambda,2)*cos(theta1 + theta2)*cos(theta1) + 2*l1*l2*pow(lambda,2)*sin(theta1 + theta2)*sin(theta1) - 2*l1^2*l2^2*cos(theta1 + theta2)*sin(theta1 + theta2)*cos(theta1)*sin(theta1));
+ */
+    
+    q1_error = dls1 * x_error + dls2 * y_error;
+    q2_error = dls3 * x_error + dls4 * y_error;
+    
+    //Angle error
+    m1_error = q1_error;
+    m2_error = q2_error;
+    
+    
+    //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
+    
+    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);
+    
+    //send PWM and DIR to motor 1
+    dir_motor1 = u1>0 ? 1 : 0;          //conditional statement dir_motor1 = [condition] ? [if met 1] : [else 0]], same as if else below. 
+    pwm_motor1.write(abs(u1));
+    
+    //send PWM and DIR to motor 2
+    dir_motor2 = u2>0 ? 0 : 1;          //conditional statement, same as if else below
+    pwm_motor2.write(abs(u2));
+    
+    /*if(u1 > 0)
+    {
+        dir_motor1 = 0;
+    else{
+        dir_motor1 = 1;
+        }
+    }    
+    pwm_motor1.write(abs(u1));
+    
+  
+    if(u2 > 0)
+    {
+        dir_motor1 = 1;
+    else{
+        dir_motor1 = 0;
+        }
+    }    
+    pwm_motor1.write(abs(u2));*/
+    
+}
+
 void mainMenu()
 {
        //Title Box