![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Signa-bot code for project BioRobotics, at University of Twente.
Dependencies: mbed QEI MODSERIAL FastPWM ttmath Math
Diff: Motor_tryout.cpp
- Revision:
- 36:22d1bcb82061
- Parent:
- 35:4cb2ed6dd2d2
diff -r 4cb2ed6dd2d2 -r 22d1bcb82061 Motor_tryout.cpp --- a/Motor_tryout.cpp Fri Nov 01 08:24:42 2019 +0000 +++ b/Motor_tryout.cpp Fri Nov 01 09:57:21 2019 +0000 @@ -4,6 +4,7 @@ #include "Math.h" #include "ttmath.h" #include "FastPWM.h" +#include "BiQuad.h" MODSERIAL pc(USBTX, USBRX); //Serial term (USBTX, USBRX); @@ -16,6 +17,203 @@ AnalogIn potmeter1(A1); +//Define objects +AnalogIn emg0( A0 ); // 1e +AnalogIn emg2( A2 ); // 2e +AnalogIn emg4( A4 ); // 3e + +float A; +float B; +Ticker sample_timer; +DigitalOut led(LED1); + +const int leng_filt = 60; +const int box_length = 50; +const int box_lengthC = 150; +const int box_checkC = 50; +const float grenswaardeA0 = 0.016; +const float grenswaardeB0 = 0.012; +const float grenswaardeC = 0.012; +float Ay1; +float Ay2; +float A_array[leng_filt] = {0}; +float A_ar[leng_filt] = {0}; +float A_ar2[leng_filt] = {0}; +float A_ar3[box_length] = {0}; +int boxcheckC = 0; +int boxcheckA = 0; +int boxcheckB = 0; + +int boxcheckCC = 0; +int boxcheckAA = 0; +int boxcheckBB = 0; + +float By1; +float By2; +float B_array[leng_filt] = {0}; +float B_ar[leng_filt] = {0}; +float B_ar2[leng_filt] = {0}; +float B_ar3[box_length] = {0}; + +float Cy1; +float Cy2; +float C_array[leng_filt] = {0}; +float C_ar[leng_filt] = {0}; +float C_ar2[leng_filt] = {0}; +float C_ar3[box_lengthC] = {0}; + +float result = 0; +float Asum = 0; +const int Fs = 2000; //Sample Frequency +const double b0 = 0.292893; +const double b1 = 0.585786; +const double b2 = 0.292893; +const double a0 = 1.000000; +const double a1 = -0.00000; +const double a2 = 0.171573; + +void sample() +{ + BiQuad lowpassA(b0,b1, b2, a0, a1, a2); + // Signaal 1 (A) + float A = emg0.read(); + float Amean = 0; + float Ay2 = 0; + + for (int j=leng_filt-1; j>=1; j--) + { A_ar[j] = A_ar[j-1]; } + + A_ar[0] = A; + + for(int i=0; i<=leng_filt-1; i++) + { Amean += A_ar[i]*1/leng_filt; } + + Ay1 = A - Amean; + Ay1 = fabs(Ay1); + Ay1 = lowpassA.step(Ay1); // First signal, after Butterworth + + for (int j=leng_filt-1; j>=1; j--) + { A_ar2[j] = A_ar2[j-1]; } + + A_ar2[0] = Ay1; + + for(int i=0; i<=leng_filt-1; i++) + { Ay2 += A_ar2[i]*1/leng_filt; } + + float Ay3; + if(Ay2>grenswaardeA0) + { Ay3 = 1; } + + //if(Ay2<=grenswaardeA0) + //{ if(Ay2>grenswaardeA1) + // { Ay3 = 0.5; } } + + if(Ay2<=grenswaardeA0) + { Ay3 = 0; } + + for (int j=box_length-1; j>=1; j--) + { A_ar3[j] = A_ar3[j-1]; } + + A_ar3[0] = Ay3; + boxcheckA = 0; + + for (int j=0; j<=box_length-1; j++) + { if(A_ar3[j] == 1) + { boxcheckA = 1; + boxcheckAA = 1;} } + + // Signaal 2 (B) + BiQuad lowpassB(b0,b1, b2, a0, a1, a2); + float B = emg2.read(); + float Bmean = 0; + float By2 = 0; + + for (int j=leng_filt-1; j>=1; j--) + { B_ar[j] = B_ar[j-1]; } + + B_ar[0] = B; + + for(int i=0; i<=leng_filt-1; i++) + { Bmean += B_ar[i]*1/leng_filt; } + + By1 = B - Bmean; + By1 = fabs(By1); + By1 = lowpassB.step(By1); // First signal, after Butterworth + + for (int j=leng_filt-1; j>=1; j--) + { B_ar2[j] = B_ar2[j-1]; } + + B_ar2[0] = By1; + + for(int i=0; i<=leng_filt-1; i++) + { By2 += B_ar2[i]*1/leng_filt; } + + float By3; + if(By2>grenswaardeB0) + { By3 = 1; } + + if(By2<=grenswaardeB0) + { By3 = 0; } + + for (int j=box_length-1; j>=1; j--) + { B_ar3[j] = B_ar3[j-1]; } + + B_ar3[0] = By3; + boxcheckB = 0; + + for (int j=0; j<=box_length-1; j++) + { if(B_ar3[j] == 1) + { boxcheckB = 1; + boxcheckBB = 1;} } + + // Signaal 3 (C) + BiQuad lowpassC(b0,b1, b2, a0, a1, a2); + float C = emg4.read(); + float Cmean = 0; + float Cy2 = 0; + + for (int j=leng_filt-1; j>=1; j--) + { C_ar[j] = C_ar[j-1]; } + + C_ar[0] = C; + + for(int i=0; i<=leng_filt-1; i++) + { Cmean += C_ar[i]*1/leng_filt; } + + Cy1 = C - Cmean; + Cy1 = fabs(Cy1); + Cy1 = lowpassC.step(Cy1); // First signal, after Butterworth + + for (int j=leng_filt-1; j>=1; j--) + { C_ar2[j] = C_ar2[j-1]; } + + C_ar2[0] = Cy1; + + for(int i=0; i<=leng_filt-1; i++) + { Cy2 += C_ar2[i]*1/leng_filt; } + + float Cy3; + if(Cy2>grenswaardeC) + { Cy3 = 1; } + + if(Cy2<=grenswaardeC) + { Cy3 = 0; } + + for (int j=box_lengthC-1; j>=1; j--) + { C_ar3[j] = C_ar3[j-1]; } + + C_ar3[0] = Cy3; + boxcheckC = 0; + int C_sum = 0; + for (int j=0; j<=box_length-1; j++) + { C_sum += C_ar3[j]; + if(C_sum >= box_checkC) + { boxcheckC = 1; + boxcheckCC=1;} + } + + led = !led; +} QEI Encoder1(D12,D13,NC,64,QEI::X4_ENCODING); @@ -218,11 +416,10 @@ //z0 = 158 - 60* sin(3*time_sin); -if (sign == 1) { - float r = 20*(1-1/(1+(time_sin))); - x0 = savedX0 - r*sin(3*time_sin); - y0 = savedY0 - r*cos(3*time_sin); -} +// float r = 20*(1-1/(1+(time_sin))); +// x0 = r*sin(3*time_sin); +// y0 = r*cos(3*time_sin); + delta_calctheta1 (); @@ -293,11 +490,10 @@ delta_calctheta2 (); delta_calctheta3 (); - pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3); + motor_timer.attach(&motor, 0.002); +// sample_timer.attach(&sample, 0.005); - char cc = pc.getc(); - - motor_timer.attach(&motor, 0.002); + char cc = pc.getc(); int frequency_pwm = 10000; //10 kHz PWM @@ -305,93 +501,51 @@ motor2_pwm.period_us(6); motor3_pwm.period_us(6); - pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3); - pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3); - pc.printf("\n\rposition (%f %f %f)", x0, y0, z0); - - pc.baud(115200); while(true){ - - while (boxcheckC == 0) { - if (boxcheckA == 1 && x0>=-76 && x0<=75) { - x0=x0+1.0f ; - wait(1.0/20); - } - if (boxcheckB== 1 && x0>=-75 && x0<=76) { - x0=x0-1.0f; - wait(1.0/20); - } - } - - wait(1.5); - savedX = x0; - - while (boxcheckC == 0) { - if (boxcheckA==1 && y0>=-76 && y0<=75){ - y0=y0+1.0f; - wait(1.0/20); - } - if (boxcheckB==1 && y0>=-75 && y0<=76){ - y0=y0-1.0f; - wait(1.0/20); - } - } - - wait(1.5); - savedY = y0; - - while (boxcheckC == 0) { - if (boxcheckA == 1 && z0>=-179 && z0<=-158){ - z0=z0+1.0f; - wait(1.0/20); - } - if (boxcheckB == 1 && z0>=-178 && z0<=-157){ - z0=z0-1.0f; - wait(1.0/20); - } - } - - wait(1.5); - savedZ = z0; - - if (boxcheckC == 1) { - sign = 1; - } - - wait(5.0); - - sign = 0; - - float diffZ = -158 - z0; - float diffX = 0 - x0; - float diffY = 0 - y0; - - for (int i=0; i<=diffZ; i++) { - z0 = z0+1; - } - - for (int i=0; i<=diffY; i++) { - if (diffY>0) { - y0 = y0+1; - } - if (diffY<0) { - y0 = y0-1; - } - } - - for (int i=0; i<=diffX; i++) { - if (diffX>0) { - x0 = x0+1; - } - if (diffX<0) { - x0 = x0-1; - } - } - -break; +// +// +// while (boxcheckCC == 0) { +// if (boxcheckAA == 1 && z0>=-179 && z0<=-158){ +// z0=z0+1.0f; +// wait(1.0/2000); +// boxcheckAA = 0; +// } +// if (boxcheckBB == 1 && z0>=-178 && z0<=-157){ +// z0=z0-1.0f; +// wait(1.0/2000); +// boxcheckBB = 0; +// } +// } +// +// wait(1.5); +// boxcheckCC = 0; +// savedZ = z0; +// +// sign = 1; +// +// wait(5.0); +// +// sign = 0; +// boxcheckCC = 0; +// +// +// float diffZ = -158 - z0; +// +// +// for (int i=0; i<=diffZ; i++) { +// if (diffZ>0) { +// z0 = z0+1; +// } +// if (diffZ<0) { +// z0 = z0-1; +// } +// wait(1.0/20); +// } +// +//break; @@ -400,44 +554,51 @@ // wait(0.1); -// char cc = pc.getc(); -// if (cc=='d' && x0>=-76 && x0<=75) { -// x0=x0+1.0f ; -// } -// -// if (cc=='a' && x0>=-75 && x0<=76) { -// -// x0=x0-1.0f; -// } -// -// if (cc=='w' && y0>=-76 && y0<=75){ -// y0=y0+1.0f; -// pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3); -// pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3); -// pc.printf("\n\rposition (%f %f %f)", x0, y0, z0); -// -// } -// -// if (cc=='s' && y0>=-75 && y0<=76){ -// y0=y0-1.0f; -// pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3); -// pc .printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3); -// pc.printf("\n\rposition (%f %f %f)", x0, y0, z0); -// } -// -// if (cc=='u' && z0>=-211 && z0<=-130){ -// pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3); -// pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3); -// pc.printf("\n\rposition (%f %f %f)", x0, y0, z0); -// z0=z0+1.0f; -// } -// -// if (cc=='j' && z0>=-210 && z0<=-129){ -// pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3); -// pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3); -// pc.printf("\n\rposition (%f %f %f)", x0, y0, z0); -// z0=z0-1.0f; -// } + char cc = pc.getc(); + if (cc=='d' && x0>=-76 && x0<=75) { + x0=x0+1.0f ; + savedX=x0; + } + + if (cc=='a' && x0>=-75 && x0<=76) { + + x0=x0-1.0f; + savedX=x0; + } + + if (cc=='w' && y0>=-76 && y0<=75){ + y0=y0+1.0f; + pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3); + pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3); + pc.printf("\n\rposition (%f %f %f)", x0, y0, z0); + savedY=y0; + + } + + if (cc=='s' && y0>=-75 && y0<=76){ + y0=y0-1.0f; + pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3); + pc .printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3); + pc.printf("\n\rposition (%f %f %f)", x0, y0, z0); + savedY=y0; + } + + if (cc=='u' && z0>=-211 && z0<=-130){ + pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3); + pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3); + pc.printf("\n\rposition (%f %f %f)", x0, y0, z0); + z0=z0+1.0f; + savedZ=z0; + } + + if (cc=='j' && z0>=-210 && z0<=-129){ + pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3); + pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3); + pc.printf("\n\rposition (%f %f %f)", x0, y0, z0); + z0=z0-1.0f; + savedZ=z0; + } + } //END MAIN } \ No newline at end of file