EMG and motor script together, Not fully working yet,
Dependencies: Encoder QEI biquadFilter mbed
Diff: main.cpp
- Revision:
- 0:eb16ed402ffa
- Child:
- 1:6aac013b0ba3
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Oct 25 10:40:18 2017 +0000 @@ -0,0 +1,408 @@ +#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); + +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; + +// Initial input value for X +int Xin=0; //<- 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.5); + led_G=1; + led_R=0; +} + + +// Couting system for values of X +int tellerX(){ + led_G=1; + led_B=1; + while(true){ + button.fall(ledtX); //This has to be replaced by EMG + /*if (EMG>threshold){ + 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) + return 0; + } + } +} + +// Initial values needed for Y (see comments at X function) +int Yin=0; +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.5); + led_G=1; + led_B=0; +} + +// Couting system for values of Y +int tellerY(){ + t.reset(); + led_G=1; + led_B=0; + while(true){ + button.fall(ledtY); //See comments at X + /*if (EMG>threshold){ + Piek(); // dit is wat je uiteindelijk wil dat er staat + }*/ + t.start(); + huidigetijdY=t.read(); + if (huidigetijdY>2){ + led_B=1; + button.fall(0); + return 0; // ga door naar het volgende programma + } + } +} + +bool B = false; + +// 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 + } + +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)*30+121; + Psy=(Yin)*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)){ + B=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(B==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); + 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; +} \ No newline at end of file