Revision 33:c495e9d8ea1f, committed 2013-11-04

Comitter:

Socrates

Date:

Mon Nov 04 15:27:44 2013 +0000

Parent:

32:5ae627e1bce8

Child:

34:961aec0a13c6

Commit message:

Opschonen. Werkt zo goed als.

Changed in this revision

--- a/main.cpp	Mon Nov 04 09:27:22 2013 +0000
+++ b/main.cpp	Mon Nov 04 15:27:44 2013 +0000
@@ -14,6 +14,9 @@
 AnalogIn emgbl(PTB0);
 PwmOut pwm_A(PTA12);
 PwmOut pwm_B(PTA5);
+PwmOut redled(LED_RED);
+PwmOut greenled(LED_GREEN);
+PwmOut blueled(LED_BLUE);
 MODSERIAL pc(USBTX,USBRX);
 DigitalOut motordirA(PTD3);
 DigitalOut motordirB(PTD1);
@@ -102,7 +105,7 @@
     kic=0.1*1.0;
     kp=1*0.1545;
     kd=1.0*2.8*pow(10.0,-3.0);
-    ki=1.0*1.0;
+    ki=0.1*1.0;
     rt=0.032805;
     gain=4.0;
     emggrens=0.35;
@@ -144,7 +147,7 @@
 
     if(startflag==true);
     {
-        motor1.setPosition(0);
+        motor1.setPosition(200);
         motor2.setPosition(1200);
         startflag=false;
     }
@@ -177,15 +180,14 @@
             }
             pwm_B.write(abs(for_B));
             motordirB.write(Bdir);
-            //pc.printf("B %f %i \n\r",for_B,errB);
-            if(errB<10) {
+            if(errB<40) {
                 calB=false;
                 pwm_B.write(0.0);
             }
         }
 
         while(calA==true) {
-            ticka=-1*motor1.getPosition();  //NOTE: deze moet volgens mij gewoon weer positief zijn.
+            ticka=-1*motor1.getPosition();
             errA=refA-ticka;
             Ap=errA*kpc;
             Ad=(errA-Ad1)*kdc/ts;
@@ -194,7 +196,7 @@
             Ai1=Ai;
             ctrlA=(Ai+Ap+Ad);
             for_A=(ctrlA)/1000.0;
-            if(ctrlA<0) { //NOTE: als ticka weer +get is, kan deze weer 0 else 1 zijn.
+            if(ctrlA<0) {
                 Adir=1;
             } else {
                 Adir=0;
@@ -202,11 +204,10 @@
             keep_in_range(&for_A, -1,1);
             if (frictionflag==true) {
                 for_A=abs(for_A)+friction;
-                keep_in_range(&for_A, 0,0.08);
+                keep_in_range(&for_A, 0,0.1);
             }
             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);
@@ -218,6 +219,7 @@
 
 //Loop.
     wait(1);
+    blueled.write(1.0);
     while(meetflag==true) {
         while(looptimerflag != true);
         {
@@ -273,7 +275,7 @@
         yabs1bl=yabsbl;
 
 //Gains om filter te compenseren.
-        zx=(zbr*gain);
+        zx=((zbr)*gain);
         zy=(zbl*gain);
 
 //Grenzen voor emg.
@@ -298,37 +300,41 @@
         if (ztr>0.15 && dirflagx == true) {
             dirflagx = false;
             xdir ^= 1;
-            //zx=0;  //NOTE: deze weghalen kan schelen?
             dirtimeout.attach(tricheck,1.0);
         }
         if (ztl>0.15 && dirflagy == true) {
             dirflagy = false;
             ydir ^= 1;
-            //zy=0;
             dirtimeout.attach(tricheck,1.0);
         }
 
         //Motoraansturing.
         if (ydir==0) {
             zy=-1.0*zy;
+            redled.write(1);
         }
-        if (xdir==0) { //NOTE: moet dit geen 0 zijn?
+        if (ydir==1){
+        redled.write(0);
+        }
+        if (xdir==0) {
             zx=-1.0*zx;
+            greenled.write(1);
+        }
+        if (xdir==1) {
+        greenled.write(0);
         }
 
-        ticka=-1*motor1.getPosition(); //NOTE:  -1 weghalen?
+        ticka=-1*motor1.getPosition();
         tickb=motor2.getPosition();
 
         //Begrenzing.
-        vxuit=zx*20.0/500.0; //NOTE: deze shizzle kan er misschien uit. In ieder geval de vier. En anders moet het /1000 zijn.
-        vyuit=zy*20.0/500.0; 
+        vxuit=zx*70.0/500.0;
+        vyuit=zy*50.0/500.0; 
         xuit += ts*vxuit;
         yuit += ts*vyuit;
 
-        //keep_in_range(&xuit,-4.5,2.75); //NOTE: Iets met de grenzen. En de richtingpins.
-        //keep_in_range(&yuit,-4.5,2.8); //NOTE: als bovenstaande weer klopt moeten de grenzen voor x zijn: .125 en 0.422. Voor y is dat 0.125 en 0.335.
-        keep_in_range(&xuit,0.125,0.422);
-        keep_in_range(&yuit,0.125,0.335);
+        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);
@@ -342,12 +348,10 @@
         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; //NOTE: maak eens een plot van ctrlA en ctrlB. Kan leerzaam zijn voor deze gains.
+        for_A=(ctrlA)/10.0; //NOTE: maak eens een plot van ctrlA en ctrlB. Kan leerzaam zijn voor deze gains.
 
         Bp=errB*kp;
         Bd=(errB-Bd1)*kd/ts;
@@ -357,7 +361,7 @@
         Bd1=Bd;
         Bi1=Bi;
         ctrlB=(Bi+Bp+Bd);
-        for_B=(ctrlB)/7000.0;
+        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.
@@ -378,8 +382,8 @@
             for_A=abs(for_A)+friction;
             for_B=abs(for_B)+friction;
 
-            keep_in_range(&for_A, friction,1.0); //NOTE: misschien valt hier nog wat te begrenzen, maar dan wel op ondergrens friction.
-            keep_in_range(&for_B, friction,1.0);
+            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_B, friction,friction+0.2);
         }
         motordirA.write(Adir);
         motordirB.write(Bdir);
@@ -388,10 +392,10 @@
 
         if(pc.txBufferGetSize(0)-pc.txBufferGetCount() > 100) {
             //pc.printf(" %f %f \n\r",zx, zy);
-            //pc.printf(" %f %f %f %f %f \n\r",zbl,ztl,zbr,ztr,kbl);
+            pc.printf(" %f %f %f %f\n\r",zbl,ztl,zbr,ztr);
             //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*1000.0,yuit*1000.0);
-            //pc.printf("A %i %i B %i %i\n\r",Adir, ticka,Bdir, tickb);
+            //pc.printf(" %i %i %i %i %f %f \n\r", ticka,tickb,refA,refB,xuit*1000.0,yuit*1000.0);
+            //pc.printf("A %f B %f\n\r",ctrlA,ctrlB);
             //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);
@@ -457,6 +461,7 @@
             motordirB.write(Bdir);
             pwm_A.write(abs(for_A));
             pwm_B.write(abs(for_B));
+        
         }
         wait(1);
         xuit=400.0;