Diff: main.cpp

Revision:

7:ca880e05bb04

Parent:

6:2674fe30b610

Child:

8:f5065cd04213

--- a/main.cpp	Mon Oct 30 12:26:53 2017 +0000
+++ b/main.cpp	Tue Oct 31 13:12:24 2017 +0000
@@ -370,8 +370,8 @@
 double          Pby = 887;
 double          Prx = 768;
 double          Pry = 443;
-double          Pex=121;
-double          Pey=308;
+double          Pex=91;
+double          Pey=278;
 double          diamtrklosje=20;
 double          pi=3.14159265359;
 double          omtrekklosje=diamtrklosje*pi;
@@ -460,20 +460,21 @@
 double GetReferenceVelocity1()
 {
     // Returns reference velocity in rad/s. Positive value means clockwise rotation.
-    double maxVelocity1=fabs(Vex+Vey)/20*8.4; // max 8.4 in rad/s of course!       
-    referenceVelocity1 = speedfactor1 * maxVelocity1;  
+    double maxVelocity1=fabs(Vex+Vey); // max 8.4 in rad/s of course!       
+    referenceVelocity1 = (-1)*speedfactor1 * maxVelocity1;  
                 
     if (Encoder1.getPulses() < (dcounto+10))
-        {   speedfactor1 = -1;
+        {   speedfactor1 = 0.05;
+            pc.printf("Zit je hier?");
             if (Encoder1.getPulses() > (dcounto-10))
-                { printf("kleiner");
+                { pc.printf("kleiner");
                 speedfactor1=0;
                 }
         }
         else if ((Encoder1.getPulses()) > (dcounto-10))
-        {   speedfactor1 = 1;
+        {   speedfactor1 = -0.05;
              if (Encoder1.getPulses() < (dcounto+10))
-                { printf("groter");
+                { pc.printf("groter");
                 speedfactor1=0;
                 }
         }
@@ -488,18 +489,18 @@
 double GetReferenceVelocity2()
 {
     // Returns reference velocity in rad/s. Positive value means clockwise rotation.
-    double maxVelocity2=fabs(Vex*25+Vey*25); // max 8.4 in rad/s of course!       
+    double maxVelocity2=fabs(Vex+Vey); // max 8.4 in rad/s of course!       
     referenceVelocity2 = (-1)*speedfactor2 * maxVelocity2;  
                 
     if (Encoder2.getPulses() < (dcountb+10))
-        {   speedfactor2 = -0.01;
+        {   speedfactor2 = 0.05;
             if (Encoder2.getPulses() > (dcountb-10))
                 { //printf("kleiner22222222222");
                 speedfactor2=0;
                 }
         }
         else if (Encoder2.getPulses() > (dcountb-10))
-        {   speedfactor2 = 0.01;
+        {   speedfactor2 = -0.05;
              if (Encoder2.getPulses() < (dcountb+10))
                 { //printf("groter");
                 speedfactor2=0;
@@ -516,18 +517,20 @@
 double GetReferenceVelocity3()
 {
     // Returns reference velocity in rad/s. Positive value means clockwise rotation.
-    double maxVelocity3=fabs(Vex*15+Vey*15); // max 8.4 in rad/s of course!       
+    double maxVelocity3=fabs(Vex+Vey); // max 8.4 in rad/s of course!       
     referenceVelocity3 = (-1)*speedfactor3 * maxVelocity3;  
+    
+    int Counts3 = Encoder3.getPulses();
                 
     if (Encoder3.getPulses() < (dcountr+10))
-        {   speedfactor3 = -0.01;
+        {   speedfactor3 = 0.05;
             if (Encoder3.getPulses() > (dcountr-10))
-                { //printf("kleiner22222222222");
+                { printf("Encoder waarde %i\n\r", Counts3);
                 speedfactor3=0;
                 }
         }
         else if (Encoder3.getPulses() > (dcountr-10))
-        {   speedfactor3 = 0.01;
+        {   speedfactor3 = -0.05;
              if (Encoder3.getPulses() < (dcountr+10))
                 { //printf("groter");
                 speedfactor3=0;
@@ -668,6 +671,8 @@
     rotr=Lru/omtrekklosje; 
     counto=roto*4200; 
     dcounto=counto-hcounto;
+    pc.printf("counto = %f \n\r", counto);
+    pc.printf("hcounto = %f \n\r", hcounto);
     pc.printf("dcounto = %f \n\r",dcounto);
     countb=rotb*4200;
     dcountb=countb-hcountb;
@@ -699,8 +704,8 @@
 //Calculating desired end position based on the EMG input                       <- Waarom maar voor een paal?
 double Ps(){
     if (Move_done==true);
-    Psx=(Xin_new)*30+121;
-    Psy=(Yin_new)*30+308;     
+    Psx=(Xin_new)*30+91;
+    Psy=(Yin_new)*30+278;     
     pc.printf("x=%.2f \n\r y=%.2f \n\r",Psx,Psy);
     return 0;
 }
@@ -748,14 +753,39 @@
     }
     }
 }
+
+void tiller(){
+    dcountb = -8148;
+    dcounto = -12487;
+    dcountr = -7386;
+    pc.printf("Tiller");
+    Vex = 20;
+    Vey = 20;
+    controlmotor1.attach(&MeasureAndControl1, 0.01);
+    calculatedelta1.attach(&calcdelta1, 0.01);
+    printdata1.attach(&readdata1, 0.5); 
+    controlmotor2.attach(&MeasureAndControl2, 0.01);
+    //calculatedelta2.attach(&calcdelta2, 0.01);
+    //printdata2.attach(&readdata2, 0.5); 
+    controlmotor3.attach(&MeasureAndControl3, 0.01);
+    //calculatedelta3.attach(&calcdelta3, 0.01);
+    //printdata3.attach(&readdata3, 0.5);
+    }
+    
+
 void setcurrentposition(){ 
+
     hcounto=4200*((sqrt(pow((Pex-Pox),2)+pow((Pey-Poy),2)))/omtrekklosje);
     hcountb=4200*((sqrt(pow((Pex-Pbx),2)+pow((Pey-Pby),2)))/omtrekklosje);
     hcountr=4200*((sqrt(pow((Pex-Prx),2)+pow((Pey-Pry),2)))/omtrekklosje);
     }
+    
 int main()
 {
+    
+    
     pc.baud(115200);
+    tiller();
     getbqChain();
     threshold_timerR.attach(&Threshold_samplingBR, 0.002);
     threshold_timerL.attach(&Threshold_samplingBL, 0.002);
@@ -770,10 +800,10 @@
         controlmotor1.attach(&MeasureAndControl1, 0.01);
         calculatedelta1.attach(&calcdelta1, 0.01);
         printdata1.attach(&readdata1, 0.5); 
-        //controlmotor2.attach(&MeasureAndControl2, 0.01);
+        controlmotor2.attach(&MeasureAndControl2, 0.01);
         //calculatedelta2.attach(&calcdelta2, 0.01);
         //printdata2.attach(&readdata2, 0.5); 
-        //controlmotor3.attach(&MeasureAndControl3, 0.01);
+        controlmotor3.attach(&MeasureAndControl3, 0.01);
         //calculatedelta3.attach(&calcdelta3, 0.01);
         //printdata3.attach(&readdata3, 0.5);
         //zakker();