Revision 0:e2ce7aa2a4a4, committed 2022-04-06

Comitter:

vermaelen

Date:

Wed Apr 06 12:25:35 2022 +0000

Commit message:

v2

Changed in this revision

diff -r 000000000000 -r e2ce7aa2a4a4 lib.cpp
--- /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

diff -r 000000000000 -r e2ce7aa2a4a4 lib.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/lib.h	Wed Apr 06 12:25:35 2022 +0000
@@ -0,0 +1,38 @@
+#define Te 0.001
+#define Ti 1000
+#define K 5.0
+#define A 0.3
+#define V 1.5
+#define mmPULSE 1.487
+#define ENTREAXE 722.56//*(1+(4.185/360))
+#define DELAY 1
+#include "mbed.h"
+#include "math.h"
+#define DEBUG 0 //1 affiche les printf, 0 n'affiche rien
+extern Timer temps;
+extern int cptG;
+extern int cptD;
+extern DigitalOut sensG;
+extern DigitalOut sensD;
+extern PwmOut motG;
+extern PwmOut motD;
+extern InterruptIn AG,AD;
+extern DigitalIn BG,BD;
+extern Ticker tic,tac;
+extern int cPosG,cPosD;
+extern int erD,somErD;
+extern int erG,somErG;
+extern int cmdG,cmdD;
+extern float cPos;
+void distance(int);
+void rotation(int);
+void init();
+int borne(int);
+void mot(int,int);
+void asserPos();
+
+void countGr();
+void countDr();
+void countGf();
+void countDf();
+void aff();
\ No newline at end of file

diff -r 000000000000 -r e2ce7aa2a4a4 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Apr 06 12:25:35 2022 +0000
@@ -0,0 +1,33 @@
+#include "mbed.h"
+#include "lib.h"
+
+void fct()
+{
+    if(cPosD==0)cPosD=220;
+    else cPosD=0;
+    if(cPosG==0)cPosG=220;
+    else cPosG=0;
+}
+void aff2(){
+    printf("cPosG=%d -- cptG=%d -- cmdG=%d -- cPos=%.2f\n\r",cPosG,cptG,cmdG,cPos);
+    }
+int main()
+{
+    init();
+//tac.attach(&aff2,0.5);
+    while(1) {
+        //printf("erD=%d -- somErD=%d -- cptD=%d -- cmd=%d\n\r",erD,somErD,cptD,cmdD);
+        printf("Go !!\n\r");
+        
+        distance(2000);
+        
+        rotation(180);
+        distance(1000);
+        rotation(360);
+        distance(1000);
+        rotation(-180);
+        
+        wait(2);
+
+    }
+}

diff -r 000000000000 -r e2ce7aa2a4a4 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Wed Apr 06 12:25:35 2022 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file