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
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