![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
EMG and motor script together, Not fully working yet,
Dependencies: Encoder QEI biquadFilter mbed
main.cpp
- Committer:
- Joost38H
- Date:
- 2017-10-25
- Revision:
- 0:eb16ed402ffa
- Child:
- 1:6aac013b0ba3
File content as of revision 0:eb16ed402ffa:
#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; }