Vivien van Veldhuizen
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
--- 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