Sem Bertelink
mag niet van hendrik D:
Dependencies: mbed MatrixMath QEI HIDScope Matrix biquadFilter MODSERIAL FastPWM
Revision 26:292492b885f7, committed 2019-10-25
- Comitter:
- sembert
- Date:
- Fri Oct 25 12:36:42 2019 +0000
- Parent:
- 25:832b26afbe0b
- Commit message:
- fucking kinematica :D
Changed in this revision
--- /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
--- /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
--- /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