Revision 34:961aec0a13c6, committed 2013-11-04

Comitter:

Socrates

Date:

Mon Nov 04 16:26:02 2013 +0000

Parent:

33:c495e9d8ea1f

Child:

35:db3385976119

Commit message:

Voor patroontekenen weg;

Changed in this revision

--- a/main.cpp	Mon Nov 04 15:27:44 2013 +0000
+++ b/main.cpp	Mon Nov 04 16:26:02 2013 +0000
@@ -45,8 +45,8 @@
 volatile bool frictionflag=true;
 
 volatile bool calflag=true;
-volatile bool meetflag=true;
-volatile bool patroonflag=false;
+volatile bool meetflag=false;
+volatile bool patroonflag=true;
 
 int main()
 {
@@ -69,6 +69,8 @@
     int xdir, ydir, Adir, Bdir;
     int ticka, tickb, refA,refB, errA, errB;
     int Aboven, Aonder, Bboven,Bonder;
+
+    int ppos=1;
 //Startwaarden.
     x1tr=0;
     y1tr=0;
@@ -134,13 +136,6 @@
     //denl1=1;
     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;
 
 //Opzetje voor calibratie
     wait(4);
@@ -148,7 +143,7 @@
     if(startflag==true);
     {
         motor1.setPosition(200);
-        motor2.setPosition(1200);
+        motor2.setPosition(1100);
         startflag=false;
     }
 
@@ -275,7 +270,7 @@
         yabs1bl=yabsbl;
 
 //Gains om filter te compenseren.
-        zx=((zbr)*gain);
+        zx=(zbr*gain);
         zy=(zbl*gain);
 
 //Grenzen voor emg.
@@ -313,15 +308,15 @@
             zy=-1.0*zy;
             redled.write(1);
         }
-        if (ydir==1){
-        redled.write(0);
+        if (ydir==1) {
+            redled.write(0);
         }
         if (xdir==0) {
             zx=-1.0*zx;
             greenled.write(1);
         }
         if (xdir==1) {
-        greenled.write(0);
+            greenled.write(0);
         }
 
         ticka=-1*motor1.getPosition();
@@ -329,7 +324,7 @@
 
         //Begrenzing.
         vxuit=zx*70.0/500.0;
-        vyuit=zy*50.0/500.0; 
+        vyuit=zy*50.0/500.0;
         xuit += ts*vxuit;
         yuit += ts*vyuit;
 
@@ -351,20 +346,17 @@
         Ad1=Ad;
         Ai1=Ai;
         ctrlA=(Ai+Ap+Ad);
-        for_A=(ctrlA)/10.0; //NOTE: maak eens een plot van ctrlA en ctrlB. Kan leerzaam zijn voor deze gains.
+        for_A=(ctrlA)/10.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)/1500.0;
-        //x en y uit emg in meters. IK naar radialen. dan radialen naar ticks.
 
-        if(ctrlA<0) { //NOTE: zeker gezien de vorige omdraaiingen moet deze volgens mij weer 0 else 1 zijn.
+        if(ctrlA<0) {
             Adir=1;
         } else {
             Adir=0;
@@ -382,7 +374,7 @@
             for_A=abs(for_A)+friction;
             for_B=abs(for_B)+friction;
 
-            keep_in_range(&for_A, friction,friction+0.2); //NOTE: misschien valt hier nog wat te begrenzen, maar dan wel op ondergrens friction.
+            keep_in_range(&for_A, friction,friction+0.2);
             keep_in_range(&for_B, friction,friction+0.2);
         }
         motordirA.write(Adir);
@@ -409,114 +401,87 @@
     while(patroonflag==true) {
         while(looptimerflag != true);
         looptimerflag = false;
-        xuit=400.0;
-        yuit=140.0;
+
+        if(ppos==1) {
+            refA=221;
+            refB=8477;
+            //xuit=0.400;
+            //yuit=0.140;
+        }
+        if(ppos==2) {
+            refA=278;
+            refB=8774;
+            //xuit=0.400;
+            //yuit=0.180;
+        }
+        if(ppos==3) {
+            //xuit=0.400;
+            //yuit=0.200;
+        }
+        if(ppos==4) {
+            //xuit=0.340;
+            //yuit=0.200;
+        }
+        if(ppos==5) {
+            //xuit=0.340;
+            //yuit=0.180;
+        }
+        if(ppos==6) {
+            //xuit=0.350;
+            //yuit=0.200;
+        }
         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);
+        //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);
-            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));
-        
+        //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)/100.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)/100.0;
+        if(ctrlA<0) {
+            Adir=1;
+        } else {
+            Adir=0;
         }
-        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));
+        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.6);
         }
-    }
+        motordirA.write(Adir);
+        motordirB.write(Bdir);
+        pwm_A.write(abs(for_A));
+        pwm_B.write(abs(for_B));
+        if(errA<40 && errB<40) {
+            ppos +=1;
+            wait(1);
+        }
 
+    }
 }
 
+
 void keep_in_range(double * in, double min, double max)
 {
 *in > min ? *in < max? : *in = max: *in = min;