Diff: main.cpp

Revision:

0:51a6e38a4d4a

Child:

1:9e871647a074

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Oct 26 12:49:05 2017 +0000
@@ -0,0 +1,561 @@
+#include "mbed.h"
+#include "math.h"
+#include "encoder.h"
+#include "QEI.h"
+#include "BiQuad.h"
+
+bool Move_done=false;
+
+
+
+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_timer;
+
+InterruptIn     button(SW2); // Wordt uiteindelijk vervangen door EMG
+
+Timer           t;
+double          speedfactor1; // = 0.01; snelheid in, zonder potmeter gebruik            <- waarom is dit zo?
+double          speedfactor2;
+// 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; 
+
+/* 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 */
+
+// Initial input value for X                                                            
+int Xin=0;  
+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);                           
+        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);      
+    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 
+}
+
+
+
+
+/*---------------------------------------------------------------------------------
+// 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.2);
+    led_G=1;
+    led_R=0;
+    wait(0.5);
+}  
+
+
+// Couting system for values of X
+int tellerX(){
+    t.reset();
+    led_G=1; 
+    led_B=1;
+    led_R=0;
+        while(true){
+    button.fall(ledtX);          //This has to be replaced by EMG   
+    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.2);
+    led_G=1;
+    led_B=0;
+    wait(0.5);
+}  
+
+// Couting system for values of Y
+int tellerY(){
+    t.reset();
+    led_G=1;
+    led_B=0; 
+    led_R=1;
+    while(true){
+    button.fall(ledtY);         //See comments at X   
+    
+    t.start();
+    huidigetijdY=t.read();
+    if (huidigetijdY>2){
+        led_B=1; 
+          
+        //button.fall(0);   // Wat is deze?
+        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;
+
+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
+
+
+
+//Deel om motor(en) aan te sturen--------------------------------------------
+
+/*double Accelaration(){
+    if (countso < statedsverander)
+        { speedfactor = 0.01;
+        speedfactor = speedfactor + 0.01;
+        }
+  }*/  
+    
+
+void calcdelta1() {    
+    delta1 = (dcounto - Encoder1.getPulses());
+    }
+
+void calcdelta2() {    
+    delta2 = (dcountb - 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)*speedfactor1 * maxVelocity1;  
+                
+    if (Encoder1.getPulses() < (dcounto+10))
+        {   speedfactor1 = 0.01;
+            if (Encoder1.getPulses() > (dcounto-10))
+                { printf("kleiner111111111");
+                speedfactor1=0;
+                }
+        }
+        else if (Encoder1.getPulses() > (dcounto-10))
+        {   speedfactor1 = -0.01;
+             if (Encoder1.getPulses() < (dcounto+10))
+                { printf("groter");
+                speedfactor1=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)*speedfactor2 * maxVelocity2;  
+                
+    if (Encoder2.getPulses() < (dcountb+10))
+        {   speedfactor2 = -0.01;
+            if (Encoder2.getPulses() > (dcountb-10))
+                { //printf("kleiner22222222222");
+                speedfactor2=0;
+                }
+        }
+        else if (Encoder2.getPulses() > (dcountb-10))
+        {   speedfactor2 = 0.01;
+             if (Encoder2.getPulses() < (dcountb+10))
+                { //printf("groter");
+                speedfactor2=0;
+                }
+        }
+        else 
+        {   speedfactor2 = 0;
+        //pc.printf("speedfactor nul;");
+        }
+        
+    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; 
+    dcounto=counto-hcounto;
+    pc.printf("dcounto = %f \n\r",dcounto);
+    countb=rotb*4200;
+    dcountb=countb-hcountb;
+    pc.printf("dcountb = %f \n\r",dcountb);
+    countr=rotr*4200;
+    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;
+    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(){
+    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);
+    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);
+    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();
+    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();
+        wait(5.0f);
+        }
+    return 0;
+}
\ No newline at end of file