Diff: main.cpp

Revision:

20:0f6a88f29a71

Parent:

17:d1acb6888b82

Child:

21:a316452da8cd

--- a/main.cpp	Fri Oct 18 13:05:44 2019 +0000
+++ b/main.cpp	Mon Oct 21 12:09:28 2019 +0000
@@ -15,6 +15,8 @@
 PwmOut motor1_pwm(D6);
 AnalogIn    emg0( A0 );
 AnalogIn    emg1( A1 );
+QEI motor1 (D8, D9, NC, 32);
+QEI motor2 (D12, D13, NC, 32);
 
 Ticker ticktick;
 Timer state_time;
@@ -39,6 +41,36 @@
 enum states{Waiting,Position_calibration,EMG_calibration,Homing,Operating,Demo,Failure};
 states currentState=Waiting;
 
+static float v_refx; //reference speeds 
+static float v_refy;
+static float T = 0.002; //measuring period
+static float L = 0.35;  //distance between the motor axles, has to be checked
+volatile float theta1_old = 0.0; //feedback variables
+volatile float theta2_old = 0.0;
+
+int m1_count; 
+float m1_angle;
+int m2_count;
+float m2_angle;
+float pi = 3.14;
+
+float error_x;
+float error_y;
+
+//PID controller variables
+bool q = true;
+float error_prev;
+static float Kp = 8;
+static float Ki = 1.02;
+static float Kd = 0.1;
+float u;
+float u_p;
+float u_i;
+float u_d;
+float error_integral = 0.0;
+
+float u_1;
+float u_2;
 
 void read_emg()
 {
@@ -162,6 +194,80 @@
    motor2_pwm.write(ain1.read());
 }
 
+void error(void){
+    m1_count = motor1.getPulses();                                                                                  // Read out both motor counts and convert them to rads.
+    m1_angle = (float)m1_count / 4200.0f * (2.0f * pi);
+    m2_count = motor2.getPulses();
+    m2_angle = (float)m1_count / 4200.0f * (2.0f * pi);
+    
+    float dvd=L * (tan(m2_angle) * pow(tan(m1_angle),2) - tan(m1_angle) * pow(tan(m2_angle),2));
+
+    float a =  (  -pow(cos(m1_angle),2) * pow((tan(m1_angle)+tan(m2_angle)),2) * pow(tan(m1_angle),2)  )  / dvd;             // inverse matrix components of differential x and differential y
+    float b =  (   pow(cos(m1_angle),2) * pow((tan(m1_angle)+tan(m2_angle)),2) * tan(m1_angle)         )  / dvd; 
+    float c =  (   pow(cos(m2_angle),2) * pow((tan(m1_angle)+tan(m2_angle)),2) * pow(tan(m2_angle),2)  )  / dvd;
+    float d =  (   pow(cos(m2_angle),2) * pow((tan(m1_angle)+tan(m2_angle)),2) * pow(tan(m2_angle),2)  )  / dvd;
+    
+    float theta1_dot = a*v_refx + b*v_refy;
+    float theta2_dot = c*v_refx + d*v_refy;
+    
+    float theta1_ref = theta1_old+theta1_dot*T;
+    float theta2_ref = theta2_old+theta2_dot*T;
+    
+    error_x = m1_angle - theta1_ref;
+    error_y = m2_angle - theta2_ref;
+    
+    theta1_old = theta1_ref;
+    theta2_old = theta2_ref;
+    
+    }
+    
+float PID(float err){
+    //P action
+    u_p = Kp * fabs(err);
+    
+    //I action 
+    error_integral = error_integral + err * T;
+    u_i = Ki * error_integral;
+    
+/*    
+    //D action
+    if(q){
+        error_prev = err;
+        q=false;
+        }
+        
+    float error_derivative = (err - error_prev)/T;
+    float filtered_error_derivative = LowPassFilter.step(error_derivative);
+    u_d = Kd * filtered_error_derivative;
+    error_prev = err;
+    
+*/
+    
+    u = u_p + u_i;
+    
+    return u;
+    }
+    
+void setPWM_controller(void){
+    u_1 = PID(error_x);
+    u_2 = PID(error_y);
+    
+    if(u_1 < 0.0f){
+        dir1 = 1;
+    }else{
+        dir1 = 0;
+        }
+    motor1_pwm.write(fabs(u_1));
+    
+    if(u_2 < 0.0f){
+        dir2 = 1;
+    }else{
+        dir2 = 0;
+        }
+    motor2_pwm.write(fabs(u_1));
+    
+    }
+
 void sample()
 {
     switch(currentState)
@@ -263,4 +369,5 @@
     while (true) {
         wait(10);
     }
-}
+
+}
\ No newline at end of file