Revision 14:c4dbce7de430, committed 2018-10-31

Comitter:

Spekdon

Date:

Wed Oct 31 20:19:01 2018 +0000

Parent:

13:3482d315877c

Commit message:

fixed everything (except for a few things)

Changed in this revision

--- a/main.cpp	Mon Oct 29 12:55:38 2018 +0000
+++ b/main.cpp	Wed Oct 31 20:19:01 2018 +0000
@@ -2,155 +2,252 @@
 #include "MODSERIAL.h"
 #include "QEI.h"
 #include "BiQuad.h"
+#include "math.h"
+#include <vector>
 
 // CHECK
 PwmOut pwmpin(D6);
 PwmOut pwmpin2(D5);
 DigitalOut directionpin2(D4);
 DigitalOut directionpin(D7);
-//MODSERIAL pc(USBTX, USBRX);
-Serial pc(USBTX, USBRX);
+MODSERIAL pc(USBTX, USBRX);
+//Serial pc(USBTX, USBRX);
 
-QEI wheel1 (D13, D12, NC, 32);
-QEI wheel2 (D11, D10, NC, 32);
+QEI wheel2 (D13, D12, NC, 32, QEI::X2_ENCODING);
+QEI wheel1 (D11, D10, NC, 32, QEI::X2_ENCODING);
 float u1,u2 = 0;
 
 // for trajectory control:
 double T = 1; // time to get to destination
 double currentx, currenty;
 double currentq1, currentq2;
-// end    
+// end
+int pulses1, pulses2 = 0;
+float angle1, angle2;
+int realangle1, realangle2;
+double error1;
+double error2;
+double actualoutput1, actualoutput2 = 0;
+bool reached;
 
 
+Ticker adjust_ref;
+Ticker pid;
+
 float angle_resolution = (360.0/32.0)*(1.0/131.0);  //degrees/counts 
-double Kp = 2;
-double Ki = 1.02;
-double Kd = 10;
+double Kp = 0.1;
+double Ki = 0.4;
+double Kd = 4;
 extern double samplingfreq = 1000;
  
-double L1 = 0.43;
-double L2 = 0.43;
+double L1 = 0.4388;
+double L2 = 0.4508;
 double x01 = 0.0;
 double y01 = 0.0;
+double ref1, ref2;
+double t = 0.0;
+vector<double> inv_kin_ref;
+vector<double> newconfig;
+vector<double> currentxy;
 
-void forwardkinematics_function(double& q1, double& q2) {
+double timetogetthere = 0.01;
+double vx_des = 0.05;
+double vy_des = -0.05;
+
+float maxpwm = 1;
+
+void PID_controller(double error1, double error2, float &u1, float &u2, double actualoutput1, double actualoutput2)
+{   
+    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)*1/samplingfreq;
+    double filtered_error_derivative1 = LowPassFilter1.step(error_derivative1);
+    double u_d1 = Kd * filtered_error_derivative1;
+    
+    double error_derivative2 = (error2 - error_prev2)*1/samplingfreq;
+    double filtered_error_derivative2 = LowPassFilter2.step(error_derivative2);
+    double u_d2 = Kd * filtered_error_derivative2;
+    
+    error_integral1 = error_integral1 +( error1 * 1/samplingfreq);
+    error_integral2 = error_integral2 +( error2 * 1/samplingfreq);
+    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; 
+     
+   
+}
+
+vector<double> forwardkinematics_function(double& q1, double& q2) {
     // input are joint angles, output are x and y position of end effector
+    q1 = (q1 + 90)*(3.14/180);
+    q2 = (q2 + 180)*(3.14/180);
     
     currentx = x01 + L1*cos(q1)-L2*cos(q2);
-    currenty = y01 + L1 * sin(q1) - L2 * sin(q2);    
+    currenty = y01 + L1 * sin(q1) - L2 * sin(q2); 
+    
+    vector<double> xy;
+    xy.push_back(currentx);
+    xy.push_back(currenty);
+    return xy;
+   
 }
  
-vector<double> inversekinematics_function(double& x, double& y, const double& T, double& q1, double& q2, double& des_vx, double& des_vy) {
-    // x, y: positions of end effector | T: time to get to position | qref1, qref2: reference thetas | q1, q2: current thetas | vx, vy: desired x, y velocities
+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.
-    q1_star_des = 1/(L1*(-x*sin(q1)-(-y+y01)*cos(q1)))*(-1*(-x+L1*cos(q1))*des_vx-x*des_vy);
-    q2_star_des = 1/(L1*(-x*sin(q1)-(-y+y01)*cos(q1)))*(-1*(-y+y01+L1*sin(q1))*des_vx+1*(-y+y01)*des_vy); // CHECK THIS ONE!!!
+    //
+    //
+    
+    q1 = (ref1 + 90)*(3.14/180);
+    q2 = (ref2 + 180)*(3.14/180);
+    
+    C = L1*L2*sin(q1)*cos(q2) - L1*L2*sin(q2)*cos(q1);
+    q1_star_des = double(1/C)*(-L2*cos(q2)*des_vy - L2*sin(q2)*des_vx);
+    q2_star_des = double(1/C)*((L2*cos(q2)-L1*cos(q1))*des_vy + (L2*sin(q2)-L1*sin(q1))*des_vx);
+    
+    qref1 = T*q1_star_des;
+    qref2 = T*(q2_star_des+q1_star_des);
+    
+    qref1 = qref1*(180/3.14); 
+    qref2 = qref2*(180/3.14);
     
-    qref1 = q1+T*q1_star_des; // Yet to adapt all these equations
-    qref2 = q2+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 control(){
+        // Calculate angle of motor 1
+        pulses1 = wheel1.getPulses();
+        angle1 = -pulses1*angle_resolution;
+        realangle1 = int(angle1) % 360;
+        
+        // Calculate angle of motor 2
+        pulses2 = wheel2.getPulses();
+        angle2 = pulses2*angle_resolution;
+        realangle2 = int(angle2) % 360;
+        
+        // Q1 and Q2 current
+        currentq1 = angle1;
+        currentq2 = angle2;
+        
+        // modify the current x and y pos
+        currentxy = forwardkinematics_function(currentq1, currentq2);
+        currentx = currentxy[0];
+        currenty = currentxy[1];
+        
+        // calculate the reference position using inverse kinematics:
+        inv_kin_ref = inversekinematics_function(timetogetthere, currentq1, currentq2, vx_des, vy_des, ref1, ref2);
+        ref1 = inv_kin_ref[0];
+        ref2 = inv_kin_ref[1];
+        
+        error1 = ref1 - realangle1;
+        error2 = ref2 - realangle2;
+              
+        // control stuff:
+        PID_controller(error1, error2, u1, u2, actualoutput1, actualoutput2);
+        
+        
+        // Check if output is larger than maximum pwm 
+        if (abs(u1) > maxpwm){
+            if (u1 > 0){
+                u1 = maxpwm;
+            }
+            else{
+                u1 = -maxpwm;
+            }  
+        }   
+        if (abs(u2) > maxpwm){
+            if (u2 > 0){
+                u2 = maxpwm;
+            }
+            else{
+                u2 = -maxpwm;
+            }  
+        }
+                   
+        pwmpin = fabs(u1);
+        pwmpin2 = fabs(u2);
 
-void PID_controller(double error1, double error2, float &u1, float &u2)
-{   
-    double u_k = Kp * error1;
-    double u_k2 = Kp * error2;
-    static double error_integral = 0;
-    static double error_prev = error1; // initialization with this value only done once!
-    static double error_prev2 = error2; // initialization with this value only done once!
-
-    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
+        // 1/true is negative, 0/false is positive 
+        directionpin2 = u2 < 0;
+        directionpin = u1 < 0;
+}
+void measure()
+{
     
-    error_integral = error_integral + error1 * 1/samplingfreq;
-    double u_i = Ki * error_integral;
-    double error_derivative = (error1 - error_prev)*samplingfreq;
-    double filtered_error_derivative = LowPassFilter.step(error_derivative);
-    double u_d = Kd * filtered_error_derivative;
-    error_prev = error1;
-    error_prev2 = error2; 
-    u1 =  float(u_k/360);//+u_i+u_d;
-    u2 = float(u_k2/360);
-    
-   
-   
 }
 
 int main()
 {   
+    pc.baud(115200);
     pwmpin.period_us(60);
-    int pulses1, pulses2 = 0;
-    float angle1, angle2;
-    int realangle1, realangle2;
-    double ref1, error1;
-    double ref2, error2;
-    bool reached;
-    
-    double vx, vy;
+    pid.attach(&control,0.01);
+    ref1 = 0;
+    ref2 = 0;
     
     while (true) {
-        pulses1 = wheel1.getPulses();
-        angle1 = pulses1*angle_resolution;
-        realangle1 = abs(int(angle1)) % 360;
-        
-        pulses2 = wheel2.getPulses();
-        angle2 = pulses2*angle_resolution;
-        realangle2 = abs(int(angle2)) % 360;
-        
-        currentq1 = angle1;
-        currentq2 = angle2;
-        
-        //forwardkinematics_function(currentq1, currentq2);
-        T = 2; // 2 seconds seems slow enough :D
-        vx = 0.1;
-        vy = 0;
-    
-        vector<double> refangles = inversekinematics_function(currentx, currenty, T, currentq1, currentq2, vx, vy);
-        
-        ref1 = 0;
-        ref2 = 0;
-        
-        error1 = ref1 - realangle1;
-        error2 = ref2 - realangle2;
-        
-        
-        //PID_controller(error1, error2, u1, u2); 
-        if(u1 > 1){
-            u1 = 1;
-        }
-        if(u1 < -1){
-            u1 = -1;
-            }
-        if(u2 > 1){
-            u2 = 1;
-        }
-        if(u2 < -1){
-            u2 = -1;
-            } 
-
-            
-        pwmpin = fabs(u1);
-        pwmpin2 = fabs(u2);
-         
-        // 1/true is negative, 0/false is positive 
-        directionpin2 = u2 < 0;
-        directionpin = u1 < 0;
-        pc.printf("angle %f, error: %f, pwm: %f \r \n",realangle1, error1, u1);
-        
-         
-        
+    pc.printf("error1: %f, error2: %f, pwm1: %f, pwm2: %f \r \n", error1, error2, u1, u2);  
+    vector<double> currentthing;    
+    currentthing = forwardkinematics_function(ref1,ref2);
+    double cx = currentthing[0];
+    double cy = currentthing[1];
+    pc.printf("x: %f, y: %f \r\n",cx,cy);
     }
 }
\ No newline at end of file