Diff: main.cpp

Revision:

37:4d7b7ced20ef

Parent:

36:4d4fc200171b

Child:

38:c8ac615d0c8f

diff -r 4d4fc200171b -r 4d7b7ced20ef main.cpp
--- a/main.cpp	Thu Oct 22 23:19:50 2015 +0000
+++ b/main.cpp	Thu Oct 22 23:26:37 2015 +0000
@@ -212,16 +212,17 @@
 
 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 dlscontrol();          //Damped Least Squares method
+void calibrate_emg();       //Instructions + measurement of Min and MVC of each muscle 
 void emg_mvc();             //Helper funcion for storing MVC value
+void emg_min();             //Helper function for storing Min value
 void calibrate_arm(void);   //Calibration of the arm with limit switches
 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);
+void start_dlscontrol(void);//Attaches DLS control function to a 100Hz ticker    
 void stop_control(void);    //Stops control function
-void emg_min();
+
 
 //Keeps the input between min and max value
 void keep_in_range(double * in, double min, double max);
@@ -252,8 +253,8 @@
     red=1; green=1; blue=1;     //Make sure debug LEDs are off  
     
     //Set PwmOut frequency to 10k Hz
-    //pwm_motor1.period(PWM_PERIOD);    
-    //pwm_motor2.period(PWM_PERIOD);
+    //pwm_motor1.period(0.0001);    
+    //pwm_motor2.period(0.0001);
     
     clearTerminal();            //Clear the putty window
     
@@ -262,10 +263,8 @@
     mainMenu();
    
     //set initial reference position
-    x = 0.5;
-    y = 0;
-    theta1=0.6980;
-    theta2=0.5200;
+    //x = 0.5;
+    //y = 0;
     
     //maybe some stop commands when a button is pressed: detach from timers.
     //stop_control();
@@ -859,6 +858,7 @@
     y_error = y - current_y;
     
     
+    if (control_method=1){
     //inverse kinematics (refpos to refangle)
     
     costheta2 = (pow(x,2) + pow(y,2) - pow(l1,2) - pow(l2,2)) / (2*l1*l2) ;
@@ -882,7 +882,22 @@
     //Angle error
     m1_error = dtheta1-theta1;
     m2_error = dtheta2-theta2;
+    }
     
+    if(control_method=2){
+    //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)*pow(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*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));
+    dls3= -(l2*pow(lambda,2)*sin(theta1 + theta2) - l1*pow(l2,2)*pow(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)*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));
+    dls4= (l2*pow(lambda,2)*cos(theta1 + theta2) - l1*pow(l2,2)*pow(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)*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));
+    
+    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