christophe vermaelen / Mbed 2 deprecated TEST_ASSER_MOT_ENCODEUR_V2

pour bastos

Dependencies:   mbed

Files at this revision

API Documentation at this revision

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

lib.cpp Show annotated file Show diff for this revision Revisions of this file
lib.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file 
--- /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

--- /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

--- /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);
+
+    }
+}

--- /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