Diff: main.cpp

Revision:

3:be922ea2415f

Parent:

2:5cace74299e7

Child:

4:8f25ecb74221

--- a/main.cpp	Tue Oct 30 09:15:02 2018 +0000
+++ b/main.cpp	Wed Oct 31 12:15:34 2018 +0000
@@ -3,6 +3,8 @@
 #include "QEI.h"
 #include "MODSERIAL.h"
 #include "BiQuad.h"
+#include <iostream>
+#include <string>
 
 enum States {WaitState, MotorCalState, EMGCalState, HomingState, MovingState, GrippingState, ScrewingState, FailureState};
 States currentState = WaitState;
@@ -18,9 +20,10 @@
 DigitalOut led2(LED2); // Green led
 DigitalOut led3(LED3); // Blue led
 QEI encoder1(D14, D15, NC, 8400, QEI::X4_ENCODING);
-//QEI encoder2(A2, A3), NC, 4200);
+QEI encoder2(D12, D13, NC, 8400, QEI::X4_ENCODING);
 //QEI encoder3(A4, A5), NC, 4200);
 AnalogIn pot(A0); // Speed knob
+AnalogIn pot2(A1);
 
 bool stateChanged = true;
 
@@ -30,14 +33,52 @@
 const double sampleTime = 0.001;
 const float maxVelocity=8.4; // in rad/s
 const double PI = 3.141592653589793238463;
-                                    
-double u1, u2, u3, u4; // u1 is motor output of the long link, u2 is motor of the short link, u3 is motor of gripper, u4 is motor of screwer
+const double  L1 = 0.328;
+const double L2 = 0.218;
+ double T1[3][3] {
+    {0, -1, 0},
+    {1, 0, 0,},
+    {0, 0, 0,}
+};
+ double  T20[3][3] {
+    {0, -1, 0},
+    {1, 0, -L1,},
+    {0, 0, 0,}
+};
+ double  H200[3][3] {
+    {1, 0, L1+L2},
+    {0, 1, 0,},
+    {0, 0, 1,}
+};
+ double Pe2 [3][1] {
+    {0},
+    {0},
+    {1}
+};
+
+double u1;
+double u2; // u1 is motor output of the long link, u2 is motor of the short link, u3 is motor of gripper, u4 is motor of screwer
+double u3;
+double u4;
 FastPWM pwmpin1(D5); //motor pwm
 DigitalOut directionpin1(D4); // motor direction
-double robotEndPoint;
+FastPWM pwmpin2 (D6);
+DigitalOut directionpin2 (D7);
+double setPointX;
+double setPointY;
+double qRef1;
+double qRef2;
+double qMeas1;
+double qMeas2;
 
-double v;
-double Dpulses;
+double v; // Global variable for printf
+double Dpulses; // Global variable for printf
+
+double C[5][5];
+
+double Kp = 0.1;
+double Ki = 0;
+double Kd = 0;
 
 void switchToFailureState ()
 {
@@ -55,26 +96,26 @@
     static double lastPulses = 0;
     double currentPulses;
     static double velocity = 0;
-    
+
     static int i = 0;
     if (i == 10) { // Encoder is not accurate enough, so with 1000 Hz the velocity can only be 0, 1000, 2000 or 3000 pulses/s
-    switch (motor) { // Check which motor to measure
-        case 1:
-            currentPulses = encoder1.getPulses();
-            break;
-        case 2:
-            //currentPulses = encoder2.getPulses();
-            break;
-        case 3:
-            //currentPulses = encoder3.getPulses();
-            break;
-    }
+        switch (motor) { // Check which motor to measure
+            case 1:
+                currentPulses = encoder1.getPulses();
+                break;
+            case 2:
+                //currentPulses = encoder2.getPulses();
+                break;
+            case 3:
+                //currentPulses = encoder3.getPulses();
+                break;
+        }
 
-    double deltaPulses = currentPulses - lastPulses;
-    Dpulses = deltaPulses;
-    velocity = deltaPulses / (sampleTime * 10); // Velocity in pulses/s
-    lastPulses = currentPulses;
-    i = 0;
+        double deltaPulses = currentPulses - lastPulses;
+        Dpulses = deltaPulses;
+        velocity = deltaPulses / (sampleTime * 10); // Velocity in pulses/s
+        lastPulses = currentPulses;
+        i = 0;
     } else {
         i += 1;
     }
@@ -82,22 +123,212 @@
     return velocity;
 }
 
-void measureAll ()
+void measurePosition() // Measure actual angle position with the encoder
 {
+    double pulses1 = encoder1.getPulses();
+    double pulses2 = encoder2.getPulses();
+    qMeas1 = pulses1 * 2 * PI / 8400 + 840; // Calculate the angle relative to the starting point (8400 pulses per revolution) + offset
+    qMeas2 = pulses2 * 2 * PI / 8400 + 70;
 
 }
 
 void getMotorControlSignal ()   // Milestone 1 code, not relevant anymore
 {
-    double potSignal = pot.read(); // read pot and scale to motor control signal
-    //pc.printf("motor control signal = %f\n", potSignal);
+    double potSignal = pot.read() * 2 - 1; // read pot and scale to motor control signal
+    //pc.printf("motor control signal = %f\n", posampleTimeignal);
     u1 = potSignal;
+    u2 = potSignal;
+}
+
+template<std::size_t N, std::size_t M, std::size_t P>
+void mult(double A[N][M], double B[M][P])
+{
+
+    for( int n =0; n < 5; n++) {
+        for(int p =0; p < 5; p++) {
+            C[n][p] =0;
+        }
+    }
+    for (int n = 0; n < N; n++) {
+        for (int p = 0; p < P; p++) {
+            double num = 0;
+            for (int m = 0; m < M; m++) {
+                num += A[n][m] * B[m][p];
+
+            }
+
+            C[n][p] = num;
+
+        }
+    }
+
+}
+
+void inverseKinematics ()
+{
+    if (currentState == MovingState) {  // Only in the HomingState should the qRef1, qRef2 consistently change
+        double potx = 0.218;//pot1.read()*0.546;
+        double  poty = 0.328;//pot2.read()*0.4;
+
+        double  Pe_set[3][1] {            // defining setpoint location of end effector
+            {potx},             //setting xcoord to pot 1
+            {poty},             // setting ycoord to pot 2
+            {1}
+        };
+
+//Calculating new H matrix
+        double T1e[3][3] {
+            {cos(qRef1), -sin(qRef1), 0},
+            {sin(qRef1), cos(qRef1), 0},
+            {0, 0, 1}
+        };
+        double  T20e[3][3] {
+            {cos(qRef2), -sin(qRef2), L1-L1*cos(qRef2)},
+            {sin(qRef2), cos(qRef2), -L1*sin(qRef2)},
+            {0, 0, 1}
+        };
+
+
+        mult<3,3,3>(T1e,T20e);  // matrix multiplication
+        double H201[3][3] {
+            {C[0][0],C[0][1],C[0][2]},
+            {C[1][0],C[1][1],C[1][2]},
+            {C[2][0],C[2][1],C[2][2]}
+        };
+
+        mult<3,3,3>(H201,H200);   // matrix multiplication
+        double H20 [3][3] {
+            {C[0][0],C[0][1],C[0][2]},
+            {C[1][0],C[1][1],C[1][2]},
+            {C[2][0],C[2][1],C[2][2]}
+        };
+
+        mult<3,3,1>(H20,Pe2);   // matrix multiplication
+        double Pe0[3][1] {
+            {C[0][0]},
+            {C[1][0]},
+            {C[2][0]}
+        };
+
+        double pe0x = Pe0[0][0];                 // seperating coordinates of end effector location
+        double pe0y = Pe0[1][0];
+
+        // Determing the jacobian
+
+        double T_1[3][1] {
+            {1},
+            {T1[0][2]},
+            {T1[1][2]}
+        };
+
+        double T_2[3][1] {
+            {1},
+            {L1*sin(qRef1)},
+            {-L1*cos(qRef1)}
+        };
+
+        double J[3][2] {
+            {T_1[0][0], T_2[0][0]},
+            {T_1[1][0], T_2[1][0]},
+            {T_1[2][0], T_2[2][0]}
+        };
+
+//Determing 'Pulling" force to setpoint
+
+        double k= 1;     //virtual stifness of the force
+        double Fs[3][1] {                                    //force vector from end effector to setpoint
+            {k*Pe_set[0][0] - k*Pe0[0][0]},
+            {k*Pe_set[1][0] - k*Pe0[1][0]},
+            {k*Pe_set[2][0] - k*Pe0[2][0]}
+        };
+        double Fx = k*potx - k*pe0x;
+        double Fy = k*poty - k*pe0y;
+        double W0t[3][1] {
+            {pe0x*Fy - pe0y*Fx},
+            {Fx},
+            {Fy}
+        };
+
+        double Jt[2][3] {                                    // transposing jacobian matrix
+            {J[0][0], J[1][0], J[2][0]},
+            {T_2[0][0], T_2[1][0], T_2[2][0]}
+        };
+
+        mult<2,3,1>(Jt,W0t);
+        double tau_st1 = C[0][0];
+        double tau_st2 = C[1][0];
+
+//Calculating joint behaviour
+
+        double b =1;
+        //joint friction coefficent
+        //double sampleTime = 1/1000;               //Time step to reach the new angle
+        double w_s1 = tau_st1/b;          // angular velocity
+        double w_s2 = tau_st2/b;          // angular velocity
+        //checking angle boundaries
+        qRef1 = qRef1 +w_s1*sampleTime;         // calculating new angle of qRef1 in time step sampleTime
+        if (qRef1 > 2*PI/3) {
+            qRef1 = 2*PI/3;
+        } else if (qRef1 < PI/6) {
+            qRef1= PI/6;
+        }
+
+        qRef2 = qRef2 + w_s2*sampleTime;        // calculating new angle of qRef2 in time step sampleTime
+        if (qRef2 > -PI/4) {
+            qRef2 = -PI/4;
+        } else if (qRef2 < -PI/2) {
+            qRef2= -PI/2;
+        }
+    }
+}
+
+void PID_controller() // Put the error trough PID control to make output 'u'
+{
+    if (currentState >= HomingState && currentState < FailureState) { // Should only work when we move the robot to a defined position
+        double error1 = qRef1 - qMeas1;
+        double error2 = qRef2 - qMeas2;
+
+        static double errorIntegral1 = 0;
+        static double errorIntegral2 = 0;
+        static double errorPrev1 = error1;
+        static double errorPrev2 = error2;
+        static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
+        //Ki = pot2.read() * 0.5; //Only Kd is controlled by a pot, Kp and Ki are constant
+
+        // Proportional part:
+        Kp = pot2.read() * 1;
+        double u_k1 = Kp * error1;
+        double u_k2 = Kp * error2;
+
+        //Integral part:
+        errorIntegral1 = errorIntegral1 + error1 * sampleTime;
+        double u_i1 = Ki * errorIntegral1;
+        errorIntegral2 = errorIntegral2 + error2 * sampleTime;
+        double u_i2 = Ki * errorIntegral2;
+
+        // Derivative part
+        double errorDerivative1 = (error1 - errorPrev1)/sampleTime;
+        double filteredErrorDerivative1 = LowPassFilter.step(errorDerivative1);
+        double u_d1 = Kd * filteredErrorDerivative1;
+        errorPrev1 = error1;
+        double errorDerivative2 = (error2 - errorPrev2)/sampleTime;
+        double filteredErrorDerivative2 = LowPassFilter.step(errorDerivative2);
+        double u_d2 = Kd * filteredErrorDerivative2;
+        errorPrev2 = error2;
+
+        // Sum all parsampleTime
+        u1 = u_k1 + u_i1 + u_d1;
+        u2 = u_k2 + u_i2 + u_d2;
+    }
+    
 }
 
 void controlMotor ()   // Control direction and speed
 {
     directionpin1 = u1 > 0.0f; // Either true or false
     pwmpin1 = fabs(u1);
+    directionpin2 = u2 > 0.0f; // Either true or false
+    pwmpin2 = fabs(u2);
 }
 
 void stateMachine ()
@@ -129,8 +360,8 @@
                 // Entry action: all the things you do once in this state
                 led2 = 0;
                 // Set motorpwm to 'low' value
-                u1 = 0.6; //TODO: Check if direction is right
-                u2 = 0.6;
+                //u1 = 0.6; //TODO: Check if direction is right
+                //u2 = 0.6;
                 stateTimer.reset();
                 stateTimer.start();
                 stateChanged = false;
@@ -139,7 +370,7 @@
             // Add stuff you do every loop
             getMotorControlSignal();
 
-            if (stateTimer >= 3.0f && fabs(measureVelocity(1)) < 100 && screwingSwitch.read() == false) { //TODO: add && fabs(measureVelocity(2)) < 0.1f 
+            if (stateTimer >= 3.0f && fabs(measureVelocity(1)) < 100 && screwingSwitch.read() == false) { //TODO: add && fabs(measureVelocity(2)) < 0.1f
                 //TODO: Add reset of encoder2
                 led2 = 1;
                 encoder1.reset(); // Reset encoder for the 0 position
@@ -171,8 +402,10 @@
             if (stateChanged == true) {
                 // Entry action: all the things you do once in this state;
                 led1 = 0;
-                led2 = 0; // Emits yellow together
-                //TODO: Set intended position
+                led2 = 0; // EmisampleTime yellow together
+                //TODO: Set qRef1 and qRef2
+                qRef1 = 90 * PI / 180;
+                qRef2 = -90 * PI / 180;
                 stateChanged = false;
             }
 
@@ -190,7 +423,7 @@
                 // Entry action: all the things you do once in this state;
                 led1 = 0;
                 led2 = 0;
-                led3 = 0; // Emits white together
+                led3 = 0; // EmisampleTime white together
                 stateChanged = false;
             }
 
@@ -207,7 +440,7 @@
             if (stateChanged == true) {
                 // Entry action: all the things you do once in this state;
                 led2 = 0;
-                led3 = 0; // Emits light blue together
+                led3 = 0; // EmisampleTime light blue together
                 stateChanged = false;
             }
 
@@ -234,7 +467,7 @@
             if (stateChanged == true) {
                 // Entry action: all the things you do once in this state;
                 led1 = 0;
-                led3 = 0; // Emits pink together
+                led3 = 0; // EmisampleTime pink together
                 stateChanged = false;
             }
 
@@ -272,11 +505,18 @@
     }
 }
 
+void measureAll ()
+{
+    measurePosition();
+    inverseKinematics();
+}
+
 void mainLoop ()
 {
     // Add measure, motor controller and output function
     measureAll();
     stateMachine();
+    PID_controller();
     controlMotor();
 }