EMG and motor script together, Not fully working yet,
Dependencies: Encoder QEI biquadFilter mbed
Diff: main.cpp
- Revision:
- 3:59b504840b95
- Parent:
- 2:2c4ee76dc830
- Child:
- 4:fddab1c875a9
diff -r 2c4ee76dc830 -r 59b504840b95 main.cpp --- a/main.cpp Wed Oct 25 13:32:29 2017 +0000 +++ b/main.cpp Thu Oct 26 09:55:24 2017 +0000 @@ -44,6 +44,8 @@ int delta1; int delta2; +bool Move_done = false; + /* Defining all the different BiQuad filters, which contain a Notch filter, High-pass filter and Low-pass filter. The Notch filter cancels all frequencies between 49 and 51 Hz, the High-pass filter cancels all frequencies below 20 Hz @@ -146,7 +148,8 @@ } // Initial input value for X -int Xin=0; //<- Hoe zit het met deze als we de robot daadwerkelijk gebruiken +int Xin=0; +int Xin_new; //<- Hoe zit het met deze als we de robot daadwerkelijk gebruiken double huidigetijdX; // Feedback system for counting values of X @@ -165,28 +168,37 @@ // Couting system for values of X int tellerX(){ - led_G=1; - led_B=1; - led_R=0; + if (Move_done == true) { + t.reset(); + led_G=1; + led_B=1; + led_R=0; while(true){ - //button.fall(ledtX); //This has to be replaced by EMG - if (emgBRcomplete > thresholdBR){ + button.fall(ledtX); //This has to be replaced by EMG + if (emgBRcomplete > thresholdBR){ ledtX(); // dit is wat je uiteindelijk wil dat er staat - } - t.start(); - huidigetijdX=t.read(); - if (huidigetijdX>2){ - led_R=1; //Go to the next program (couting values for Y) - if (emgBRcomplete > thresholdBR){ - 0; // dit is wat je uiteindelijk wil dat er staat } - return 0; - } - } + + t.start(); + huidigetijdX=t.read(); + if (huidigetijdX>2){ + led_R=1; //Go to the next program (couting values for Y) + Xin_new = Xin; + Xin = 0; + //button.fall(0); // Wat is deze? + + return Xin_new; + } + + } + + } + return 0; } // Initial values needed for Y (see comments at X function) int Yin=0; +int Yin_new; double huidigetijdY; //Feedback system for couting values of Y @@ -204,6 +216,7 @@ // Couting system for values of Y int tellerY(){ + if (Move_done == true) { t.reset(); led_G=1; led_B=0; @@ -212,22 +225,27 @@ //button.fall(ledtY); //See comments at X if (emgBRcomplete > thresholdBR){ ledtY(); // dit is wat je uiteindelijk wil dat er staat - } + } t.start(); huidigetijdY=t.read(); if (huidigetijdY>2){ led_B=1; - if (emgBRcomplete > thresholdBR){ - 0; // dit is wat je uiteindelijk wil dat er staat - } + Yin_new = Yin; + Yin = 0; + //if (emgBRcomplete > thresholdBR){ + //0; // dit is wat je uiteindelijk wil dat er staat + //} + //button.fall(0); // Wat is deze? + Move_done = false; + return Yin_new; - //button.fall(0); // Wat is deze? - return 0; // ga door naar het volgende programma } } + } + return 0; // ga door naar het volgende programma } -bool B = false; + // Declaring all variables needed for calculating rope lengths, double Pox = 0; @@ -456,8 +474,8 @@ //Calculating desired end position based on the EMG input <- Waarom maar voor een paal? double Ps(){ - Psx=(Xin)*30+121; - Psy=(Yin)*30+308; + Psx=(Xin_new)*30+121; + Psy=(Yin_new)*30+308; //pc.printf("x=%.2f \n\r y=%.2f \n\r",Psx,Psy); return 0; } @@ -474,7 +492,7 @@ Pst(); pc.printf("Vex=%.2f \r\n Vey=%.2f \r\n",Vex,Vey); if((abs(Vex)<0.01f)&&(abs(Vey)<0.01f)){ - B=true; + Move_done=true; loop.detach(); } } @@ -490,7 +508,7 @@ void zakker(){ while(1){ wait(1); - if(B==true){ //misschien moet je hier als voorwaarden een delta is 1 zetten // hierdoor wacht dit programma totdat de beweging klaar is + if(Move_done==true){ //misschien moet je hier als voorwaarden een delta is 1 zetten // hierdoor wacht dit programma totdat de beweging klaar is dLod=sqrt(pow(Lou,2)+pow(397.85,2))-Lou; //dit is wat je motoren moeten doen om te zakken dLbd=sqrt(pow(Lbu,2)+pow(397.85,2))-Lbu; dLrd=sqrt(pow(Lru,2)+pow(397.85,2))-Lru; @@ -518,12 +536,11 @@ calculator(); controlmotor1.attach(&MeasureAndControl1, 0.01); calculatedelta1.attach(&calcdelta1, 0.01); - printdata1.attach(&readdata1, 0.5); + //printdata1.attach(&readdata1, 0.5); controlmotor2.attach(&MeasureAndControl2, 0.01); calculatedelta2.attach(&calcdelta2, 0.01); - printdata2.attach(&readdata2, 0.5); + //printdata2.attach(&readdata2, 0.5); //zakker(); - wait(5.0f); } return 0; } \ No newline at end of file