Diff: lib.cpp

Revision:

0:e2ce7aa2a4a4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/lib.cpp	Wed Apr 06 12:25:35 2022 +0000
@@ -0,0 +1,301 @@
+#include "mbed.h"
+#include "lib.h"
+Timer temps;
+DigitalOut sensG(D7);
+DigitalOut sensD(D9);
+PwmOut motG(D6);
+PwmOut motD(D8);
+DigitalIn BG(D14);
+InterruptIn AG(D13);
+InterruptIn AD(D12);
+DigitalIn BD(D11);
+int cptG=0,cptD=0;
+int cPosG=0,cPosD=0;
+int erD,somErD=0;
+int erG,somErG=0;
+int cmdD=0,cmdG=0;
+float cPos;
+Ticker tic;
+Ticker tac;
+void asserPos()
+{
+    erD=cPosD-cptD;
+    somErD=somErD+erD;
+    if(somErD>100000)somErD=100000;
+    if(somErD<-100000)somErD=-100000;
+    cmdD=K*erD+K*Te*somErD/Ti;
+
+    erG=cPosG-cptG;
+    somErG=somErG+erG;
+    if(somErG>100000)somErG=100000;
+    if(somErG<-100000)somErG=-100000;
+    cmdG=K*erG+K*Te*somErG/Ti;
+
+    mot(cmdG,cmdD);
+}
+void distance(int d)//distance passee en mm
+{
+    int signe;
+    if(d>0) signe=0;
+    else {
+        signe=1;
+        d=-1*d;
+    }
+    cptG=0;
+    cptD=0;
+    float dlim=1000*V*V/A; //valeur limite entre le profil en trapeze et celui en triangle
+
+    if(DEBUG) printf("Debut asservissement de distance... \n\rDistance a parcourir %d mm... \n\r",d);
+    if(d>dlim) { // profil en trapeze
+
+        float t0=V/A;
+        float t1=d/(V*1000.0);
+        if(DEBUG) printf("duree prevue : %.1f s \n\r",t0+t1);
+        temps.reset();
+        temps.start();
+        cPosG=0;
+        cPosD=0;
+        tic.attach(&asserPos,Te); // on lance l'asservissement de position
+        float t=0;
+        while(t<t0) {
+            t=temps.read();
+            cPos=A*t*t/2.0;
+            if(signe==0)cPosG=cPos*1000.0/mmPULSE;
+            else cPosG=-1*cPos*1000.0/mmPULSE;
+            if(signe==0)cPosD=cPos*1000.0/mmPULSE;
+            else cPosD=-1*cPos*1000.0/mmPULSE;
+        }
+        while(t<t1) {
+            t=temps.read();
+            cPos=(A*t0*t0/2.0)+(V*(t-t0));
+            if(signe==0)cPosG=cPos*1000.0/mmPULSE;
+            else cPosG=-1*cPos*1000.0/mmPULSE;
+            if(signe==0)cPosD=cPos*1000.0/mmPULSE;
+            else cPosD=-1*cPos*1000.0/mmPULSE;
+        }
+        while(t<(t1+t0)) {
+            t=temps.read();
+            cPos=A*t*t/(-2.0)+(V+A*t1)*t+(-1*A*t1*t1/2.0)-V*t1+A*t0*t0/2.0+V*(t1-t0);
+            if(signe==0)cPosG=cPos*1000.0/mmPULSE;
+            else cPosG=-1*cPos*1000.0/mmPULSE;
+            if(signe==0)cPosD=cPos*1000.0/mmPULSE;
+            else cPosD=-1*cPos*1000.0/mmPULSE;
+        }
+        while(t<(t1+t0+DELAY)) {
+            t=temps.read();
+        }
+        if(DEBUG) printf("distance parcourue = %d mm -- mode trapeze\n\r",d);
+        tic.detach();
+        mot(0,0);
+    } else {            //profil en triangle
+        float tA=sqrt(d/(1000.0*A));
+        float t2A=2*tA;
+        if(DEBUG) printf("duree prevue : %.1f s \n\r",t2A);
+        temps.reset();
+        temps.start();
+        cPosG=0;
+        cPosD=0;
+        tac.attach(&asserPos,Te);
+        float t=0;
+        while(t<tA) {
+            t=temps.read();
+            cPos=A*t*t/2;
+            if(signe==0)cPosG=cPos*1000.0/mmPULSE;
+            else cPosG=-1*cPos*1000.0/mmPULSE;
+            if(signe==0)cPosD=cPos*1000.0/mmPULSE;
+            else cPosD=-1*cPos*1000.0/mmPULSE;
+        }
+        while(t<t2A) {
+            t=temps.read();
+            cPos=(2*A*tA*t)-(A*t*t/2)-d/1000.0;
+            if(signe==0)cPosG=cPos*1000.0/mmPULSE;
+            else cPosG=-1*cPos*1000.0/mmPULSE;
+            if(signe==0)cPosD=cPos*1000.0/mmPULSE;
+            else cPosD=-1*cPos*1000.0/mmPULSE;
+        }
+        while(t<(t2A+DELAY)) {
+            t=temps.read();
+        }
+        if(DEBUG) printf("distance parcourue = %d mm -- mode triangle\n\r",d);
+        tac.detach();
+        mot(0,0);
+    }
+    temps.stop();
+    temps.reset();
+
+}
+void rotation(int deg)
+{
+
+    int d=deg*ENTREAXE/360.0;
+    int signe;
+    if(d>0) signe=0;
+    else {
+        signe=1;
+        d=-1*d;
+    }
+    cptG=0;
+    cptD=0;
+    float dlim=1000*V*V/A;
+
+    if(DEBUG) printf("Debut asservissement de distance... \n\rDistance a parcourir %d mm... \n\r",d);
+    if(d>dlim) {
+
+        float t0=V/A;
+        float t1=d/(V*1000.0);
+        if(DEBUG) printf("duree prevue : %.1f s \n\r",t0+t1);
+        temps.reset();
+        temps.start();
+        cPosG=0;
+        cPosD=0;
+        tic.attach(&asserPos,Te);
+        float t=0;
+        while(t<t0) {
+            t=temps.read();
+            cPos=A*t*t/2.0;
+            if(signe==0) cPosG=cPos*1000.0/mmPULSE;
+            else cPosG=-1*cPos*1000.0/mmPULSE;
+            if(signe==0) cPosD=-1*cPos*1000.0/mmPULSE;
+            else cPosD=cPos*1000.0/mmPULSE;
+        }
+        while(t<t1) {
+            t=temps.read();
+            cPos=(A*t0*t0/2.0)+(V*(t-t0));
+            if(signe==0) cPosG=cPos*1000.0/mmPULSE;
+            else cPosG=-1*cPos*1000.0/mmPULSE;
+            if(signe==0) cPosD=-1*cPos*1000.0/mmPULSE;
+            else cPosD=cPos*1000.0/mmPULSE;
+        }
+        while(t<(t1+t0)) {
+            t=temps.read();
+            cPos=A*t*t/(-2.0)+(V+A*t1)*t+(-1*A*t1*t1/2.0)-V*t1+A*t0*t0/2.0+V*(t1-t0);
+            if(signe==0) cPosG=cPos*1000.0/mmPULSE;
+            else cPosG=-1*cPos*1000.0/mmPULSE;
+            if(signe==0) cPosD=-1*cPos*1000.0/mmPULSE;
+            else cPosD=cPos*1000.0/mmPULSE;
+        }
+        while(t<(t1+t0+DELAY)) {
+            t=temps.read();
+        }
+        if(DEBUG) printf("distance parcourue = %d mm -- mode trapeze\n\r",d);
+        tic.detach();
+        mot(0,0);
+    } else {
+        float tA=sqrt(d/(1000.0*A));
+        float t2A=2*tA;
+        if(DEBUG) printf("duree prevue : %.1f s \n\r",t2A);
+        temps.reset();
+        temps.start();
+        cPosG=0;
+        cPosD=0;
+        tac.attach(&asserPos,Te);
+        float t=0;
+        while(t<tA) {
+            t=temps.read();
+            cPos=A*t*t/2;
+            if(signe==0) cPosG=cPos*1000.0/mmPULSE;
+            else cPosG=-1*cPos*1000.0/mmPULSE;
+            if(signe==0) cPosD=-1*cPos*1000.0/mmPULSE;
+            else cPosD=cPos*1000.0/mmPULSE;
+        }
+        while(t<t2A) {
+            t=temps.read();
+            cPos=(2*A*tA*t)-(A*t*t/2)-d/1000.0;
+            if(signe==0) cPosG=cPos*1000.0/mmPULSE;
+            else cPosG=-1*cPos*1000.0/mmPULSE;
+            if(signe==0) cPosD=-1*cPos*1000.0/mmPULSE;
+            else cPosD=cPos*1000.0/mmPULSE;
+        }
+        while(t<(t2A+DELAY)) {
+            t=temps.read();
+        }
+        if(DEBUG) printf("distance parcourue = %d mm -- mode triangle\n\r",d);
+        tac.detach();
+        mot(0,0);
+    }
+    temps.stop();
+    temps.reset();
+
+
+}
+void init()
+{
+    motG.period_us(100);
+    motD.period_us(100);
+    mot(0,0);
+    AG.rise(&countGr);
+    AG.fall(&countGf);
+    AD.rise(&countDr);
+    AD.fall(&countDf);
+    //tic.attach(&asserPos,Te);
+
+}
+void countGr()
+{
+    if(BG==1)
+        cptG++;
+    else
+        cptG--;
+}
+void countDr()
+{
+    if(BD==0)
+        cptD++;
+    else
+        cptD--;
+}
+void countGf()
+{
+    if(BG==0)
+        cptG++;
+    else
+        cptG--;
+}
+void countDf()
+{
+    if(BD==1)
+        cptD++;
+    else
+        cptD--;
+}
+
+
+void aff()
+{
+    float vD=30*cptD/(22.0*5.0*Te);
+    float vG=30*cptG/(22.0*5.0*Te);
+    int valD=100*motD.read();
+    int valG=100*motG.read();
+    //printf("cptG=%d -- motG=%.3f cm/s -- cptD=%d -- motD=%.3f cm/s -- valD=%d\n\r",cptG,vG,cptD,vD,valD);
+
+
+}
+int borne(int a)
+{
+    if(a>100) a=100;
+    if(a<-100) a=-100;
+    return a;
+}
+void mot(int g,int d)
+{
+    g=borne(g);
+    d=borne(d);
+    if(g>0) {
+        sensG.write(1);
+        g=100-g;
+        motG.pulsewidth_us(g);
+    } else {
+        sensG.write(0);
+        g=100+g;
+        motG.pulsewidth_us(g);
+    }
+    if(d>0) {
+        sensD.write(1);
+        d=100-d;
+        motD.pulsewidth_us(d);
+    } else {
+        sensD.write(0);
+        d=100+d;
+        motD.pulsewidth_us(d);
+    }
+}
\ No newline at end of file