mag niet van hendrik D:

Dependencies:   mbed MatrixMath QEI HIDScope Matrix biquadFilter MODSERIAL FastPWM

Files at this revision

API Documentation at this revision

Comitter:
sembert
Date:
Fri Oct 25 12:36:42 2019 +0000
Parent:
25:832b26afbe0b
Commit message:
fucking kinematica :D

Changed in this revision

Matrix.lib Show annotated file Show diff for this revision Revisions of this file
MatrixMath.lib Show annotated file Show diff for this revision Revisions of this file
kinematics.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r 832b26afbe0b -r 292492b885f7 Matrix.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Matrix.lib	Fri Oct 25 12:36:42 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/Yo_Robot/code/Matrix/#a4014ab0a8cf
diff -r 832b26afbe0b -r 292492b885f7 MatrixMath.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MatrixMath.lib	Fri Oct 25 12:36:42 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/Yo_Robot/code/MatrixMath/#93948a9bbde2
diff -r 832b26afbe0b -r 292492b885f7 kinematics.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/kinematics.cpp	Fri Oct 25 12:36:42 2019 +0000
@@ -0,0 +1,91 @@
+#include "QEI.h"
+#include "mbed.h"
+#include "Matrix.h"
+#include "BiQuad.h"
+#include "FastPWM.h"
+#include "HIDScope.h"
+#include "MODSERIAL.h"
+#include "MatrixMath.h"
+
+volatile double theta_1;
+//volatile float  theta_error_1;
+volatile float  theta_reference_1;
+volatile double theta_2;
+//volatile float  theta_error_2;
+volatile float  theta_reference_2;
+const float l = 0.535;
+const float EMGx_velocity=0.02;
+const float EMGy_velocity=0;
+float Ts    = 0.001;
+float Kp;
+float Ki;
+float Kd;
+QEI encoder_1(D10,D11,NC,8400,QEI::X4_ENCODING);
+QEI encoder_2(D12,D13,NC,8400,QEI::X4_ENCODING);
+float theta;
+float thetav_1;
+float thetav_2;
+
+//float ReadEncoder()
+//{
+//    theta_1 = ((360.0f/64.0f)*(float)encoder_1.getPulses())/131.25f; // 360/64 voor de 64 CPR Encoder, 131.25 omdat de gear ratio 131.25:1 is. Hiermee is 1 omwenteling 360.
+//    theta_2 = ((360.0f/64.0f)*(float)encoder_2.getPulses())/131.25f;
+//    float theta[2][1] = {{theta_1},{theta_2}};
+//    return theta
+//}
+
+float Controller(float theta_error, bool motor)
+{
+    if (motor == false) {
+        float K     = 1;
+        float ti    = 0.1;
+        float td    = 10;
+        Kp    = K*(1+td/ti);
+        Ki    = K/ti;
+        Kd    = K*td;
+    } else {
+        float K     = 1;
+        float ti    = 0.1;
+        float td    = 10;
+        Kp    = K*(1+td/ti);
+        Ki    = K/ti;
+        Kd    = K*td;
+    }
+    static float error_integral = 0;
+    static float error_prev = 0;
+    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
+
+    // Proportional part:
+    float torque_p = Kp * theta_error;
+
+    // Integral part:
+    error_integral = error_integral + theta_error * Ts;
+    float torque_i = Ki * error_integral;
+
+    // Derivative part:
+    float error_derivative = (theta_error - error_prev)/Ts;
+    float filtered_error_derivative = LowPassFilter.step(error_derivative);
+    float torque_d = Kd * filtered_error_derivative;
+    error_prev = theta_error;
+
+    // Sum all parts and return it
+    float torque = torque_p + torque_i + torque_d;
+    return torque;
+}
+
+void AngleVelocity(float theta_1,float theta_2)
+{
+    float DET_jacobian= 1.0f/((-l*sin(theta_1)-l*sin(theta_1+theta_2))*(l*cos(theta_1+theta_2))-(-l*sin(theta_1+theta_2))*(l*cos(theta_1)+l*cos(theta_1+theta_2)));
+    float INV_jacobian[2][2]={{DET_jacobian*l*cos(theta_1+theta_2) , DET_jacobian*l*sin(theta_1+theta_2)} , {DET_jacobian*-l*cos(theta_1)-l*cos(theta_1+theta_2) , DET_jacobian*-l*sin(theta_1)-l*sin(theta_1+theta_2)}};   
+    thetav_1=DET_jacobian*l*cos(theta_1+theta_2)*EMGx_velocity + DET_jacobian*l*sin(theta_1+theta_2)*EMGy_velocity;
+    thetav_2= DET_jacobian*-l*cos(theta_1)-l*cos(theta_1+theta_2)*EMGx_velocity + DET_jacobian*-l*sin(theta_1)-l*sin(theta_1+theta_2)*EMGy_velocity;
+}
+
+int main(void)
+{
+    while(1)
+    {
+    AngleVelocity(0.2f,0.0f); 
+    pc.printf('%f \n %f /n/r',thetav_1, thetav_2);
+        }  
+}
\ No newline at end of file