EMG and motor script together, Not fully working yet,
Dependencies: Encoder QEI biquadFilter mbed
main.cpp
- Committer:
- Joost38H
- Date:
- 2017-10-26
- Revision:
- 4:fddab1c875a9
- Parent:
- 3:59b504840b95
- Child:
- 5:81d3b53087c0
File content as of revision 4:fddab1c875a9:
#include "mbed.h" #include "math.h" #include "encoder.h" #include "QEI.h" #include "BiQuad.h" Serial pc(USBTX, USBRX); //Defining all in- and outputs //EMG input AnalogIn emgBR( A0 ); //Right Biceps AnalogIn emgBL( A1 ); //Left Biceps //Output motor 1 and reading Encoder motor 1 DigitalOut motor1DirectionPin(D4); PwmOut motor1MagnitudePin(D5); QEI Encoder1(D12,D13,NC,32); //Output motor 2 and reading Encoder motor 2 DigitalOut motor2DirectionPin(D7); PwmOut motor2MagnitudePin(D6); QEI Encoder2(D10,D11,NC,32); //LED output, needed for feedback DigitalOut led_R(LED_RED); DigitalOut led_G(LED_GREEN); DigitalOut led_B(LED_BLUE); //Setting Tickers for sampling EMG and determing if the threshold is met Ticker sample_timer; Ticker threshold_timerR; Ticker threshold_timerL; Timer t_thresholdR; Timer t_thresholdL; double currentTimeTR; double currentTimeTL; InterruptIn button(SW2); // Wordt uiteindelijk vervangen door EMG Timer t; double speedfactor; // = 0.01; snelheid in, zonder potmeter gebruik <- waarom is dit zo? // Getting the counts from the Encoder int counts1 = Encoder1.getPulses(); int counts2 = Encoder2.getPulses(); // Defining variables delta (the difference between position and desired position) <- Is dit zo? 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 and the Low-pass filter cancels out all frequencies below 4 Hz */ /* Defining all the normalized values of b and a in the Notch filter for the creation of the Notch BiQuad */ BiQuad bqNotch1( 0.9876, -1.5981, 0.9876, -1.5981, 0.9752 ); BiQuad bqNotch2( 0.9876, -1.5981, 0.9876, -1.5981, 0.9752 ); BiQuad bqNotchTR( 0.9876, -1.5981, 0.9876, -1.5981, 0.9752 ); BiQuad bqNotchTL( 0.9876, -1.5981, 0.9876, -1.5981, 0.9752 ); /* Defining all the normalized values of b and a in the High-pass filter for the creation of the High-pass BiQuad */ BiQuad bqHigh1( 0.8371, -1.6742, 0.8371, -1.6475, 0.7009 ); BiQuad bqHigh2( 0.8371, -1.6742, 0.8371, -1.6475, 0.7009 ); BiQuad bqHighTR( 0.8371, -1.6742, 0.8371, -1.6475, 0.7009 ); BiQuad bqHighTL( 0.8371, -1.6742, 0.8371, -1.6475, 0.7009 ); /* Defining all the normalized values of b and a in the Low-pass filter for the creation of the Low-pass BiQuad */ BiQuad bqLow1( 6.0985e-4, 0.0012, 6.0985e-4, -1.9289, 0.9314 ); BiQuad bqLow2( 6.0985e-4, 0.0012, 6.0985e-4, -1.9289, 0.9314 ); BiQuad bqLowTR( 6.0985e-4, 0.0012, 6.0985e-4, -1.9289, 0.9314 ); BiQuad bqLowTL( 6.0985e-4, 0.0012, 6.0985e-4, -1.9289, 0.9314 ); // Creating a variable needed for the creation of the BiQuadChain BiQuadChain bqChain1; BiQuadChain bqChain2; BiQuadChain bqChainTR; BiQuadChain bqChainTL; //Defining all doubles needed in the filtering process double emgBRfiltered; //Right biceps Notch+High pass filter double emgBRrectified; //Right biceps rectified double emgBRcomplete; //Right biceps low-pass filter, filtering complete double emgBLfiltered; //Left biceps Notch+High pass filter double emgBLrectified; //Left biceps rectified double emgBLcomplete; //Left biceps low-pass filter, filtering complete int countBR = 0; int countBL = 0; //double threshold = 0.03; double numsamples = 500; double emgBRsum = 0; double emgBRmeanMVC; double thresholdBR; double emgBLsum = 0; double emgBLmeanMVC; double thresholdBL; /* Function to sample the EMG and get a Threshold value from it, which can be used throughout the process */ void Threshold_samplingBR() { t_thresholdR.start(); currentTimeTR = t_thresholdR.read(); if (currentTimeTR <= 1) { emgBRfiltered = bqChainTR.step( emgBR.read() ); //Notch+High-pass emgBRrectified = fabs(emgBRfiltered); //Rectification emgBRcomplete = bqLowTR.step(emgBRrectified); //Low-pass emgBRsum = emgBRsum + emgBRcomplete; } emgBRmeanMVC = emgBRsum/numsamples; thresholdBR = emgBRmeanMVC * 0.20; //pc.printf("ThresholdBR = %f \n", thresholdBR); } void Threshold_samplingBL() { t_thresholdL.start(); currentTimeTL = t_thresholdL.read(); if (currentTimeTL <= 1) { emgBLfiltered = bqChain2.step( emgBL.read() ); //Notch+High-pass emgBLrectified = fabs( emgBLfiltered ); //Rectification emgBLcomplete = bqLow2.step( emgBLrectified ); //Low-pass emgBLsum = emgBLsum + emgBLcomplete; } emgBLmeanMVC = emgBLsum/numsamples; thresholdBL = emgBLmeanMVC * 0.20; } // EMG functions void EMG_sample() { //Filtering steps for the Right Biceps EMG emgBRfiltered = bqChain1.step( emgBR.read() ); //Notch+High-pass emgBRrectified = fabs(emgBRfiltered); //Rectification emgBRcomplete = bqLow1.step(emgBRrectified); //Low-pass /*Getting threshold value for Right Biceps, a value of 20% of Maximum Voluntary Contraction is chosen as threshold value */ /*if (countBR < numsamples) { emgBRsum = emgBRsum + emgBRcomplete; countBR++; led_R = 0; led_B = 0; led_G = 1; } emgBRmeanMVC = emgBRsum / numsamples; thresholdBR = emgBRmeanMVC * 0.25;*/ //Filtering steps for the Left Biceps EMG emgBLfiltered = bqChain2.step( emgBL.read() ); //Notch+High-pass emgBLrectified = fabs( emgBLfiltered ); //Rectification emgBLcomplete = bqLow2.step( emgBLrectified ); //Low-pass /*Getting threshold value for Left Biceps, a value of 20% of Maximum Voluntary Contraction is chosen as threshold value */ /*if (countBL < numsamples) { emgBLsum = emgBLsum + emgBLcomplete; countBL++; } emgBLmeanMVC = emgBLsum / numsamples; thresholdBL = emgBLmeanMVC * 0.25; //pc.printf("ThresholdBR = %0.3f, ThresholdBL = %0.3f \n", thresholdBR,thresholdBL);*/ } // Function to make the BiQuadChain for the Notch and High pass filter for both filters void getbqChain() { bqChain1.add(&bqNotch1).add(&bqHigh1); bqChain2.add(&bqNotch2).add(&bqHigh2); } // Initial input value for X 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 void ledtX(){ t.reset(); Xin++; pc.printf("Xin is %i\n",Xin); led_G=0; led_R=1; wait(0.2); led_G=1; led_R=0; wait(0.5); } // Couting system for values of X int tellerX(){ 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){ 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) 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 void ledtY(){ t.reset(); Yin++; pc.printf("Yin is %i\n",Yin); led_G=0; led_B=1; wait(0.2); led_G=1; led_B=0; wait(0.5); } // Couting system for values of Y int tellerY(){ if (Move_done == true) { t.reset(); led_G=1; led_B=0; led_R=1; while(true){ //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; 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; } } } return 0; // ga door naar het volgende programma } // Declaring all variables needed for calculating rope lengths, double Pox = 0; double Poy = 0; double Pbx = 0; double Pby = 887; double Prx = 768; double Pry = 443; double Pex=121; double Pey=308; double diamtrklosje=20; double pi=3.14159265359; double omtrekklosje=diamtrklosje*pi; double Lou; double Lbu; double Lru; double dLod; double dLbd; double dLrd; // Declaring variables needed for calculating motor counts double roto; double rotb; double rotr; double rotzo; double rotzb; double rotzr; double counto; double countb; double countr; double countzo; double countzb; double countzr; // Declaring variables neeeded for calculating motor movements to get to a certain point <- klopt dit? double Psx; double Psy; double Vex; double Vey; double Kz=0.7; // nadersnelheid instellen double modVe; double Vmax=20; double Pstx; double Psty; double T=0.02;//seconds //Deel om motor(en) aan te sturen-------------------------------------------- void calcdelta1() { delta1 = (counto - Encoder1.getPulses()); } void calcdelta2() { delta2 = (countb - Encoder2.getPulses()); // <------- de reden dat de delta negatief is (jitse) } double referenceVelocity1; double motorValue1; double referenceVelocity2; double motorValue2; Ticker controlmotor1; // één ticker van maken? Ticker calculatedelta1; Ticker printdata1; //aparte ticker om print pc aan te kunnen spreken zonder get te worden van hoeveelheid geprinte waardes Ticker controlmotor2; // één ticker van maken? Ticker calculatedelta2; Ticker printdata2; //aparte ticker om print pc aan te kunnen spreken zonder get te worden van hoeveelheid geprinte waardes double GetReferenceVelocity1() { // Returns reference velocity in rad/s. Positive value means clockwise rotation. double maxVelocity1=Vex*25+Vey*25; // max 8.4 in rad/s of course! referenceVelocity1 = (-1)*speedfactor * maxVelocity1; if (Encoder1.getPulses() < (counto+1)) { speedfactor = 0.1; } else if (Encoder1.getPulses() > (counto-1)) { speedfactor = -0.1; } else { speedfactor = 0; } return referenceVelocity1; } double GetReferenceVelocity2() { // Returns reference velocity in rad/s. Positive value means clockwise rotation. double maxVelocity2=Vex*25+Vey*25; // max 8.4 in rad/s of course! referenceVelocity2 = (-1)*speedfactor * maxVelocity2; if (Encoder2.getPulses() < (counto+1)) { speedfactor = 0.1; } else if (Encoder2.getPulses() > (counto-1)) { speedfactor = -0.1; } else { speedfactor = 0; } return referenceVelocity2; } void SetMotor1(double motorValue1) { // Given -1<=motorValue<=1, this sets the PWM and direction bits for motor 1. Positive value makes // motor rotating clockwise. motorValues outside range are truncated to within range if (motorValue1 >=0) motor1DirectionPin=1; else motor1DirectionPin=0; if (fabs(motorValue1)>1) motor1MagnitudePin = 1; else motor1MagnitudePin = fabs(motorValue1); } void SetMotor2(double motorValue2) { // Given -1<=motorValue<=1, this sets the PWM and direction bits for motor 1. Positive value makes // motor rotating clockwise. motorValues outside range are truncated to within range if (motorValue2 >=0) motor2DirectionPin=1; else motor2DirectionPin=0; if (fabs(motorValue2)>1) motor2MagnitudePin = 1; else motor2MagnitudePin = fabs(motorValue2); } double FeedForwardControl1(double referenceVelocity1) { // very simple linear feed-forward control const double MotorGain=8.4; // unit: (rad/s) / PWM, max 8.4 double motorValue1 = referenceVelocity1 / MotorGain; return motorValue1; } double FeedForwardControl2(double referenceVelocity2) { // very simple linear feed-forward control const double MotorGain=8.4; // unit: (rad/s) / PWM, max 8.4 double motorValue2 = referenceVelocity2 / MotorGain; return motorValue2; } void MeasureAndControl1() { // This function measures the potmeter position, extracts a reference velocity from it, // and controls the motor with a simple FeedForward controller. Call this from a Ticker. double referenceVelocity1 = GetReferenceVelocity1(); double motorValue1 = FeedForwardControl1(referenceVelocity1); SetMotor1(motorValue1); } void MeasureAndControl2() { // This function measures the potmeter position, extracts a reference velocity from it, // and controls the motor with a simple FeedForward controller. Call this from a Ticker. double referenceVelocity2 = GetReferenceVelocity2(); double motorValue2 = FeedForwardControl2(referenceVelocity2); SetMotor2(motorValue2); } void readdata1() { //pc.printf("CurrentState = %i \r\n",Encoder.getCurrentState()); pc.printf("Pulses_M1 = %i \r\n",Encoder1.getPulses()); //pc.printf("Revolutions = %i \r\n",Encoder.getRevolutions()); pc.printf("Delta_M1 = %i \r\n",delta1); } void readdata2() { //pc.printf("CurrentState = %i \r\n",Encoder.getCurrentState()); pc.printf("Pulses_M2 = %i \r\n",Encoder2.getPulses()); //pc.printf("Revolutions = %i \r\n",Encoder.getRevolutions()); pc.printf("Delta_M2 = %i \r\n",delta2); } // einde deel motor------------------------------------------------------------------------------------ Ticker loop; /*Calculates ropelengths that are needed to get to new positions, based on the set coordinates and the position of the poles */ double touwlengtes(){ Lou=sqrt(pow((Pstx-Pox),2)+pow((Psty-Poy),2)); Lbu=sqrt(pow((Pstx-Pbx),2)+pow((Psty-Pby),2)); Lru=sqrt(pow((Pstx-Prx),2)+pow((Psty-Pry),2)); return 0; } /* Calculates rotations (and associated counts) of the motor to get to the desired new position*/ double turns(){ roto=Lou/omtrekklosje; rotb=Lbu/omtrekklosje; rotr=Lru/omtrekklosje; counto=roto*4200; //counto = (int)(counto + 0.5); // omzetten van rotaties naar counts countb=rotb*4200; //countb = (int)(countb + 0.5); countr=rotr*4200; //countr = (int)(countr + 0.5); return 0; } // Waar komen Pstx en Psty vandaan en waar staan ze voor? En is dit maar voor een paal? double Pst(){ Pstx=Pex+Vex*T; Psty=Pey+Vey*T; touwlengtes(); Pex=Pstx; Pey=Psty; //pc.printf("een stappie verder\n\r x=%.2f\n\r y=%.2f\n\r",Pstx,Psty); //pc.printf("met lengtes:\n\r Lo=%.2f Lb=%.2f Lr=%.2f\n\r",Lou,Lbu,Lru); turns(); //pc.printf("rotatie per motor:\n\r o=%.2f b=%.2f r=%.2f\n\r",roto,rotb,rotr); pc.printf("counts per motor:\n\r o=%.2f b=%.2f r=%.2f\n\r",counto,countb,countr); /*float R; R=Vex/Vey; // met dit stukje kan je zien dat de verhouding tussen Vex en Vey constant is en de end efector dus een rechte lijn maakt pc.printf("\n\r R=%f",R);*/ return 0; } //Calculating desired end position based on the EMG input <- Waarom maar voor een paal? double Ps(){ Psx=(Xin_new)*30+121; Psy=(Yin_new)*30+308; //pc.printf("x=%.2f \n\r y=%.2f \n\r",Psx,Psy); return 0; } // Rekent dit de snelheid uit waarmee de motoren bewegen? void Ve(){ Vex=Kz*(Psx-Pex); Vey=Kz*(Psy-Pey); modVe=sqrt(pow(Vex,2)+pow(Vey,2)); if(modVe>Vmax){ Vex=(Vex/modVe)*Vmax; Vey=(Vey/modVe)*Vmax; } Pst(); pc.printf("Vex=%.2f \r\n Vey=%.2f \r\n",Vex,Vey); if((abs(Vex)<0.01f)&&(abs(Vey)<0.01f)){ Move_done=true; loop.detach(); } } // Calculating the desired position, so that the motors can go here int calculator(){ Ps(); loop.attach(&Ve,0.02); return 0; } // Function which makes it possible to lower the end-effector to pick up a piece void zakker(){ while(1){ wait(1); 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; rotzo=dLod/omtrekklosje; rotzb=dLbd/omtrekklosje; rotzr=dLrd/omtrekklosje; countzo=rotzo*4200; countzb=rotzb*4200; countzr=rotzr*4200; pc.printf("o=%.2fb=%.2fr=%.2f",countzo,countzb,countzr); // hier moet komen te staan hoe het zakken gaat } } } int main() { pc.baud(115200); getbqChain(); threshold_timerR.attach(&Threshold_samplingBR, 0.002); threshold_timerL.attach(&Threshold_samplingBL, 0.002); while(true){ sample_timer.attach(&EMG_sample, 0.002); wait(2.5f); tellerX(); tellerY(); calculator(); controlmotor1.attach(&MeasureAndControl1, 0.01); calculatedelta1.attach(&calcdelta1, 0.01); //printdata1.attach(&readdata1, 0.5); controlmotor2.attach(&MeasureAndControl2, 0.01); calculatedelta2.attach(&calcdelta2, 0.01); //printdata2.attach(&readdata2, 0.5); //zakker(); } return 0; }