EMG and motor script together, Not fully working yet,
Dependencies: Encoder QEI biquadFilter mbed
Revision 5:81d3b53087c0, committed 2017-10-27
- Comitter:
- Joost38H
- Date:
- Fri Oct 27 08:44:21 2017 +0000
- Parent:
- 4:fddab1c875a9
- Commit message:
- new mastercode, with code for three motors
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r fddab1c875a9 -r 81d3b53087c0 main.cpp --- a/main.cpp Thu Oct 26 11:32:53 2017 +0000 +++ b/main.cpp Fri Oct 27 08:44:21 2017 +0000 @@ -3,121 +3,126 @@ #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); +//Output motor 3 and reading Encoder motor 3 +DigitalOut motor3DirectionPin(D8); +PwmOut motor3MagnitudePin(D9); +QEI Encoder3(D2,D3,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; - +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(); - +int counts3 = Encoder3.getPulses(); + // Defining variables delta (the difference between position and desired position) <- Is dit zo? int delta1; int delta2; +int delta3; -bool Move_done = false; - +// Boolean needed to know if new input coordinates have to be given +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 */ - +and the Low-pass filter cancels out all frequencies below 4 Hz. The filters are +declared four times, so that they can be used for sampling of right and left +biceps, during measurements and calibration. */ + /* 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 ); - + +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 ); - + +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 ); - + +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 +BiQuadChain bqChain1; +BiQuadChain bqChain2; + +BiQuadChain bqChainTR; +BiQuadChain bqChainTL; + +//Declaring 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 -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 */ - +// Declaring all variables needed for getting the Threshold value +double numsamples = 500; +double emgBRsum = 0; +double emgBRmeanMVC; +double thresholdBR; + +double emgBLsum = 0; +double emgBLmeanMVC; +double thresholdBL; + +/* Function to sample the EMG of the Right Biceps and get a Threshold value +from it, which can be used throughout the process */ + void Threshold_samplingBR() { t_thresholdR.start(); currentTimeTR = t_thresholdR.read(); @@ -132,11 +137,12 @@ } emgBRmeanMVC = emgBRsum/numsamples; thresholdBR = emgBRmeanMVC * 0.20; - - - + //pc.printf("ThresholdBR = %f \n", thresholdBR); } +/* Function to sample the EMG of the Left Biceps and get a Threshold value +from it, which can be used throughout the process */ + void Threshold_samplingBL() { t_thresholdL.start(); currentTimeTL = t_thresholdL.read(); @@ -152,64 +158,130 @@ emgBLmeanMVC = emgBLsum/numsamples; thresholdBL = emgBLmeanMVC * 0.20; - + } + +// EMG sampling and filtering -// 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 +// Function to make the BiQuadChain for the Notch and High pass filter for all three filters void getbqChain() { - bqChain1.add(&bqNotch1).add(&bqHigh1); + bqChain1.add(&bqNotch1).add(&bqHigh1); //Making the BiQuadChain bqChain2.add(&bqNotch2).add(&bqHigh2); + + bqChainTR.add(&bqNotchTR).add(&bqHighTR); + bqChainTL.add(&bqNotchTR).add(&bqHighTL); } - -// Initial input value for X + +// Initial input value for couting the X-values int Xin=0; -int Xin_new; //<- Hoe zit het met deze als we de robot daadwerkelijk gebruiken +int Xin_new; 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); + if (emgBRcomplete > thresholdBR) { + ledtX(); + } + t.start(); + huidigetijdX=t.read(); + if (huidigetijdX>2){ + led_R=1; //Go to the next program (counting values for Y) + Xin_new = Xin; + Xin = 0; + + 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); + if (emgBRcomplete > thresholdBR) { + ledtY(); + } + t.start(); + huidigetijdY=t.read(); + if (huidigetijdY>2){ + led_B=1; + Yin_new = Yin; + Yin = 0; + + Move_done = false; + return Yin_new; + + } + } + } + return 0; // ga door naar het volgende programma +} + + + + // Oude shit voor input waardes geven +/*--------------------------------------------------------------------------------- // Feedback system for counting values of X void ledtX(){ t.reset(); @@ -226,37 +298,28 @@ // Couting system for values of X int tellerX(){ - if (Move_done == true) { - t.reset(); - led_G=1; - led_B=1; - led_R=0; + led_G=1; + led_B=1; + led_R=0; while(true){ - button.fall(ledtX); //This has to be replaced by EMG + //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){ - ledtX(); // dit is wat je uiteindelijk wil dat er staat + 0; // 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; + 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 @@ -274,7 +337,6 @@ // Couting system for values of Y int tellerY(){ - if (Move_done == true) { t.reset(); led_G=1; led_B=0; @@ -283,136 +345,202 @@ //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 - //} + 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 } } - } - 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; - +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; - +double roto; +double rotb; +double rotr; +double rotzo; +double rotzb; +double rotzr; +double counto; +double countb; +double countr; +double countzo; +double countzb; +double countzr; + +double hcounto; +double dcounto; +double hcountb; +double dcountb; +double hcountr; +double dcountr; + // 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 - - +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 + +double speedfactor1; +double speedfactor2; +double speedfactor3; + + //Deel om motor(en) aan te sturen-------------------------------------------- - + + + void calcdelta1() { - delta1 = (counto - Encoder1.getPulses()); + delta1 = (dcounto - Encoder1.getPulses()); + } + +void calcdelta2() { + delta2 = (dcountb - Encoder2.getPulses()); // <------- de reden dat de delta negatief is (jitse) } + +void calcdelta3() { + delta3 = (dcountr - Encoder3.getPulses()); // <------- de reden dat de delta negatief is (jitse) + } + +double referenceVelocity1; +double motorValue1; + +double referenceVelocity2; +double motorValue2; -void calcdelta2() { - delta2 = (countb - Encoder2.getPulses()); // <------- de reden dat de delta negatief is (jitse) - } - -double referenceVelocity1; -double motorValue1; +double referenceVelocity3; +double motorValue3; + +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 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 - +Ticker controlmotor3; // één ticker van maken? +Ticker calculatedelta3; +Ticker printdata3; //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; + referenceVelocity1 = (-1)*speedfactor1 * maxVelocity1; - if (Encoder1.getPulses() < (counto+1)) - { speedfactor = 0.1; + if (dcounto < (10)) + { speedfactor1 = 0.01; + if (dcounto > (-10)) + { printf("kleiner111111111"); + speedfactor1=0; + } } - else if (Encoder1.getPulses() > (counto-1)) - { speedfactor = -0.1; + else if (dcounto > (-10)) + { speedfactor1 = -0.01; + if (dcounto < (10)) + { printf("groter"); + speedfactor1=0; + } } - else - { speedfactor = 0; + else + { speedfactor1 = 0; + pc.printf("speedfactor nul;"); } 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; + referenceVelocity2 = (-1)*speedfactor2 * maxVelocity2; - if (Encoder2.getPulses() < (counto+1)) - { speedfactor = 0.1; + if (Encoder2.getPulses() < (dcountb+10)) + { speedfactor2 = -0.01; + if (Encoder2.getPulses() > (dcountb-10)) + { //printf("kleiner22222222222"); + speedfactor2=0; + } } - else if (Encoder2.getPulses() > (counto-1)) - { speedfactor = -0.1; + else if (Encoder2.getPulses() > (dcountb-10)) + { speedfactor2 = 0.01; + if (Encoder2.getPulses() < (dcountb+10)) + { //printf("groter"); + speedfactor2=0; + } } - else - { speedfactor = 0; + else + { speedfactor2 = 0; + //pc.printf("speedfactor nul;"); } return referenceVelocity2; -} +} +double GetReferenceVelocity3() +{ + // Returns reference velocity in rad/s. Positive value means clockwise rotation. + double maxVelocity3=Vex*25+Vey*25; // max 8.4 in rad/s of course! + referenceVelocity3 = (-1)*speedfactor3 * maxVelocity3; + + if (Encoder3.getPulses() < (dcountr+10)) + { speedfactor3 = -0.01; + if (Encoder3.getPulses() > (dcountr-10)) + { //printf("kleiner22222222222"); + speedfactor3=0; + } + } + else if (Encoder3.getPulses() > (dcountr-10)) + { speedfactor3 = 0.01; + if (Encoder3.getPulses() < (dcountr+10)) + { //printf("groter"); + speedfactor3=0; + } + } + else + { speedfactor3 = 0; + //pc.printf("speedfactor nul;"); + } + + return referenceVelocity3; +} + void SetMotor1(double motorValue1) { // Given -1<=motorValue<=1, this sets the PWM and direction bits for motor 1. Positive value makes @@ -423,7 +551,7 @@ 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 @@ -435,6 +563,17 @@ } +void SetMotor3(double motorValue3) +{ + // 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 (motorValue3 >=0) motor3DirectionPin=1; + else motor3DirectionPin=0; + if (fabs(motorValue3)>1) motor3MagnitudePin = 1; + else motor3MagnitudePin = fabs(motorValue3); + +} + double FeedForwardControl1(double referenceVelocity1) { // very simple linear feed-forward control @@ -442,7 +581,7 @@ double motorValue1 = referenceVelocity1 / MotorGain; return motorValue1; } - + double FeedForwardControl2(double referenceVelocity2) { // very simple linear feed-forward control @@ -450,6 +589,14 @@ double motorValue2 = referenceVelocity2 / MotorGain; return motorValue2; } + +double FeedForwardControl3(double referenceVelocity3) +{ + // very simple linear feed-forward control + const double MotorGain=8.4; // unit: (rad/s) / PWM, max 8.4 + double motorValue3 = referenceVelocity3 / MotorGain; + return motorValue3; +} void MeasureAndControl1() { @@ -459,7 +606,7 @@ double motorValue1 = FeedForwardControl1(referenceVelocity1); SetMotor1(motorValue1); } - + void MeasureAndControl2() { // This function measures the potmeter position, extracts a reference velocity from it, @@ -469,24 +616,40 @@ SetMotor2(motorValue2); } +void MeasureAndControl3() +{ + // 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 referenceVelocity3 = GetReferenceVelocity3(); + double motorValue3 = FeedForwardControl3(referenceVelocity3); + SetMotor3(motorValue3); +} + void readdata1() { //pc.printf("CurrentState = %i \r\n",Encoder.getCurrentState()); - pc.printf("Pulses_M1 = %i \r\n",Encoder1.getPulses()); + //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); + //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("Pulses_M2 = %i \r\n",Encoder2.getPulses()); //pc.printf("Revolutions = %i \r\n",Encoder.getRevolutions()); - pc.printf("Delta_M2 = %i \r\n",delta2); + //pc.printf("Delta_M2 = %i \r\n",delta2); } - + +void readdata3() + { //pc.printf("CurrentState = %i \r\n",Encoder.getCurrentState()); + //pc.printf("Pulses_M2 = %i \r\n",Encoder3.getPulses()); + //pc.printf("Revolutions = %i \r\n",Encoder.getRevolutions()); + //pc.printf("Delta_M2 = %i \r\n",delta3); + } + // 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(){ @@ -495,7 +658,7 @@ 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(){ @@ -504,14 +667,17 @@ rotb=Lbu/omtrekklosje; rotr=Lru/omtrekklosje; counto=roto*4200; - //counto = (int)(counto + 0.5); // omzetten van rotaties naar counts + dcounto=counto-hcounto; + pc.printf("dcounto = %f \n\r",dcounto); countb=rotb*4200; - //countb = (int)(countb + 0.5); + dcountb=countb-hcountb; + pc.printf("dcountb = %f \n\r",dcountb); countr=rotr*4200; - //countr = (int)(countr + 0.5); + dcountr=countr-hcountr; + 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; @@ -519,7 +685,7 @@ touwlengtes(); Pex=Pstx; Pey=Psty; - //pc.printf("een stappie verder\n\r x=%.2f\n\r y=%.2f\n\r",Pstx,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); @@ -529,15 +695,19 @@ 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(){ + if (Move_done==true); Psx=(Xin_new)*30+121; Psy=(Yin_new)*30+308; - //pc.printf("x=%.2f \n\r y=%.2f \n\r",Psx,Psy); + pc.printf("x=%.2f \n\r y=%.2f \n\r",Psx,Psy); + hcounto=4200*((sqrt(pow((Pex-Pox),2)+pow((Pey-Poy),2)))/omtrekklosje); + hcountb=4200*((sqrt(pow((Pex-Pbx),2)+pow((Pey-Pby),2)))/omtrekklosje); + hcountr=4200*((sqrt(pow((Pex-Prx),2)+pow((Pey-Pry),2)))/omtrekklosje); return 0; } - + // Rekent dit de snelheid uit waarmee de motoren bewegen? void Ve(){ Vex=Kz*(Psx-Pex); @@ -548,20 +718,20 @@ Vey=(Vey/modVe)*Vmax; } Pst(); - pc.printf("Vex=%.2f \r\n Vey=%.2f \r\n",Vex,Vey); + //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){ @@ -577,11 +747,11 @@ countzb=rotzb*4200; countzr=rotzr*4200; - pc.printf("o=%.2fb=%.2fr=%.2f",countzo,countzb,countzr); // hier moet komen te staan hoe het zakken gaat + //pc.printf("o=%.2fb=%.2fr=%.2f",countzo,countzb,countzr); // hier moet komen te staan hoe het zakken gaat } } } - + int main() { pc.baud(115200); @@ -591,16 +761,22 @@ 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); + printdata1.attach(&readdata1, 0.5); + //controlmotor2.attach(&MeasureAndControl2, 0.01); + //calculatedelta2.attach(&calcdelta2, 0.01); //printdata2.attach(&readdata2, 0.5); + //controlmotor3.attach(&MeasureAndControl3, 0.01); + //calculatedelta3.attach(&calcdelta3, 0.01); + //printdata3.attach(&readdata3, 0.5); //zakker(); + wait(5.0f); } - return 0; -} \ No newline at end of file + +} + \ No newline at end of file