Revision 29:8600b02ab223, committed 2013-11-01

Comitter:

Socrates

Date:

Fri Nov 01 15:18:14 2013 +0000

Parent:

28:2904487e0a1e

Child:

30:c569058f10aa

Commit message:

Calibratie: werkt, op ??n punt.; Meten: gaat best ok, richtingpins moeten nog beter.; Patroon tekenen: werkt voor geen bal.

Changed in this revision

--- a/main.cpp	Fri Nov 01 13:01:08 2013 +0000
+++ b/main.cpp	Fri Nov 01 15:18:14 2013 +0000
@@ -42,6 +42,7 @@
 volatile bool meetflag=true;
 volatile bool calA, calB;
 volatile bool patroonflag=false;
+volatile bool startflag=true;
 
 int main()
 {
@@ -102,18 +103,18 @@
     kd=0.1*2.8*pow(10.0,-3.0);
     ki=0.1*1.0;
     rt=0.032805;
-    gain=5.0;
-    emggrens=0.3;
+    gain=4.0;
+    emggrens=0.35;
     friction=0.4;
     Ai1=0;
     Ad1=0;
     Bi1=0;
     Bd1=0;
     pc.baud(115200);
-    Aboven=800;
-    Aonder=185;
-    Bboven=10800;
-    Bonder=3500;
+    Aboven=820;
+    Aonder=165;
+    Bboven=10900;
+    Bonder=3400;
 
 //Filtercoëfficienten.
     //High pass, 35Hz, 1e orde, 4 ms.
@@ -138,13 +139,19 @@
     //denl3=0.982385450614126;
 
 //Opzetje voor calibratie
-    motor1.setPosition(0);
-    motor2.setPosition(1200);
-    wait(6);
+    wait(3);
     calA=true;
     calB=true;
 
+if(startflag==true);
+{
+    motor1.setPosition(0);
+    motor2.setPosition(1200);
+    startflag=false;
+    }
+    
     while(calflag==true) {
+    
         while(looptimerflag != true);
         looptimerflag = false;
         //515,3536 voor rechtsonder.
@@ -160,7 +167,7 @@
             Bi1=Bi;
             ctrlB=(Bi+Bp+Bd);
             for_B=(ctrlB)/1000.0;
-            if(ctrlB<0) { //Is nu omgedraaid. Was 0-1.
+            if(ctrlB<0) {
                 Bdir=0;
             } else {
                 Bdir=1;
@@ -212,7 +219,7 @@
 //Einde opzetje.
 
 //Loop.
-    wait(2);
+    wait(1);
     while(meetflag==true) {
         while(looptimerflag != true);
         {
@@ -290,13 +297,13 @@
         zy=zy*(1.0/(1.0-emggrens));
 
 //Richting omdraaien met triceps.
-        if (ztr>0.1 && dirflagx == true) {
+        if (ztr>0.15 && dirflagx == true) {
             dirflagx = false;
             xdir ^= 1;
             zx=0;
             dirtimeout.attach(tricheck,1.0);
         }
-        if (ztl>0.1 && dirflagy == true) {
+        if (ztl>0.15 && dirflagy == true) {
             dirflagy = false;
             ydir ^= 1;
             zy=0;
@@ -304,7 +311,7 @@
         }
 
         //Motoraansturing.
-        if (ydir==1) {
+        if (ydir==0) {
             zy=-1.0*zy;
         }
         if (xdir==1) {
@@ -316,14 +323,17 @@
 
         //Begrenzing.
         vxuit=zx*4.0*pow(10.0,-1.0); // 4cm/s
-        vyuit=zy*4.0*pow(10.0,-1.0); // 4cm/s
+        vyuit=zy*4.0*pow(10.0,-2.0); // 4cm/s
         xuit += ts*vxuit;
         yuit += ts*vyuit;
 
+keep_in_range(&xuit,-4.5,2.75); //Iets met de grenzen. En de richtingpins. 
+keep_in_range(&yuit,-4.5,2.8);
+
         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);
 
         errA=refA-ticka;
         errB=refB-tickb;
@@ -351,9 +361,9 @@
         //x en y uit emg in meters. IK naar radialen. dan radialen naar ticks.
 
         if(ctrlA<0) {
-            Adir=0;
+            Adir=1;
         } else {
-            Adir=1;
+            Adir=0;
         }
         if(ctrlB<0) {
             Bdir=0;
@@ -368,8 +378,8 @@
             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);
+            keep_in_range(&for_A, 0,1.0);
+            keep_in_range(&for_B, 0,1.0);
         }
         motordirA.write(Adir);
         motordirB.write(Bdir);
@@ -377,9 +387,9 @@
         pwm_B.write(abs(for_B));
 
         if(pc.txBufferGetSize(0)-pc.txBufferGetCount() > 100) {
-            //pc.printf(" %f\n\r",zy);
-            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(" %f %f \n\r",zx, zy);
+            //pc.printf(" %f %i %i %f %i %i\n\r",for_A,ticka,refA,for_B,tickb, refB);
+            pc.printf(" %i %i %i %i %f %f \n\r", ticka,tickb,refA,refB,xuit,yuit);
             //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);
@@ -392,43 +402,38 @@
     }
 
     while(patroonflag==true) {
-
-        zx=400;
-        zy=140;
-        while(1) {
+        while(looptimerflag != true);
+        looptimerflag = false;
+        xuit=400.0;
+        yuit=140.0;
+        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*1000.0);
+        errA=refA-ticka;
+        errB=refB-tickb;
+        while(errA>20 && errB>20) {
             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);
+            Ap=errA*kpc;
+            Ad=(errA-Ad1)*kdc/ts;
+            Ai=(Ai1+ts*errA)*kic;
             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);
+            for_A=(ctrlA)/1000.0;
+            Bp=errB*kpc;
+            Bd=(errB-Bd1)*kdc/ts;
+            Bi=(Bi1+ts*errB)*kic;
             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.
-
+            for_B=(ctrlB)/1000.0;
             if(ctrlA<0) {
                 Adir=0;
             } else {
@@ -439,22 +444,68 @@
             } 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);
+                keep_in_range(&for_A, 0,0.08);
+                keep_in_range(&for_B, 0,0.1);
             }
             motordirA.write(Adir);
             motordirB.write(Bdir);
             pwm_A.write(abs(for_A));
             pwm_B.write(abs(for_B));
-
+        }
+        wait(1);
+        xuit=400.0;
+        yuit=180.0;
+        while(errA>20 && errB>20) {
+            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*1000.0);
+            //keep_in_rangeint(&refA,Aonder,Aboven);
+            //keep_in_rangeint(&refB,Bonder,Bboven);
+            errA=refA-ticka;
+            errB=refB-tickb;
+            //Controllers
+            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;
+            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(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.08);
+                keep_in_range(&for_B, 0,0.1);
+            }
+            motordirA.write(Adir);
+            motordirB.write(Bdir);
+            pwm_A.write(abs(for_A));
+            pwm_B.write(abs(for_B));
         }
     }