Diff: main.cpp

Revision:

35:db3385976119

Parent:

34:961aec0a13c6

Child:

36:3fad1225c3ad

--- a/main.cpp	Mon Nov 04 16:26:02 2013 +0000
+++ b/main.cpp	Tue Nov 05 08:56:16 2013 +0000
@@ -2,10 +2,7 @@
 #include "MODSERIAL.h"
 #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.
-//Rechts is x, links is y
+//Rechterarm is x, linkerarm is y.
 
 //Inputs.
 AnalogIn emgtr(PTB3);
@@ -45,8 +42,7 @@
 volatile bool frictionflag=true;
 
 volatile bool calflag=true;
-volatile bool meetflag=false;
-volatile bool patroonflag=true;
+volatile bool meetflag=true;
 
 int main()
 {
@@ -65,12 +61,11 @@
     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, kdc, kpc, kic;
-    double gain, emggrens,friction;
+    double gain, emggrens,frictiona,frictionb, schrijfgainx, schrijfgainy;
     int xdir, ydir, Adir, Bdir;
     int ticka, tickb, refA,refB, errA, errB;
     int Aboven, Aonder, Bboven,Bonder;
-
-    int ppos=1;
+    int Astart, Bstart;
 //Startwaarden.
     x1tr=0;
     y1tr=0;
@@ -111,7 +106,12 @@
     rt=0.032805;
     gain=4.0;
     emggrens=0.35;
-    friction=0.45;
+    frictiona=0.7;
+    frictionb=0.5;
+    schrijfgainx=0.1;
+    schrijfgainy=0.05;
+    Astart=000;
+    Bstart=1200;
     Ai1=0;
     Ad1=0;
     Bi1=0;
@@ -137,17 +137,20 @@
     denl2=-1.822694925196308;
     denl3=0.837181651256023;
 
-//Opzetje voor calibratie
+//Calibratie
     wait(4);
 
     if(startflag==true);
     {
-        motor1.setPosition(200);
-        motor2.setPosition(1100);
+
         startflag=false;
     }
 
     while(calflag==true) {
+        wait(0.1);
+        motor1.setPosition(Astart);
+        wait(0.1);
+        motor2.setPosition(Bstart);
         while(looptimerflag != true);
         looptimerflag = false;
         //515 - 3536 voor rechtsonder.
@@ -170,7 +173,7 @@
             }
             keep_in_range(&for_B, -1.0,1.0);
             if (frictionflag==true) {
-                for_B=abs(for_B)+friction;
+                for_B=abs(for_B)+frictionb;
                 keep_in_range(&for_B, 0.0,0.1);
             }
             pwm_B.write(abs(for_B));
@@ -198,7 +201,7 @@
             }
             keep_in_range(&for_A, -1,1);
             if (frictionflag==true) {
-                for_A=abs(for_A)+friction;
+                for_A=abs(for_A)+frictiona;
                 keep_in_range(&for_A, 0,0.1);
             }
             pwm_A.write(abs(for_A));
@@ -210,7 +213,6 @@
             }
         }
     }
-//Einde opzetje.
 
 //Loop.
     wait(1);
@@ -322,15 +324,16 @@
         ticka=-1*motor1.getPosition();
         tickb=motor2.getPosition();
 
-        //Begrenzing.
-        vxuit=zx*70.0/500.0;
-        vyuit=zy*50.0/500.0;
+        //Omzetten naar schrijfsnelheid.
+        vxuit=zx*schrijfgainx;
+        vyuit=zy*schrijfgainy;
         xuit += ts*vxuit;
         yuit += ts*vyuit;
 
         keep_in_range(&xuit,0.115,0.422);
         keep_in_range(&yuit,0.115,0.335);
 
+
         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);
@@ -371,12 +374,12 @@
         keep_in_range(&for_B, -1.0,1.0);
 
         if (frictionflag==true) {
-            for_A=abs(for_A)+friction;
-            for_B=abs(for_B)+friction;
+            for_A=abs(for_A)+frictiona;
+            for_B=abs(for_B)+frictionb;
+            keep_in_range(&for_A, frictiona,1);
+            keep_in_range(&for_B, frictionb,1);
+        }
 
-            keep_in_range(&for_A, friction,friction+0.2);
-            keep_in_range(&for_B, friction,friction+0.2);
-        }
         motordirA.write(Adir);
         motordirB.write(Bdir);
         pwm_A.write(abs(for_A));
@@ -397,91 +400,8 @@
             //pc.printf("%f %f\n\r",for_A,for_B);
         }
     }
-
-    while(patroonflag==true) {
-        while(looptimerflag != true);
-        looptimerflag = false;
-
-        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);
-        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)/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;
-        }
-        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;