Revision 27:5d0c94b991aa, committed 2013-11-01

Comitter:

Socrates

Date:

Fri Nov 01 09:16:46 2013 +0000

Parent:

26:539f131cf07c

Child:

28:2904487e0a1e

Commit message:

Voor begrenzing op vrijdag.

Changed in this revision

--- a/main.cpp	Thu Oct 31 09:51:47 2013 +0000
+++ b/main.cpp	Fri Nov 01 09:16:46 2013 +0000
@@ -24,7 +24,8 @@
 DigitalIn sluis2(PTE1);
 
 //Functies en flags.
-void keep_in_range(float * in, float min, float max);
+void keep_in_range(double * in, double min, double max);
+void keep_in_rangeint(int * in, int min, int max);
 
 volatile bool looptimerflag;
 void setlooptimerflag(void)
@@ -41,6 +42,8 @@
 
 volatile bool calflag=false;
 volatile bool grensflag=false;
+volatile bool frictionflag=true;
+
 int main()
 {
 //Constantes en tickers.
@@ -48,21 +51,21 @@
     pwm_B.period(1.0/2500.0);
     Ticker looptimer;
     Timeout dirtimeout;
-    const float ts=0.004;
+    const double ts=0.004;
     looptimer.attach(setlooptimerflag,ts);
-    float numh1,numh2,denh2,numl1,numl2,numl3,denl2,denl3;
-    float xtr,ytr,y1tr,x1tr,ztr,z1tr,z2tr,yabstr,yabs1tr,yabs2tr,ktr;
-    float xbr,ybr,y1br,x1br,zbr,z1br,z2br,yabsbr,yabs1br,yabs2br,kbr;
-    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, xuit,yuit, rt;
-    float vxuit, vyuit, Ap, Ad, Ad1, kd, kp, ki, Ai, Ai1, Bp, Bd, Bd1, Bi, Bi1;
-    float for_A, for_B, ctrlA, ctrlB;
-    float Asluis, Bsluis, calA, calApos, calB, calBpos;
-    float gain, grens;
+    double numh1,numh2,denh2,numl1,numl2,numl3,denl2,denl3;
+    double xtr,ytr,y1tr,x1tr,ztr,z1tr,z2tr,yabstr,yabs1tr,yabs2tr,ktr;
+    double xbr,ybr,y1br,x1br,zbr,z1br,z2br,yabsbr,yabs1br,yabs2br,kbr;
+    double xtl,ytl,y1tl,x1tl,ztl,z1tl,z2tl,yabstl,yabs1tl,yabs2tl,ktl;
+    double xbl,ybl,y1bl,x1bl,zbl,z1bl,z2bl,yabsbl,yabs1bl,yabs2bl,kbl;
+    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;
+    double Asluis, Bsluis, calA, calApos, calB, calBpos;
+    double gain, grens,friction;
     int calAdir, calBdir, xdir, ydir, Adir, Bdir;
     int ticka, tickb, refA,refB, errA, errB;
-
+    int Aboven, Aonder, Bboven,Bonder;
 //Startwaarden.
     x1tr=0;
     y1tr=0;
@@ -100,11 +103,16 @@
     rt=0.032805;
     gain=5.0;
     grens=0.3;
+    friction=0.2;
     Ai1=0;
     Ad1=0;
     Bi1=0;
     Bd1=0;
     pc.baud(115200);
+    Aboven=500;
+    Aonder=-500;
+    Bboven=7000;
+    Bonder=-500;
 
 //Filtercoëfficienten.
     //High pass, 35Hz, 1e orde, 4 ms.
@@ -127,48 +135,49 @@
     //denl1=1;
     //denl2=-1.982228929792529;
     //denl3=0.982385450614126;
- 
-//Tijdstap voor sinusinput   
-//float t;
+
+//Tijdstap voor sinusinput
+//double t;
 //t=0;
 
 //Opzetje voor calibratie
-while(calflag==true){
+    while(calflag==true) {
 
-Asluis=sluis1.read();
-Bsluis=sluis2.read();
+        Asluis=sluis1.read();
+        Bsluis=sluis2.read();
 
-while(Asluis>0.5) {
-calA=0.3;
-calAdir=1;
-calApos=motor1.getPosition();
-while(abs(calApos<1500)){} //Ter voorkoming van schade.
-Asluis=sluis1.read();
-motordirA.write(calAdir);
-pwm_A.write(calA);
-}
-pwm_A.write(0);
+        while(Asluis>0.5) {
+            calA=0.3;
+            calAdir=1;
+            calApos=motor1.getPosition();
+            while(abs(calApos<1500)) {} //Ter voorkoming van schade.
+            Asluis=sluis1.read();
+            motordirA.write(calAdir);
+            pwm_A.write(calA);
+        }
+        pwm_A.write(0);
 
-while(Bsluis>0.5) {
-calB=0.3;
-calBdir=1;
-calBpos=motor2.getPosition();
-while(abs(calBpos>2000)){}
-Bsluis=sluis2.read();
-motordirB.write(calBdir);
-pwm_A.write(calB);
-}
-pwm_B.write(0);
+        while(Bsluis>0.5) {
+            calB=0.3;
+            calBdir=1;
+            calBpos=motor2.getPosition();
+            while(abs(calBpos>2000)) {}
+            Bsluis=sluis2.read();
+            motordirB.write(calBdir);
+            pwm_B.write(calB);
+        }
+        pwm_B.write(0);
 
-motor1.setPosition(0);
-motor2.setPosition(0);
-calflag=false;
-}
+        motor1.setPosition(0);
+        motor2.setPosition(0);
+        calflag=false;
+    }
 //Einde opzetje.
 
 //Loop.
+wait(2);
     while(1) {
-        while(looptimerflag != true); 
+        while(looptimerflag != true);
         {
         }
         looptimerflag = false;
@@ -244,13 +253,13 @@
         zy=zy*(1.0/(1.0-grens));
 
 //Richting omdraaien met triceps.
-        if ((ztr>(zbr+0.1)) && dirflagx == true) {
+        if (ztr>0.1 && dirflagx == true) {
             dirflagx = false;
             xdir ^= 1;
             zx=0;
             dirtimeout.attach(tricheck,1.0);
         }
-        if ((ztl>(zbl+0.1)) && dirflagy == true) {
+        if (ztl>0.1 && dirflagy == true) {
             dirflagy = false;
             ydir ^= 1;
             zy=0;
@@ -267,20 +276,65 @@
 
         ticka=motor1.getPosition();
         tickb=motor2.getPosition();
+        
+        //Begrenzing.
+        if(grensflag==true) {
+            if(ticka>Aboven) {
+                //pwm_A.write(friction+0.4);
+                //motordirA.write(0);
+                zx=0;
+                zy=0;
+                //wait(0.1);
+                //motor1.setPosition(Aboven+50);
+            }
+            if(ticka<Aonder) {
+                //pwm_A.write(friction+0.4);
+                //motordirA.write(1);                
+                zx=0;
+                zy=0;
+                //wait(0.01);
+                //motor1.setPosition(Aonder-50);
+}
+            
+            if(tickb>Bboven) {
+                //pwm_B.write(friction+0.2);
+                //motordirB.write(0);
+                zx=0;
+                zy=0;
+                //wait(0.01);
+                //motor2.setPosition(Bboven+50);
+            }
+            if(tickb<Bonder) {
+                //pwm_B.write(friction+0.2);
+                //motordirB.write(1);
+                zx=0;
+                zy=0;
+                //wait(0.01);
+                //motor2.setPosition(Bonder-50);
+            }
+        }
+        
         vxuit=zx*4.0*pow(10.0,-1.0); // 4cm/s
         vyuit=zy*4.0*pow(10.0,-1.0); // 4cm/s
         xuit += ts*vxuit;
         yuit += ts*vyuit;
 
+keep_in_range(&xuit,
+
         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);
+
         //refA=500*sin(t)*tanh(0.1*t);
         //refB=500*sin(t)*tanh(0.1*t);
         //t +=ts;
         errA=refA-ticka;
         errB=refB-tickb;
 
-        
+
         //Controllers
         Ap=errA*kp;
         Ad=(errA-Ad1)*kd/ts;
@@ -305,79 +359,51 @@
 
         if(ctrlA<0) {
             Adir=0;
-            for_A=for_A-0.25;
         } else {
             Adir=1;
         }
         if(ctrlB<0) {
             Bdir=0;
-            for_B=for_B-0.25;
         } else {
-            Bdir=1;     
-            for_B=for_B-0.25;
+            Bdir=1;
         }
 
-if(0<for_A<0.25)
-{
-for_A=for_A+0.25;
-}
-if(-0.25<for_A<-0.001);
-{
-for_A=for_A-0.25;
-}
-
-if(0<for_A<0.25)
-{
-for_A=for_A+0.25;
-}
-if(-0.25<for_A<-0.001);
-{
-for_A=for_A-0.25;
-}
         keep_in_range(&for_A, -1,1);
         keep_in_range(&for_B, -1,1);
-
-//Begrenzing.
-if(grensflag==true)
-{
-if(ticka>1500){
-for_A=0.2;
-Adir =1;
-}
-if(ticka<0){
-for_A=0.2;
-Adir =0;
-}
-if(tickb>10000){
-for_B=0.2;
-Bdir =1;
-}
-if(tickb<0){
-for_B=0.2;
-Bdir =0;
-}
-}
+ 
+ if (frictionflag==true)
+ {       
+for_A=abs(for_A)+friction+0.2;
+for_B=abs(for_B)+friction;
+ 
+keep_in_range(&for_A, 0,1);
+keep_in_range(&for_B, 0,1);        
+ }      
         motordirA.write(Adir);
         motordirB.write(Bdir);
         pwm_A.write(abs(for_A));
         pwm_B.write(abs(for_B));
 
-        if(pc.txBufferGetSize(0)-pc.txBufferGetCount() > 100){
+        if(pc.txBufferGetSize(0)-pc.txBufferGetCount() > 100) {
             //pc.printf(" %f\n\r",zy);
+            pc.printf(" %f %f %f %f %i %i\n\r",zx,zy, for_A,for_B,refA,refB);
+            //pc.printf("A %i %i B %i %i\n\r",Adir, ticka,Bdir, tickb);
             //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);
             //pc.printf("A %i %i %i %i %f %f\n\r",ticka,refA,errA,ctrlA,for_A);
-            //pc.printf("B %i %i %i\n\r",tickb,refB,errB);
-            //pc.printf(" %f %f %f %f \n\r",kbl,zy,vyuit,yuit);
+            //pc.printf("tr %f br %f tl %f bl %f\n\r",ztr,zbr,ztl,zbl); 
             //pc.printf("%i\n",motor2.getPosition());
-            //pc.printf("%f\n\r",for_A);
-            pc.printf("%f\n\r",for_B);
-            }
+            //pc.printf("%f %f\n\r",for_A,for_B);
+        }
     }
 }
 
-void keep_in_range(float * in, float min, float max)
+void keep_in_range(double * in, double min, double max)
+{
+*in > min ? *in < max? : *in = max: *in = min;
+}
+void keep_in_rangeint(int * in, int min, int max)
 {
 *in > min ? *in < max? : *in = max: *in = min;
 }
\ No newline at end of file