Revision 25:bfe7c49e76cd, committed 2013-10-30

Comitter:

Socrates

Date:

Wed Oct 30 16:06:26 2013 +0000

Parent:

24:2e5290d8230b

Child:

26:539f131cf07c

Commit message:

Werkende regelaar voor lage snelheid sinus.

Changed in this revision

--- a/main.cpp	Wed Oct 30 13:02:45 2013 +0000
+++ b/main.cpp	Wed Oct 30 16:06:26 2013 +0000
@@ -3,8 +3,8 @@
 #include "encoder.h"
 #define PI 3.14159265358979323
 //XenY
-//Een pwm van 0.05 is net genoeg om de heugel te bewegen. linksom bewegen is negatief voor de encoder. getposition gaat in ticks. 
-//4123 ticks is een rondje. 
+//Een pwm van 0.05 is net genoeg om de heugel te bewegen. linksom bewegen is negatief voor de encoder. getposition gaat in ticks.
+//4123 ticks is een rondje.
 //Rechts is x, links is y
 AnalogIn emgtr(PTB3);
 AnalogIn emgbr(PTB2);
@@ -23,7 +23,7 @@
 volatile bool looptimerflag;
 void setlooptimerflag(void)
 {
-looptimerflag = true;
+    looptimerflag = true;
 }
 
 volatile bool dirflagx=true;
@@ -31,17 +31,17 @@
 
 void tricheck(void)
 {
-dirflagx=true;
-dirflagy=true;
+    dirflagx=true;
+    dirflagy=true;
 }
 
 int main()
 {
-    pwm_A.period(1.0/22000.0);
-    pwm_B.period(1.0/22000.0);
+    pwm_A.period(1.0/1000.0);
+    pwm_B.period(1.0/1000.0);
     Ticker looptimer;
     Timeout dirtimeout;
-    const float ts=0.001;
+    const float ts=0.004;
     looptimer.attach(setlooptimerflag,ts);
     float numh1,numh2,denh2,numl1,numl2,numl3,denl2,denl3;
     float xtr,ytr,y1tr,x1tr,ztr,z1tr,z2tr,yabstr,yabs1tr,yabs2tr,ktr;
@@ -49,7 +49,7 @@
     float xtl,ytl,y1tl,x1tl,ztl,z1tl,z2tl,yabstl,yabs1tl,yabs2tl,ktl;
     float xbl,ybl,y1bl,x1bl,zbl,z1bl,z2bl,yabsbl,yabs1bl,yabs2bl,kbl;
     float zx,zy;
-    float xuit,yuit, rt; 
+    float xuit,yuit, rt;
     int xdir, ydir, Adir, Bdir;
     float vxuit, vyuit, Ap, Ad, Ad1, kd, kp, ki, Ai, Ai1, Bp, Bd, Bd1, Bi, Bi1;
     float for_A, for_B;
@@ -57,74 +57,125 @@
     float ctrlA;
     float ctrlB;
     //Startwaarden
-    x1tr=0;    y1tr=0;    z1tr=0;    z2tr=0;    yabs1tr=0;    yabs2tr=0;
-    x1br=0;    y1br=0;    z1br=0;    z2br=0;    yabs1br=0;    yabs2br=0;
-    x1tl=0;    y1tl=0;    z1tl=0;    z2tl=0;    yabs1tl=0;    yabs2tl=0;
-    x1bl=0;    y1bl=0;    z1bl=0;    z2bl=0;    yabs1bl=0;    yabs2bl=0;
-    zx=0;      zy=0;      xdir=0;    ydir=0;    xuit=0;       yuit=0;
-    kp=0.1;    ki=0.0000;   kd=0.0000;   rt=0.032805; Ai1=0; Ad1=0;
-    Bi1=0;     Bd1=0;     
+    x1tr=0;
+    y1tr=0;
+    z1tr=0;
+    z2tr=0;
+    yabs1tr=0;
+    yabs2tr=0;
+    x1br=0;
+    y1br=0;
+    z1br=0;
+    z2br=0;
+    yabs1br=0;
+    yabs2br=0;
+    x1tl=0;
+    y1tl=0;
+    z1tl=0;
+    z2tl=0;
+    yabs1tl=0;
+    yabs2tl=0;
+    x1bl=0;
+    y1bl=0;
+    z1bl=0;
+    z2bl=0;
+    yabs1bl=0;
+    yabs2bl=0;
+    zx=0;
+    zy=0;
+    xdir=0;
+    ydir=0;
+    xuit=0;
+    yuit=0;
+    kp=1.0*0.1545;
+    kd=1.0*0.0*2.8*pow(10.0,-3.0);
+    ki=0.0*1.0;
+    rt=0.032805;
+    Ai1=0;
+    Ad1=0;
+    Bi1=0;
+    Bd1=0;
 
-    //High pass, 35Hz, 1e orde
-    numh1=0.900575535279376;
-    numh2=-0.900575535279376;
+    //High pass, 35Hz, 1e orde 4 ms
+    numh1=0.680011076547878;
+    numh2=-0.680011076547878;
     //denh1=1;
-    denh2=-0.801151070558751;
-    
-    //Low pass, 5 Hz, 2e orde.
-    numl1=0.241359049041961*pow(10.0,-3.0);
-    numl2=0.482718098083923*pow(10.0,-3.0);
-    numl3=0.241359049041961*pow(10.0,-3.0);
+    denh2=-0.360022153095757;
+
+    //Low pass, 5 Hz, 2e orde. 4 ms
+    numl1=0.003621681514929;
+    numl2=0.007243363029857;
+    numl3=0.003621681514929;
     //denl1=1;
-    denl2=-1.955578240315036;
-    denl3=0.956543676511203;
-    //Low pass, 2 Hz, 2e orde.
+    denl2=-1.822694925196308;
+    denl3=0.837181651256023;
+    //Low pass, 2 Hz, 2e orde. 1 ms
     //numl1=0.391302053991682*pow(10.0,-4.0);
     //numl2=0.782604107983365*pow(10.0,-4.0);
     //numl3=0.391302053991682*pow(10.0,-4.0);
     //denl1=1;
     //denl2=-1.982228929792529;
     //denl3=0.982385450614126;
-
+float t;
+t=0;
     pc.baud(115200);
-
     while(1) {
-        while(looptimerflag != true);{
+        while(looptimerflag != true);
+        {
         }
         looptimerflag = false;
-
 //EMG lezen.
-ktr=emgtr.read();
+        ktr=emgtr.read();
         xtr=(ktr-0.5)*2.0;
         ytr=xtr*numh1+x1tr*numh2-y1tr*denh2;
         yabstr=abs(ytr);
         ztr=yabstr*numl1+yabs1tr*numl2+yabs2tr*numl3-z1tr*denl2-z2tr*denl3;
-        x1tr=xtr;        y1tr=ytr;        z2tr=z1tr;        z1tr=ztr;        yabs2tr=yabs1tr;        yabs1tr=yabstr;
+        x1tr=xtr;
+        y1tr=ytr;
+        z2tr=z1tr;
+        z1tr=ztr;
+        yabs2tr=yabs1tr;
+        yabs1tr=yabstr;
 
-kbr=emgbr.read();
+        kbr=emgbr.read();
         xbr=(kbr-0.5)*2.0;
         ybr=xbr*numh1+x1br*numh2-y1br*denh2;
         yabsbr=abs(ybr);
         zbr=yabsbr*numl1+yabs1br*numl2+yabs2br*numl3-z1br*denl2-z2br*denl3;
-        x1br=xbr;        y1br=ybr;        z2br=z1br;        z1br=zbr;        yabs2br=yabs1br;        yabs1br=yabsbr;
+        x1br=xbr;
+        y1br=ybr;
+        z2br=z1br;
+        z1br=zbr;
+        yabs2br=yabs1br;
+        yabs1br=yabsbr;
 
-ktl=emgtl.read();
+        ktl=emgtl.read();
         xtl=(ktl-0.5)*2.0;
         ytl=xtl*numh1+x1tl*numh2-y1tl*denh2;
         yabstl=abs(ytl);
         ztl=yabstl*numl1+yabs1tl*numl2+yabs2tl*numl3-z1tl*denl2-z2tl*denl3;
-        x1tl=xtl;        y1tl=ytl;        z2tl=z1tl;        z1tl=ztl;        yabs2tl=yabs1tl;        yabs1tl=yabstl;
+        x1tl=xtl;
+        y1tl=ytl;
+        z2tl=z1tl;
+        z1tl=ztl;
+        yabs2tl=yabs1tl;
+        yabs1tl=yabstl;
 
-kbl=emgbl.read();
+        kbl=emgbl.read();
         xbl=(kbl-0.5)*2.0;
         ybl=xbl*numh1+x1bl*numh2-y1bl*denh2;
         yabsbl=abs(ybl);
         zbl=yabsbl*numl1+yabs1bl*numl2+yabs2bl*numl3-z1bl*denl2-z2bl*denl3;
-        x1bl=xbl;        y1bl=ybl;        z2bl=z1bl;        z1bl=zbl;        yabs2bl=yabs1bl;        yabs1bl=yabsbl;
+        x1bl=xbl;
+        y1bl=ybl;
+        z2bl=z1bl;
+        z1bl=zbl;
+        yabs2bl=yabs1bl;
+        yabs1bl=yabsbl;
 
         zx=(zbr*5.0);
         zy=(zbl*5.0);
-        
+
         //Grenzen.
         if (zx>1.0) {
             zx=0.99999;
@@ -132,77 +183,86 @@
         if (zy>1.0) {
             zy=0.99999;
         }
-        if (zx<0.3){
-        zx=0.3;
+        if (zx<0.3) {
+            zx=0.3;
         }
-        if (zy<0.3){
-        zy=0.3;
+        if (zy<0.3) {
+            zy=0.3;
         }
         zx=zx-0.3;
         zx=zx*(1.0/0.7);
         zy=zy-0.3;
         zy=zy*(1.0/0.7);
-        //Richting omdraaien met triceps. 
+        //Richting omdraaien met triceps.
         if ((ztr>(zbr+0.1)) && dirflagx == true) {
             dirflagx = false;
             xdir ^= 1;
             zx=0;
             dirtimeout.attach(tricheck,1.0);
         }
-          if ((ztl>(zbl+0.1)) && dirflagy == true) {
+        if ((ztl>(zbl+0.1)) && dirflagy == true) {
             dirflagy = false;
             ydir ^= 1;
             zy=0;
             dirtimeout.attach(tricheck,1.0);
         }
-        
+
         //Motoraansturing.
-        if (ydir==1)
-        {
-        zy=-1.0*zy;
+        if (ydir==1) {
+            zy=-1.0*zy;
         }
-        if (xdir==1)
-        {
-        zx=-1.0*zx;
+        if (xdir==1) {
+            zx=-1.0*zx;
         }
-        
-        ticka=motor1.getPosition(); tickb=motor2.getPosition();
+
+        ticka=motor1.getPosition();
+        tickb=motor2.getPosition();
         vxuit=zx*4.0*pow(10.0,-1.0); // 4cm/s
         vyuit=zy*4.0*pow(10.0,-1.0); // 4cm/s
         xuit += ts*vxuit;
         yuit += ts*vyuit;
-        
+
         refA=(4123.0*atan2(yuit,xuit)/(2.0*PI));
         refB=(4123.0*sqrt(xuit*xuit+yuit*yuit)/(2.0*PI*rt));
+        refA=500*sin(t)*tanh(0.1*t);
+        refB=500*sin(t)*tanh(0.1*t);
+        t +=ts;
         errA=refA-ticka;
-        errB=refB-tickb;     
+        errB=refB-tickb;
+
         
-if(errA<0){
-Adir=0;}
-else{
-Adir=1;
-}
-if(errB<0){
-Bdir=0;}
-else{
-Bdir=1;
-}
         //Controllers
-        Ap=errA*kp;          Ad=(errA-Ad1)*kd/ts;        Ai=(Ai1+ts*errA)*ki;
+        Ap=errA*kp;
+        Ad=(errA-Ad1)*kd/ts;
+        Ai=(Ai1+ts*errA)*ki;
         //keep_in_range(&Ad,-0.1,0.1);
         //keep_in_range(&Ai,-0.1,0.1);
-        Ad1=Ad;           Ai1=Ai;
+        Ad1=Ad;
+        Ai1=Ai;
         ctrlA=(Ai+Ap+Ad);
         for_A=(ctrlA)/10.0;
-        
-        Bp=errB*kp;          Bd=(errB-Bd1)*kd/ts;        Bi=(Bi1+ts*errB)*ki;
+
+        Bp=errB*kp;
+        Bd=(errB-Bd1)*kd/ts;
+        Bi=(Bi1+ts*errB)*ki;
         //keep_in_range(&Bd,-0.1,0.1);
         //keep_in_range(&Bi,-0.1,0.1);
-        Bd1=Bd;           Bi1=Bi;
+        Bd1=Bd;
+        Bi1=Bi;
         ctrlB=(Bi+Bp+Bd);
         for_B=(ctrlB)/10.0;
-        //x en y uit emg in meters. IK naar radialen. dan radialen naar ticks.  
-        
+        //x en y uit emg in meters. IK naar radialen. dan radialen naar ticks.
+
+        if(ctrlA<0) {
+            Adir=0;
+        } else {
+            Adir=1;
+        }
+        if(ctrlB<0) {
+            Bdir=0;
+        } else {
+            Bdir=1;
+        }
         keep_in_range(&for_A, -1,1);
         keep_in_range(&for_B, -1,1);
 
@@ -210,19 +270,20 @@
         motordirB.write(Bdir);
         pwm_A.write(abs(for_A));
         pwm_B.write(abs(for_B));
-        
+
         //pc.printf(" %f %f %f %f \n\r",kbl,zy,vyuit,yuit);
         //pc.printf(" %i %i %i\n\r",tickb,refB,errB);
         //pc.printf(" %f %f %f %f \n\r",kbl,zy,vyuit, yuit);
         //pc.printf("A %i %i %i %i %f %f\n\r",ticka,refA,errA,ctrlA,for_A);
-        pc.printf("B %i %i %i %i %i %f %f\n\r",Bdir,tickb,refB,errB,ctrlB,for_B);
-//Conclusies: tot en met yuit gaat het goed. Kabel A lijkt het beter te doen dan kabel B. Als de EMG niet aangesloten zit is er nog steeds ruis van de
-//encoder.
-
+        if(pc.txBufferGetSize(0)-pc.txBufferGetCount() > 100){
+            pc.printf("B %i %i %i\n\r",tickb,refB,errB);
+            //pc.printf(" %f %f %f %f \n\r",kbl,zy,vyuit,yuit);
+            //pc.printf("%i\n",motor2.getPosition());
+            }
     }
 }
 
 void keep_in_range(float * in, float min, float max)
 {
-    *in > min ? *in < max? : *in = max: *in = min;
+*in > min ? *in < max? : *in = max: *in = min;
 }
\ No newline at end of file