Diff: main.cpp

Revision:

37:c6d753123173

Parent:

36:3fad1225c3ad

Child:

38:fb5c45d85ead

--- a/main.cpp	Wed Nov 06 10:48:02 2013 +0000
+++ b/main.cpp	Thu Nov 07 12:23:00 2013 +0000
@@ -37,16 +37,15 @@
     dirflagy=true;
 }
 
-volatile bool startflag=true;
-volatile bool calA=true, calB=true;
-volatile bool frictionflag=true;
+volatile bool calA=true, calB=true; //Calibratie per motor.
+volatile bool frictionflag=true; //Wrijvingscompensatie.
 
-volatile bool calflag=true;
-volatile bool meetflag=true;
+volatile bool calflag=true; //Calibreren?
+volatile bool meetflag=true; //Meten?
 
 int main()
 {
-//Constantes en tickers.
+//Constanten en tickers.
     pwm_A.period(1.0/2500.0);
     pwm_B.period(1.0/2500.0);
     Ticker looptimer;
@@ -62,10 +61,8 @@
     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,frictiona,frictionb, schrijfgainx, schrijfgainy;
-    int xdir, ydir, Adir, Bdir;
-    int ticka, tickb, refA,refB, errA, errB;
-    int Aboven, Aonder, Bboven,Bonder;
-    int Astart, Bstart;
+    int xdir, ydir, Adir, Bdir, ticka, tickb, refA,refB, errA, errB;
+    int Aboven, Aonder, Bboven,Bonder, Astart, Bstart;
 //Startwaarden.
     x1tr=0;
     y1tr=0;
@@ -103,15 +100,15 @@
     kp=1*0.1545;
     kd=1.0*2.8*pow(10.0,-3.0);
     ki=0.05*1.0;
-    rt=0.032805;
-    gain=4.0;
+    rt=0.032805; //Straal tandwiel.
+    gain=4.0; //Tegen filterverlies.
     emggrens=0.35;
     frictiona=0.65;
     frictionb=0.5;
     schrijfgainx=0.1;
     schrijfgainy=0.05;
-    Astart=000;
-    Bstart=1200;
+    Astart=0;
+    Bstart=1200; //Beginpositie pen in ticks.
     Ai1=0;
     Ad1=0;
     Bi1=0;
@@ -120,7 +117,7 @@
     Aboven=820;
     Aonder=165;
     Bboven=10900;
-    Bonder=3400;
+    Bonder=3400; //Grenzen in ticks. Komen overeen met hoeken van A4.
 
 //Filtercoëfficienten.
     //High pass, 35Hz, 1e orde, 4 ms.
@@ -139,23 +136,13 @@
 
 //Calibratie
     wait(4);
-
-    if(startflag==true);
-    {
-
-        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.
         refA=515;
-        refB=3536;
+        refB=3536; //515 - 3536 voor rechtsonder.
         while(calB==true) {
             tickb=motor2.getPosition();
             errB=refB-tickb;
@@ -174,18 +161,19 @@
             keep_in_range(&for_B, -1.0,1.0);
             if (frictionflag==true) {
                 for_B=abs(for_B)+frictionb;
-                keep_in_range(&for_B, 0.0,0.1);
+                keep_in_range(&for_B, 0.0,0.1); 
+                //Vreemd dat een bovengrens van 0.1 werkt ook al is friction groter dan 0.1. Bij hogere waarden gaat de motor te snel.
             }
             pwm_B.write(abs(for_B));
             motordirB.write(Bdir);
-            if(errB<90) {
+            if(errB<20) {
                 calB=false;
                 pwm_B.write(0.0);
             }
         }
 
         while(calA==true) {
-            ticka=-1*motor1.getPosition();
+            ticka=-1*motor1.getPosition(); //Omdat de motor op zijn kop staat.
             errA=refA-ticka;
             Ap=errA*kpc;
             Ad=(errA-Ad1)*kdc/ts;
@@ -214,17 +202,16 @@
         }
     }
 
-//Loop.
+//Meetloop.
     wait(1);
-    blueled.write(1.0);
     //motor1.setPosition(515);
     //motor2.setPosition(3565);
+    //Bovenstaande toegevoegd voor het geval de calibratie handmatig gedaan wordt.
     while(meetflag==true) {
         while(looptimerflag != true);
-        {
-        }
         looptimerflag = false;
 //EMG lezen.
+        //tr = triceps rechts.
         ktr=emgtr.read();
         xtr=(ktr-0.5)*2.0;
         ytr=xtr*numh1+x1tr*numh2-y1tr*denh2;
@@ -277,7 +264,7 @@
         zx=(zbr*gain);
         zy=(zbl*gain);
 
-//Grenzen voor emg.
+//Grenzen en threshold voor emg.
         if (zx>1.0) {
             zx=0.99999;
         }
@@ -290,10 +277,11 @@
         if (zy<emggrens) {
             zy=emggrens;
         }
+        //Schaling om ook outputs lager dan de grenswaarde te krijgen.
         zx=zx-emggrens;
-        zx=zx*(1.0/(1.0-emggrens));
+        zx=zx/(1.0-emggrens);
         zy=zy-emggrens;
-        zy=zy*(1.0/(1.0-emggrens));
+        zy=zy/(1.0-emggrens);
 
 //Richting omdraaien met triceps.
         if (ztr>0.15 && dirflagx == true) {
@@ -307,7 +295,7 @@
             dirtimeout.attach(tricheck,1.0);
         }
 
-        //Motoraansturing.
+        //Bepalen juiste teken van verplaatsing.
         if (ydir==0) {
             zy=-1.0*zy;
             redled.write(1);
@@ -323,28 +311,28 @@
             greenled.write(0);
         }
 
-        ticka=-1*motor1.getPosition();
+        ticka=-1*motor1.getPosition(); //Zie calibratie.
         tickb=motor2.getPosition();
 
         //Omzetten naar schrijfsnelheid.
         vxuit=zx*schrijfgainx;
         vyuit=zy*schrijfgainy;
         xuit += ts*vxuit;
-        yuit += ts*vyuit;
+        yuit += ts*vyuit; //Integreren naar positie.
 
+        //Begrenzing penpositie, getallen in meter.
         keep_in_range(&xuit,0.115,0.422);
         keep_in_range(&yuit,0.115,0.335);
 
-
+        //Inversie kinematica en begrenzing motorhoeken.
         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);
 
+        //Controllers
         errA=refA-ticka;
         errB=refB-tickb;
-
-        //Controllers
         Ap=errA*kp;
         Ad=(errA-Ad1)*kd/ts;
         Ai=(Ai1+ts*errA)*ki;
@@ -361,6 +349,7 @@
         ctrlB=(Bi+Bp+Bd);
         for_B=(ctrlB)/1500.0;
 
+        //Motorrichting bepalen.
         if(ctrlA<0) {
             Adir=1;
         } else {
@@ -372,9 +361,11 @@
             Bdir=1;
         }
 
+        //PWM-begrenzing.
         keep_in_range(&for_A, -1.0,1.0);
         keep_in_range(&for_B, -1.0,1.0);
 
+        //Wrijvingscompensatie.
         if (frictionflag==true) {
             for_A=abs(for_A)+frictiona;
             for_B=abs(for_B)+frictionb;
@@ -387,6 +378,8 @@
         pwm_A.write(abs(for_A));
         pwm_B.write(abs(for_B));
 
+
+            //Verscheidene prints die nuttig zijn gebeleken.
         if(pc.txBufferGetSize(0)-pc.txBufferGetCount() > 100) {
             //pc.printf(" %f %f \n\r",zx, zy);
             pc.printf(" %f %f %f %f\n\r",zbl,ztl,zbr,ztr);