Revision 26:539f131cf07c, committed 2013-10-31

Comitter:

Socrates

Date:

Thu Oct 31 09:51:47 2013 +0000

Parent:

25:bfe7c49e76cd

Child:

27:5d0c94b991aa

Commit message:

voor gerard en lichtpoorten

Changed in this revision

--- a/main.cpp	Wed Oct 30 16:06:26 2013 +0000
+++ b/main.cpp	Thu Oct 31 09:51:47 2013 +0000
@@ -6,6 +6,8 @@
 //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
+
+//Inputs.
 AnalogIn emgtr(PTB3);
 AnalogIn emgbr(PTB2);
 AnalogIn emgtl(PTB1);
@@ -18,6 +20,10 @@
 Encoder motor1(PTD0,PTC9);
 Encoder motor2(PTD5,PTC8);
 
+DigitalIn sluis1(PTE0);
+DigitalIn sluis2(PTE1);
+
+//Functies en flags.
 void keep_in_range(float * in, float min, float max);
 
 volatile bool looptimerflag;
@@ -25,20 +31,21 @@
 {
     looptimerflag = true;
 }
-
 volatile bool dirflagx=true;
 volatile bool dirflagy=true;
-
 void tricheck(void)
 {
     dirflagx=true;
     dirflagy=true;
 }
 
+volatile bool calflag=false;
+volatile bool grensflag=false;
 int main()
 {
-    pwm_A.period(1.0/1000.0);
-    pwm_B.period(1.0/1000.0);
+//Constantes en tickers.
+    pwm_A.period(1.0/2500.0);
+    pwm_B.period(1.0/2500.0);
     Ticker looptimer;
     Timeout dirtimeout;
     const float ts=0.004;
@@ -48,15 +55,15 @@
     float xbr,ybr,y1br,x1br,zbr,z1br,z2br,yabsbr,yabs1br,yabs2br,kbr;
     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;
-    int xdir, ydir, Adir, Bdir;
+    float zx,zy, xuit,yuit, rt;
     float vxuit, vyuit, Ap, Ad, Ad1, kd, kp, ki, Ai, Ai1, Bp, Bd, Bd1, Bi, Bi1;
-    float for_A, for_B;
+    float for_A, for_B, ctrlA, ctrlB;
+    float Asluis, Bsluis, calA, calApos, calB, calBpos;
+    float gain, grens;
+    int calAdir, calBdir, xdir, ydir, Adir, Bdir;
     int ticka, tickb, refA,refB, errA, errB;
-    float ctrlA;
-    float ctrlB;
-    //Startwaarden
+
+//Startwaarden.
     x1tr=0;
     y1tr=0;
     z1tr=0;
@@ -89,38 +96,79 @@
     yuit=0;
     kp=1.0*0.1545;
     kd=1.0*0.0*2.8*pow(10.0,-3.0);
-    ki=0.0*1.0;
+    ki=0.1*1.0;
     rt=0.032805;
+    gain=5.0;
+    grens=0.3;
     Ai1=0;
     Ad1=0;
     Bi1=0;
     Bd1=0;
+    pc.baud(115200);
 
-    //High pass, 35Hz, 1e orde 4 ms
+//Filtercoëfficienten.
+    //High pass, 35Hz, 1e orde, 4 ms.
     numh1=0.680011076547878;
     numh2=-0.680011076547878;
     //denh1=1;
     denh2=-0.360022153095757;
 
-    //Low pass, 5 Hz, 2e orde. 4 ms
+    //Low pass, 5 Hz, 2e orde, 4 ms.
     numl1=0.003621681514929;
     numl2=0.007243363029857;
     numl3=0.003621681514929;
     //denl1=1;
     denl2=-1.822694925196308;
     denl3=0.837181651256023;
-    //Low pass, 2 Hz, 2e orde. 1 ms
+    //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);
+ 
+//Tijdstap voor sinusinput   
+//float t;
+//t=0;
+
+//Opzetje voor calibratie
+while(calflag==true){
+
+Asluis=sluis1.read();
+Bsluis=sluis2.read();
+
+while(Asluis>0.5) {
+calA=0.3;
+calAdir=1;
+calApos=motor1.getPosition();
+while(abs(calApos<1500)){} //Ter voorkoming van schade.
+Asluis=sluis1.read();
+motordirA.write(calAdir);
+pwm_A.write(calA);
+}
+pwm_A.write(0);
+
+while(Bsluis>0.5) {
+calB=0.3;
+calBdir=1;
+calBpos=motor2.getPosition();
+while(abs(calBpos>2000)){}
+Bsluis=sluis2.read();
+motordirB.write(calBdir);
+pwm_A.write(calB);
+}
+pwm_B.write(0);
+
+motor1.setPosition(0);
+motor2.setPosition(0);
+calflag=false;
+}
+//Einde opzetje.
+
+//Loop.
     while(1) {
-        while(looptimerflag != true);
+        while(looptimerflag != true); 
         {
         }
         looptimerflag = false;
@@ -173,27 +221,29 @@
         yabs2bl=yabs1bl;
         yabs1bl=yabsbl;
 
-        zx=(zbr*5.0);
-        zy=(zbl*5.0);
+//Gains om filter te compenseren.
+        zx=(zbr*gain);
+        zy=(zbl*gain);
 
-        //Grenzen.
+//Grenzen.
         if (zx>1.0) {
             zx=0.99999;
         }
         if (zy>1.0) {
             zy=0.99999;
         }
-        if (zx<0.3) {
-            zx=0.3;
+        if (zx<grens) {
+            zx=grens;
         }
-        if (zy<0.3) {
-            zy=0.3;
+        if (zy<grens) {
+            zy=grens;
         }
-        zx=zx-0.3;
-        zx=zx*(1.0/0.7);
-        zy=zy-0.3;
-        zy=zy*(1.0/0.7);
-        //Richting omdraaien met triceps.
+        zx=zx-grens;
+        zx=zx*(1.0/(1.0-grens));
+        zy=zy-grens;
+        zy=zy*(1.0/(1.0-grens));
+
+//Richting omdraaien met triceps.
         if ((ztr>(zbr+0.1)) && dirflagx == true) {
             dirflagx = false;
             xdir ^= 1;
@@ -222,11 +272,11 @@
         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;
+        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;
 
@@ -240,7 +290,7 @@
         Ad1=Ad;
         Ai1=Ai;
         ctrlA=(Ai+Ap+Ad);
-        for_A=(ctrlA)/10.0;
+        for_A=(ctrlA)/1000.0;
 
         Bp=errB*kp;
         Bd=(errB-Bd1)*kd/ts;
@@ -250,35 +300,79 @@
         Bd1=Bd;
         Bi1=Bi;
         ctrlB=(Bi+Bp+Bd);
-        for_B=(ctrlB)/10.0;
+        for_B=(ctrlB)/1000.0;
         //x en y uit emg in meters. IK naar radialen. dan radialen naar ticks.
 
         if(ctrlA<0) {
             Adir=0;
+            for_A=for_A-0.25;
         } else {
             Adir=1;
         }
         if(ctrlB<0) {
             Bdir=0;
+            for_B=for_B-0.25;
         } else {
-            Bdir=1;
+            Bdir=1;     
+            for_B=for_B-0.25;
         }
+
+if(0<for_A<0.25)
+{
+for_A=for_A+0.25;
+}
+if(-0.25<for_A<-0.001);
+{
+for_A=for_A-0.25;
+}
+
+if(0<for_A<0.25)
+{
+for_A=for_A+0.25;
+}
+if(-0.25<for_A<-0.001);
+{
+for_A=for_A-0.25;
+}
         keep_in_range(&for_A, -1,1);
         keep_in_range(&for_B, -1,1);
 
+//Begrenzing.
+if(grensflag==true)
+{
+if(ticka>1500){
+for_A=0.2;
+Adir =1;
+}
+if(ticka<0){
+for_A=0.2;
+Adir =0;
+}
+if(tickb>10000){
+for_B=0.2;
+Bdir =1;
+}
+if(tickb<0){
+for_B=0.2;
+Bdir =0;
+}
+}
         motordirA.write(Adir);
         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);
         if(pc.txBufferGetSize(0)-pc.txBufferGetCount() > 100){
-            pc.printf("B %i %i %i\n\r",tickb,refB,errB);
+            //pc.printf(" %f\n\r",zy);
+            //pc.printf(" %f %f %f %f \n\r",kbl,zy,vyuit,yuit);
+            //pc.printf(" %i %i %i %f\n\r",tickb,refB,errB,for_B);
+            //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\n\r",tickb,refB,errB);
             //pc.printf(" %f %f %f %f \n\r",kbl,zy,vyuit,yuit);
             //pc.printf("%i\n",motor2.getPosition());
+            //pc.printf("%f\n\r",for_A);
+            pc.printf("%f\n\r",for_B);
             }
     }
 }