Revision 28:2904487e0a1e, committed 2013-11-01

Comitter:

Socrates

Date:

Fri Nov 01 13:01:08 2013 +0000

Parent:

27:5d0c94b991aa

Child:

29:8600b02ab223

Commit message:

Voor patroon. calibratie werkt. Aansturing nog niet echt.

Changed in this revision

--- a/main.cpp	Fri Nov 01 09:16:46 2013 +0000
+++ b/main.cpp	Fri Nov 01 13:01:08 2013 +0000
@@ -20,9 +20,6 @@
 Encoder motor1(PTD0,PTC9);
 Encoder motor2(PTD5,PTC8);
 
-DigitalIn sluis1(PTE0);
-DigitalIn sluis2(PTE1);
-
 //Functies en flags.
 void keep_in_range(double * in, double min, double max);
 void keep_in_rangeint(int * in, int min, int max);
@@ -40,9 +37,11 @@
     dirflagy=true;
 }
 
-volatile bool calflag=false;
-volatile bool grensflag=false;
+volatile bool calflag=true;
 volatile bool frictionflag=true;
+volatile bool meetflag=true;
+volatile bool calA, calB;
+volatile bool patroonflag=false;
 
 int main()
 {
@@ -60,10 +59,9 @@
     double xbl,ybl,y1bl,x1bl,zbl,z1bl,z2bl,yabsbl,yabs1bl,yabs2bl,kbl;
     double zx,zy, xuit,yuit, rt;
     double vxuit, vyuit, Ap, Ad, Ad1, kd, kp, ki, Ai, Ai1, Bp, Bd, Bd1, Bi, Bi1;
-    double for_A, for_B, ctrlA, ctrlB;
-    double Asluis, Bsluis, calA, calApos, calB, calBpos;
-    double gain, grens,friction;
-    int calAdir, calBdir, xdir, ydir, Adir, Bdir;
+    double for_A, for_B, ctrlA, ctrlB, kdc, kpc, kic;
+    double gain, emggrens,friction;
+    int xdir, ydir, Adir, Bdir;
     int ticka, tickb, refA,refB, errA, errB;
     int Aboven, Aonder, Bboven,Bonder;
 //Startwaarden.
@@ -97,22 +95,25 @@
     ydir=0;
     xuit=0;
     yuit=0;
-    kp=1.0*0.1545;
-    kd=1.0*0.0*2.8*pow(10.0,-3.0);
+    kpc=1.0*0.1545;
+    kdc=0.0*2.8*pow(10.0,-3.0);
+    kic=0.1*1.0;
+    kp=0.1*0.1545;
+    kd=0.1*2.8*pow(10.0,-3.0);
     ki=0.1*1.0;
     rt=0.032805;
     gain=5.0;
-    grens=0.3;
-    friction=0.2;
+    emggrens=0.3;
+    friction=0.4;
     Ai1=0;
     Ad1=0;
     Bi1=0;
     Bd1=0;
     pc.baud(115200);
-    Aboven=500;
-    Aonder=-500;
-    Bboven=7000;
-    Bonder=-500;
+    Aboven=800;
+    Aonder=185;
+    Bboven=10800;
+    Bonder=3500;
 
 //Filtercoëfficienten.
     //High pass, 35Hz, 1e orde, 4 ms.
@@ -136,47 +137,83 @@
     //denl2=-1.982228929792529;
     //denl3=0.982385450614126;
 
-//Tijdstap voor sinusinput
-//double t;
-//t=0;
-
 //Opzetje voor calibratie
-    while(calflag==true) {
-
-        Asluis=sluis1.read();
-        Bsluis=sluis2.read();
+    motor1.setPosition(0);
+    motor2.setPosition(1200);
+    wait(6);
+    calA=true;
+    calB=true;
 
-        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);
+    while(calflag==true) {
+        while(looptimerflag != true);
+        looptimerflag = false;
+        //515,3536 voor rechtsonder.
+        refA=515;
+        refB=3536;
+        while(calB==true) {
+            tickb=motor2.getPosition();
+            errB=refB-tickb;
+            Bp=errB*kpc;
+            Bd=(errB-Bd1)*kdc/ts;
+            Bi=(Bi1+ts*errB)*kic;
+            Bd1=Bd;
+            Bi1=Bi;
+            ctrlB=(Bi+Bp+Bd);
+            for_B=(ctrlB)/1000.0;
+            if(ctrlB<0) { //Is nu omgedraaid. Was 0-1.
+                Bdir=0;
+            } else {
+                Bdir=1;
+            }
+            keep_in_range(&for_B, -1,1);
+            if (frictionflag==true) {
+                for_B=abs(for_B)+friction;
+                keep_in_range(&for_B, 0,0.1);
+            }
+            pwm_B.write(abs(for_B));
+            motordirB.write(Bdir);
+            //pc.printf("B %f %i \n\r",for_B,errB);
+            if(errB<10) {
+                calB=false;
+                pwm_B.write(0);
+            }
         }
-        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_B.write(calB);
+        while(calA==true) {
+            ticka=-motor1.getPosition();
+            errA=refA-ticka;
+            Ap=errA*kpc;
+            Ad=(errA-Ad1)*kdc/ts;
+            Ai=(Ai1+ts*errA)*kic;
+            Ad1=Ad;
+            Ai1=Ai;
+            ctrlA=(Ai+Ap+Ad);
+            for_A=(ctrlA)/1000.0;
+            if(ctrlA<0) {
+                Adir=1;
+            } else {
+                Adir=0;
+            }
+            keep_in_range(&for_A, -1,1);
+            if (frictionflag==true) {
+                for_A=abs(for_A)+friction;
+                keep_in_range(&for_A, 0,0.08);
+            }
+            pwm_A.write(abs(for_A));
+            motordirA.write(Adir);
+            //pc.printf("A %f %i \n\r",for_A,errA);
+            if(errA<20) {
+                calA=false;
+                pwm_A.write(0);
+                calflag=false;
+            }
         }
-        pwm_B.write(0);
-
-        motor1.setPosition(0);
-        motor2.setPosition(0);
-        calflag=false;
     }
 //Einde opzetje.
 
 //Loop.
-wait(2);
-    while(1) {
+    wait(2);
+    while(meetflag==true) {
         while(looptimerflag != true);
         {
         }
@@ -234,23 +271,23 @@
         zx=(zbr*gain);
         zy=(zbl*gain);
 
-//Grenzen.
+//Grenzen voor emg.
         if (zx>1.0) {
             zx=0.99999;
         }
         if (zy>1.0) {
             zy=0.99999;
         }
-        if (zx<grens) {
-            zx=grens;
+        if (zx<emggrens) {
+            zx=emggrens;
         }
-        if (zy<grens) {
-            zy=grens;
+        if (zy<emggrens) {
+            zy=emggrens;
         }
-        zx=zx-grens;
-        zx=zx*(1.0/(1.0-grens));
-        zy=zy-grens;
-        zy=zy*(1.0/(1.0-grens));
+        zx=zx-emggrens;
+        zx=zx*(1.0/(1.0-emggrens));
+        zy=zy-emggrens;
+        zy=zy*(1.0/(1.0-emggrens));
 
 //Richting omdraaien met triceps.
         if (ztr>0.1 && dirflagx == true) {
@@ -274,67 +311,23 @@
             zx=-1.0*zx;
         }
 
-        ticka=motor1.getPosition();
+        ticka=-1*motor1.getPosition();
         tickb=motor2.getPosition();
-        
+
         //Begrenzing.
-        if(grensflag==true) {
-            if(ticka>Aboven) {
-                //pwm_A.write(friction+0.4);
-                //motordirA.write(0);
-                zx=0;
-                zy=0;
-                //wait(0.1);
-                //motor1.setPosition(Aboven+50);
-            }
-            if(ticka<Aonder) {
-                //pwm_A.write(friction+0.4);
-                //motordirA.write(1);                
-                zx=0;
-                zy=0;
-                //wait(0.01);
-                //motor1.setPosition(Aonder-50);
-}
-            
-            if(tickb>Bboven) {
-                //pwm_B.write(friction+0.2);
-                //motordirB.write(0);
-                zx=0;
-                zy=0;
-                //wait(0.01);
-                //motor2.setPosition(Bboven+50);
-            }
-            if(tickb<Bonder) {
-                //pwm_B.write(friction+0.2);
-                //motordirB.write(1);
-                zx=0;
-                zy=0;
-                //wait(0.01);
-                //motor2.setPosition(Bonder-50);
-            }
-        }
-        
         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;
 
-keep_in_range(&xuit,
-
         refA=4123.0*atan2(yuit,xuit)/(2.0*PI);
         refB=4123.0*sqrt(xuit*xuit+yuit*yuit)/(2.0*PI*rt);
-        
+        //keep_in_rangeint(&refA,Aonder,Aboven);
+        //keep_in_rangeint(&refB,Bonder,Bboven);
 
-        keep_in_rangeint(&refA,Aonder,Aboven);
-        keep_in_rangeint(&refB,Bonder,Bboven);
-
-        //refA=500*sin(t)*tanh(0.1*t);
-        //refB=500*sin(t)*tanh(0.1*t);
-        //t +=ts;
         errA=refA-ticka;
         errB=refB-tickb;
 
-
         //Controllers
         Ap=errA*kp;
         Ad=(errA-Ad1)*kd/ts;
@@ -344,7 +337,7 @@
         Ad1=Ad;
         Ai1=Ai;
         ctrlA=(Ai+Ap+Ad);
-        for_A=(ctrlA)/1000.0;
+        for_A=(ctrlA)/700.0;
 
         Bp=errB*kp;
         Bd=(errB-Bd1)*kd/ts;
@@ -354,7 +347,7 @@
         Bd1=Bd;
         Bi1=Bi;
         ctrlB=(Bi+Bp+Bd);
-        for_B=(ctrlB)/1000.0;
+        for_B=(ctrlB)/7000.0;
         //x en y uit emg in meters. IK naar radialen. dan radialen naar ticks.
 
         if(ctrlA<0) {
@@ -368,17 +361,16 @@
             Bdir=1;
         }
 
-        keep_in_range(&for_A, -1,1);
-        keep_in_range(&for_B, -1,1);
- 
- if (frictionflag==true)
- {       
-for_A=abs(for_A)+friction+0.2;
-for_B=abs(for_B)+friction;
- 
-keep_in_range(&for_A, 0,1);
-keep_in_range(&for_B, 0,1);        
- }      
+        keep_in_range(&for_A, -1.0,1.0);
+        keep_in_range(&for_B, -1.0,1.0);
+
+        if (frictionflag==true) {
+            for_A=abs(for_A)+friction;
+            for_B=abs(for_B)+friction;
+
+            keep_in_range(&for_A, 0,0.5);
+            keep_in_range(&for_B, 0,0.5);
+        }
         motordirA.write(Adir);
         motordirB.write(Bdir);
         pwm_A.write(abs(for_A));
@@ -386,17 +378,86 @@
 
         if(pc.txBufferGetSize(0)-pc.txBufferGetCount() > 100) {
             //pc.printf(" %f\n\r",zy);
-            pc.printf(" %f %f %f %f %i %i\n\r",zx,zy, for_A,for_B,refA,refB);
+            pc.printf(" %f %f %i %i %i %i\n\r",for_A,for_B, refA,refB,errA,errB);
+            //pc.printf(" %i %i %i %i \n\r", ticka,tickb,refA,refB);
             //pc.printf("A %i %i B %i %i\n\r",Adir, ticka,Bdir, tickb);
             //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("tr %f br %f tl %f bl %f\n\r",ztr,zbr,ztl,zbl); 
+            //pc.printf("tr %f br %f tl %f bl %f\n\r",ztr,zbr,ztl,zbl);
             //pc.printf("%i\n",motor2.getPosition());
             //pc.printf("%f %f\n\r",for_A,for_B);
         }
     }
+
+    while(patroonflag==true) {
+
+        zx=400;
+        zy=140;
+        while(1) {
+            ticka=-1*motor1.getPosition();
+            tickb=motor2.getPosition();
+
+            refA=4123.0*atan2(yuit,xuit)/(2.0*PI);
+            refB=4123.0*sqrt(xuit*xuit+yuit*yuit)/(2.0*PI*rt);
+            //keep_in_rangeint(&refA,Aonder,Aboven);
+            //keep_in_rangeint(&refB,Bonder,Bboven);
+
+            errA=refA-ticka;
+            errB=refB-tickb;
+
+            //Controllers
+            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;
+            ctrlA=(Ai+Ap+Ad);
+            for_A=(ctrlA)/700.0;
+
+            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;
+            ctrlB=(Bi+Bp+Bd);
+            for_B=(ctrlB)/7000.0;
+            //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.0,1.0);
+            keep_in_range(&for_B, -1.0,1.0);
+
+            if (frictionflag==true) {
+                for_A=abs(for_A)+friction;
+                for_B=abs(for_B)+friction;
+
+                keep_in_range(&for_A, 0,0.5);
+                keep_in_range(&for_B, 0,0.5);
+            }
+            motordirA.write(Adir);
+            motordirB.write(Bdir);
+            pwm_A.write(abs(for_A));
+            pwm_B.write(abs(for_B));
+
+        }
+    }
+
 }
 
 void keep_in_range(double * in, double min, double max)