Diff: main.cpp

Revision:

19:09c4b5249cec

Parent:

18:6c0200364678

Child:

20:013e9c00e058

--- a/main.cpp	Fri Oct 25 12:36:02 2013 +0000
+++ b/main.cpp	Mon Oct 28 16:09:13 2013 +0000
@@ -1,5 +1,11 @@
 #include "mbed.h"
 #include "MODSERIAL.h"
+#include "encoder.h"
+#define PI 3.14159265358979323
+
+//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. 
+
 //XenY
 AnalogIn emgtr(PTB3);
 AnalogIn emgbr(PTB2);
@@ -11,7 +17,8 @@
 MODSERIAL pc(USBTX,USBRX);
 DigitalOut motordirA(PTD3);
 DigitalOut motordirB(PTD1);
-
+Encoder motor1(PTD0,PTC9);
+Encoder motor2(PTD5,PTC8);
 void keep_in_range(float * in, float min, float max);
 
 
@@ -44,17 +51,19 @@
     float xtl,ytl,y1tl,x1tl,ztl,z1tl,z2tl,yabstl,yabs1tl,yabs2tl,ktl;
     float xbl,ybl,y1bl,x1bl,zbl,z1bl,z2bl,yabsbl,yabs1bl,yabs2bl,kbl;
     float zx,zy;
-    float gainb, xuit, kp, ki, zx1, yuit, zy1, pwmA, pwmB,rt;
+    float gain, xuit, kp, ki, zx1, yuit, zy1, pwmA, pwmB,rt;
     int xdir;
     int ydir;
+    float vxuit, vyuit, Ap, Ad, Ad1, kd, kp, ki, Ai, Ai1, Bp, Bd, Bd1, Bi, Bi1;
+    float for_A, for_B;
+    int ticka, tickb, refA,refB, errA, errB,
 
     x1tr=0;    y1tr=0;    z1tr=0;    z2tr=0;    yabs1tr=0;    yabs2tr=0;
     x1br=0;    y1br=0;    z1br=0;    z2br=0;    yabs1br=0;    yabs2br=0;
     x1tl=0;    y1tl=0;    z1tl=0;    z2tl=0;    yabs1tl=0;    yabs2tl=0;
     x1bl=0;    y1bl=0;    z1bl=0;    z2bl=0;    yabs1bl=0;    yabs2bl=0;
-    zx=0; zy=0;
-    xdir=0; ydir=0;
-    gainb=3.0; kp=0; ki=1; zx1=0; zy1=0; rt=33.0/1000.0;
+    zx=0;      zy=0;      xdir=0;    ydir=0;
+    kp=0;      ki=1;      zx1=0;     zy1=0;     rt=32.805/1000.0;
 
 
     //High pass, 35Hz, 1e
@@ -81,6 +90,7 @@
         }
         looptimerflag = false;
 
+//EMG lezen.
 ktr=emgtr.read();
         xtr=(ktr-0.5)*2.0;
         ytr=xtr*numh1+x1tr*numh2-y1tr*denh2;
@@ -108,17 +118,17 @@
         yabsbl=abs(ybl);
         zbl=yabsbl*numl1+yabs1bl*numl2+yabs2bl*numl3-z1bl*denl2-z2bl*denl3;
         x1bl=xbl;        y1bl=ybl;        z2bl=z1bl;        z1bl=zbl;        yabs2bl=yabs1bl;        yabs1bl=yabsbl;
+
+        zx=(zbr);
+        zy=(zbl);
         
-        zx=(zbr*gainb);
-        zy=(zbl*gainb);
-        
+        //Grenzen.
         if (zx>1.0) {
             zx=0.99999;
         }
         if (zy>1.0) {
             zy=0.99999;
         }
-        
         if (zx<0.30){
         zx=0;
         }
@@ -126,36 +136,55 @@
         zy=0;
         }
         
+        //Richting omdraaien met triceps. 
         if ((ztr>(zbr+0.1)) && dirflagx == true) {
             dirflagx = false;
             xdir ^= 1;
-            zx=0;
             dirtimeout.attach(tricheck,1.5);
         }
           if ((ztl>(zbl+0.1)) && dirflagy == true) {
             dirflagy = false;
             ydir ^= 1;
-            zy=0;
             dirtimeout.attach(tricheck,1.5);
         }
-        xuit=(kp*zx+ki*zx1)/(kp+ki);
-        yuit=(kp*zy+ki*zy1)/(kp+ki);
-        //pwmA=(atan(yuit/(xuit+0.0001)))/(2.0*3.14159265359);
-        //pwmB=(sqrt(xuit*xuit+yuit*yuit))/(2.0*3.14159265359*rt);
-        //keep_in_range(&pwmA, -1,1);
-        //keep_in_range(&pwmB, -1,1);
+        
+        //Motoraansturing.
+        if (ydir==1)
+        {
+        zy=-1.0*zy;
+        }
+        if (xdir==1)
+        {
+        zx=-1.0*zx;
+        }
+        
+        ticka=motor1.getPosition(); tickb=motor2.getPosition();
+        vxuit=zx*4.0*pow(10.0,-5.0); // 4cm/s
+        vyuit=zy*4.0*pow(10.0,-5.0); // 4cm/s
+        xuit += ts*vxuit;
+        yuiy += ts*vyuit;
+        
+        refA=4123.0*atan2(yuit,xuit)/(2.0*PI);
+        refB=4123.0*sqrt(xuit*xuit+yuit*yuit)*(2.0*PI*rt);//nog keer omtrek
+        errA=refA-ticka;
+        errB=refB-tickb;
 
-        zx1=zx;
-        zy1=zy;
+        //Controllers
+        Ap=errA*kp;          Ad=(errA-Ad1)*kd/ts;        Ai=(Ai1+ts*errA)*ki;
+        Ad1=Ad;           Ai1=Ai;
+        for_a=Ai+Ap+Ad;
+        Bp=errB*kp;          Bd=(errB-Bd1)*kd/ts;        Bi=(Bi1+ts*errB)*ki;
+        Bd1=Bd;           Bi1=Bi;
+        for_B=Bi+Bp+Bd;
+        //x en y uit emg in meters. IK naar radialen. dan radialen naar ticks.  
         
-        if (pwmA<0.0002)
-        {
-        pwmA=0;
-        }
+        keep_in_range(&for_A, -1,1);
+        keep_in_range(&for_B, -1,1);
+
         motordirA.write(xdir);
         motordirB.write(ydir);
-        pwm_A.write((yuit*3.0));
-        pwm_B.write((yuit*3.0));
+        pwm_A.write(for_A);
+        pwm_B.write(for_B);
         
         //pc.printf("Bi: %f, Tri: %f, Richting: %d \n\r",zbl*3.0,ztl*3.0,ydir);
         pc.printf(" %f\n\r",zy);