Sem Bertelink / Mbed 2 deprecated MOD09_Biorobotics_Groep_14

mag niet van hendrik D:

Dependencies:   mbed MatrixMath QEI HIDScope Matrix biquadFilter MODSERIAL FastPWM

kinematics.cpp@26:292492b885f7, 2019-10-25 (annotated)

Committer:
sembert
Date:
Fri Oct 25 12:36:42 2019 +0000
Revision:
26:292492b885f7
fucking kinematica :D

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sembert 26:292492b885f7 1 #include "QEI.h"
sembert 26:292492b885f7 2 #include "mbed.h"
sembert 26:292492b885f7 3 #include "Matrix.h"
sembert 26:292492b885f7 4 #include "BiQuad.h"
sembert 26:292492b885f7 5 #include "FastPWM.h"
sembert 26:292492b885f7 6 #include "HIDScope.h"
sembert 26:292492b885f7 7 #include "MODSERIAL.h"
sembert 26:292492b885f7 8 #include "MatrixMath.h"
sembert 26:292492b885f7 9
sembert 26:292492b885f7 10 volatile double theta_1;
sembert 26:292492b885f7 11 //volatile float theta_error_1;
sembert 26:292492b885f7 12 volatile float theta_reference_1;
sembert 26:292492b885f7 13 volatile double theta_2;
sembert 26:292492b885f7 14 //volatile float theta_error_2;
sembert 26:292492b885f7 15 volatile float theta_reference_2;
sembert 26:292492b885f7 16 const float l = 0.535;
sembert 26:292492b885f7 17 const float EMGx_velocity=0.02;
sembert 26:292492b885f7 18 const float EMGy_velocity=0;
sembert 26:292492b885f7 19 float Ts = 0.001;
sembert 26:292492b885f7 20 float Kp;
sembert 26:292492b885f7 21 float Ki;
sembert 26:292492b885f7 22 float Kd;
sembert 26:292492b885f7 23 QEI encoder_1(D10,D11,NC,8400,QEI::X4_ENCODING);
sembert 26:292492b885f7 24 QEI encoder_2(D12,D13,NC,8400,QEI::X4_ENCODING);
sembert 26:292492b885f7 25 float theta;
sembert 26:292492b885f7 26 float thetav_1;
sembert 26:292492b885f7 27 float thetav_2;
sembert 26:292492b885f7 28
sembert 26:292492b885f7 29 //float ReadEncoder()
sembert 26:292492b885f7 30 //{
sembert 26:292492b885f7 31 // 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.
sembert 26:292492b885f7 32 // theta_2 = ((360.0f/64.0f)*(float)encoder_2.getPulses())/131.25f;
sembert 26:292492b885f7 33 // float theta[2][1] = {{theta_1},{theta_2}};
sembert 26:292492b885f7 34 // return theta
sembert 26:292492b885f7 35 //}
sembert 26:292492b885f7 36
sembert 26:292492b885f7 37 float Controller(float theta_error, bool motor)
sembert 26:292492b885f7 38 {
sembert 26:292492b885f7 39 if (motor == false) {
sembert 26:292492b885f7 40 float K = 1;
sembert 26:292492b885f7 41 float ti = 0.1;
sembert 26:292492b885f7 42 float td = 10;
sembert 26:292492b885f7 43 Kp = K*(1+td/ti);
sembert 26:292492b885f7 44 Ki = K/ti;
sembert 26:292492b885f7 45 Kd = K*td;
sembert 26:292492b885f7 46 } else {
sembert 26:292492b885f7 47 float K = 1;
sembert 26:292492b885f7 48 float ti = 0.1;
sembert 26:292492b885f7 49 float td = 10;
sembert 26:292492b885f7 50 Kp = K*(1+td/ti);
sembert 26:292492b885f7 51 Ki = K/ti;
sembert 26:292492b885f7 52 Kd = K*td;
sembert 26:292492b885f7 53 }
sembert 26:292492b885f7 54 static float error_integral = 0;
sembert 26:292492b885f7 55 static float error_prev = 0;
sembert 26:292492b885f7 56 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
sembert 26:292492b885f7 57
sembert 26:292492b885f7 58 // Proportional part:
sembert 26:292492b885f7 59 float torque_p = Kp * theta_error;
sembert 26:292492b885f7 60
sembert 26:292492b885f7 61 // Integral part:
sembert 26:292492b885f7 62 error_integral = error_integral + theta_error * Ts;
sembert 26:292492b885f7 63 float torque_i = Ki * error_integral;
sembert 26:292492b885f7 64
sembert 26:292492b885f7 65 // Derivative part:
sembert 26:292492b885f7 66 float error_derivative = (theta_error - error_prev)/Ts;
sembert 26:292492b885f7 67 float filtered_error_derivative = LowPassFilter.step(error_derivative);
sembert 26:292492b885f7 68 float torque_d = Kd * filtered_error_derivative;
sembert 26:292492b885f7 69 error_prev = theta_error;
sembert 26:292492b885f7 70
sembert 26:292492b885f7 71 // Sum all parts and return it
sembert 26:292492b885f7 72 float torque = torque_p + torque_i + torque_d;
sembert 26:292492b885f7 73 return torque;
sembert 26:292492b885f7 74 }
sembert 26:292492b885f7 75
sembert 26:292492b885f7 76 void AngleVelocity(float theta_1,float theta_2)
sembert 26:292492b885f7 77 {
sembert 26:292492b885f7 78 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)));
sembert 26:292492b885f7 79 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)}};
sembert 26:292492b885f7 80 thetav_1=DET_jacobian*l*cos(theta_1+theta_2)*EMGx_velocity + DET_jacobian*l*sin(theta_1+theta_2)*EMGy_velocity;
sembert 26:292492b885f7 81 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;
sembert 26:292492b885f7 82 }
sembert 26:292492b885f7 83
sembert 26:292492b885f7 84 int main(void)
sembert 26:292492b885f7 85 {
sembert 26:292492b885f7 86 while(1)
sembert 26:292492b885f7 87 {
sembert 26:292492b885f7 88 AngleVelocity(0.2f,0.0f);
sembert 26:292492b885f7 89 pc.printf('%f \n %f /n/r',thetav_1, thetav_2);
sembert 26:292492b885f7 90 }
sembert 26:292492b885f7 91 }