Diff: main.cpp

Revision:

43:8e2ea92fee01

Parent:

42:bcd496523c66

Child:

44:25eec278c623

--- a/main.cpp	Wed Oct 31 16:18:40 2018 +0000
+++ b/main.cpp	Thu Nov 01 11:31:37 2018 +0000
@@ -6,6 +6,8 @@
 #include "PID_controller.h"
 #include "kinematics.h"
 #include "processing_chain_emg.h"
+#include <vector>
+
 
 // emg inputs
 AnalogIn    emg0( A0 );
@@ -77,6 +79,16 @@
 double q1old;
 double q2old;
 
+vector<double> currentconfig;
+vector<double> newconfig;
+vector<double> ref;
+
+double currentx;
+double currenty;
+
+double L1 = 0.4388;
+double L2 = 0.4508;
+
 // Meaning of process_emg_0 and such
 // - process_emg_0 is right biceps
 // - process_emg_1 is right triceps
@@ -91,12 +103,69 @@
 double resolution = (2.0f*3.1415926535f/double(counts_per_rotation))*(1.0/131.0); // In radians
 
 // Functions
+
+vector<double> forwardkinematics_function(double& q1, double& q2) {
+    // input are joint angles, output are x and y position of end effector
+    
+    //previously +x01 and +y01
+    currentx = L1*cos(q1)-L2*cos(q2);
+    currenty = L1 * sin(q1) - L2 * sin(q2); 
+    
+    vector<double> xy;
+    xy.push_back(currentx);
+    xy.push_back(currenty);
+    return xy;
+   
+}
+ 
+vector<double> inversekinematics_function(const double& T, double& q1, double& q2, double& des_vx, double& des_vy, double ref1, double ref2) {
+    // x, y: positions of end effector | T: time to hold current velocity | qref1, qref2: reference thetas | q1, q2: current thetas | vx, vy: desired x, y velocities
+    
+    // pseudo inverse jacobian to get joint speeds
+    // input are desired vx and vy of end effector, output joint angle speeds
+ 
+    double q1_star_des; // desired joint velocity of q1_star
+    double q2_star_des; // same as above but then for q2_star
+    double C;
+    double qref1, qref2;
+ 
+    // The calculation below assumes that the end effector position is calculated before this function is executed
+    // In our case the determinant will not equal zero, hence no problems with singularies I think.
+    
+    C = L1*L2*sin(ref1)*cos(ref2) - L1*L2*sin(ref2)*cos(ref1);
+    q1_star_des = double(1/C)*(-L2*cos(ref2)*des_vy - L2*sin(ref2)*des_vx);
+    q2_star_des = double(1/C)*((L2*cos(ref2)-L1*cos(ref1))*des_vy + (L2*sin(ref2)-L1*sin(ref1))*des_vx);
+    
+    qref1 = T*q1_star_des;
+    qref2 = T*(q2_star_des+q1_star_des);
+    
+    double testq1 = ref1+qref1;
+    double testq2 = ref2+qref2;
+    newconfig = forwardkinematics_function(testq1, testq2);
+    
+    if (sqrt(pow(newconfig[0],2)+pow(newconfig[1],2)) > 0.73)
+    {
+        qref1 = ref1;
+        qref2 = ref2;
+    }
+    else
+    {
+        qref1 = ref1 + qref1;
+        qref2 = ref2 + qref2;
+    }
+
+    vector<double> thetas;
+    thetas.push_back(qref1);
+    thetas.push_back(qref2);
+    return thetas;
+}
+
 void measure_all() 
 {
 
     q1 = motor_1_encoder.getPulses()*2.0f*3.1415926535f/counts_per_rotation + off_set_q1; //do this here, and not in the encoder interrupt, to reduce computational load
     q2 = motor_2_encoder.getPulses()*2.0f*3.1415926535f/counts_per_rotation + off_set_q2;
-    forwardkinematics_function(q1,q2,x,y);  
+    currentconfig = forwardkinematics_function(q1,q2); //previously x,y also input 
     raw_emg_0 = emg0.read(); //sample analog voltages (all sampling theory applies, you might get aliasing etc.)
     raw_emg_1 = emg1.read();
     processing_chain_emg(raw_emg_0, raw_emg_1, raw_emg_2, raw_emg_3, process_emg_0, process_emg_1, process_emg_2, process_emg_3);  // processes the emg signals
@@ -115,6 +184,57 @@
     button_suppressed = false;    
 }
 
+
+
+void PID_controller(double error1, double error2, float &u1, float &u2, double T)
+{   
+    static double error_prev1 = error1; // initialization with this value only done once!
+    static double error_prev2 = error2; // initialization with this value only done once!
+    double u_pid2, u_pid1;
+    static double error_integral1, error_integral2 = 0;
+  
+    double u_k1 = Kp * error1;
+    double u_k2 = Kp * error2;
+    
+    static BiQuad LowPassFilter1(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
+    static BiQuad LowPassFilter2(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
+    
+    double error_derivative1 = (error1 - error_prev1)*T;
+    double filtered_error_derivative1 = LowPassFilter1.step(error_derivative1);
+    double u_d1 = Kd * filtered_error_derivative1;
+    
+    double error_derivative2 = (error2 - error_prev2)*T;
+    double filtered_error_derivative2 = LowPassFilter2.step(error_derivative2);
+    double u_d2 = Kd * filtered_error_derivative2;
+    
+    error_integral1 = error_integral1 +( error1 * T);
+    error_integral2 = error_integral2 +( error2 * T);
+    if (error_integral1 > 1){
+        error_integral1 = 1;
+    }
+    else if (error_integral1 < -1){
+        error_integral1 = -1;
+    }
+    if (error_integral2 > 1){
+        error_integral2 = 1;
+    }
+    else if (error_integral2 < -1){
+        error_integral2 = -1;
+    }
+    
+    double u_i1 = Ki*error_integral1;
+    double u_i2 = Ki*error_integral2;
+    
+    u_pid1 = u_k1 + u_d1 + u_i1;
+    u_pid2 = u_k2 + u_d2 + u_i2;
+    
+    u1 = -u_pid1;
+    u2 = -u_pid2;
+    
+    error_prev1 = error1;
+    error_prev2 = error2;       
+}
+
 void state_machine() {
     switch(current_state) { //States can be: failure, wait, calib_enc, calib_emg, operational, demo,
         case waiting:  //Nothing useful here, maybe a blinking LED for fun and communication to the user
@@ -157,12 +277,12 @@
                     q2old = q2;
                     break;
                 case calib_enc_q1:
-                    if (state_timer.read() > 1.0f){u1=0.55;}
+                    if (state_timer.read() > 0.5f){u1=0.55;}
                     if (q1 - q1old == 0.0 && state_timer.read() > 5.0f)
                     {
                         calib_enc_state = not_in_calib_enc;
                         current_emg_calibration_state = calib_right_bicep;
-                        current_state = failure;
+                        current_state = calib_emg;
                         state_changed = true;
                         off_set_q1 = calib_q1 - q1;
                         u1 = 0;
@@ -300,25 +420,25 @@
                             
             // here we have to determine the desired velocity based on the processed emg signals and calibration
             // x velocity
-            if (x_norm >= 0.8) { des_vx = 0.4; }
-            else if(x_norm >= 0.6) { des_vx = 0.3; }
-            else if(x_norm >= 0.4) { des_vx = 0.2; }
-            else if(x_norm >= 0.2) { des_vx = 0.1; } 
-            else if(x_norm <= -0.8) { des_vx = -0.4; }
-            else if(x_norm <= -0.6) { des_vx = -0.3; }
-            else if(x_norm <= -0.4) { des_vx = -0.2; }
-            else if(x_norm <= -0.2) { des_vx = -0.1; }
+            if (x_norm >= 0.8) { des_vx = 0.2; }
+            else if(x_norm >= 0.6) { des_vx = 0.15; }
+            else if(x_norm >= 0.4) { des_vx = 0.1; }
+            else if(x_norm >= 0.2) { des_vx = 0.05; } 
+            else if(x_norm <= -0.8) { des_vx = -0.2; }
+            else if(x_norm <= -0.6) { des_vx = -0.15; }
+            else if(x_norm <= -0.4) { des_vx = -0.1; }
+            else if(x_norm <= -0.2) { des_vx = -0.05; }
             else { des_vx = 0; }
             
             // y velocity
-            if (y_norm >= 0.8) { des_vy = 0.4; }
-            else if(y_norm >= 0.6) { des_vy = 0.3; }
-            else if(y_norm >= 0.4) { des_vy = 0.2; }
-            else if(y_norm >= 0.2) { des_vy = 0.1; } 
-            else if(y_norm <= -0.8) { des_vy = -0.4; }
-            else if(y_norm <= -0.6) { des_vy = -0.3; }
-            else if(y_norm <= -0.4) { des_vy = -0.2; }
-            else if(y_norm <= -0.2) { des_vy = -0.1; }
+            if (y_norm >= 0.8) { des_vy = 0.2; }
+            else if(y_norm >= 0.6) { des_vy = 0.15; }
+            else if(y_norm >= 0.4) { des_vy = 0.1; }
+            else if(y_norm >= 0.2) { des_vy = 0.05; } 
+            else if(y_norm <= -0.8) { des_vy = -0.2; }
+            else if(y_norm <= -0.6) { des_vy = -0.15; }
+            else if(y_norm <= -0.4) { des_vy = -0.1; }
+            else if(y_norm <= -0.2) { des_vy = -0.05; }
             else { des_vy = 0; }
             
             if (button.read() == true && button_suppressed == false ) {
@@ -338,6 +458,16 @@
                     state_changed = false;
                     state_timer.reset();
                     state_timer.start();
+                    des_vx = 0.05;
+            double old_vx;
+            double old_vy;        
+            if (state_timer > 2.0f){
+                old_vx = des_vx;
+                old_vy = des_vy;
+                des_vx = des_vy;
+                des_vy = -des_vx;
+                state_timer.reset();   
+            }
             //        des_vx = 0.1;           // first we move to the right
             //        des_vy = 0.0;
             }
@@ -384,7 +514,9 @@
 void motor_controller() 
 {
     if (current_state >= operational) { // we can (ab)use the fact that an enum is actually an integer, so math/logic rules still apply
-        inversekinematics_function(x,y,T,qref1,qref2,q1,q2,des_vx,des_vy); //many different states can modify your reference position, so just do the inverse kinematics once, here
+        ref = inversekinematics_function(T,qref1,qref2,q1,q2,des_vx,des_vy); //many different states can modify your reference position, so just do the inverse kinematics once, here
+        qref1 = ref[0];
+        qref2 = ref[1];
         e1 = qref1 - q1; //tracking error (q_ref - q_meas)
         e2 = qref2 - q2;
         PID_controller(e1,e2,u1,u2,T); //feedback controller or with possibly fancy controller additions...; pass by reference