Diff: main.cpp

Revision:

13:3a17bdfbbba1

Parent:

12:2249fcee09e6

Child:

14:df5822cd0198

--- a/main.cpp	Thu Nov 02 19:56:49 2017 +0000
+++ b/main.cpp	Thu Nov 02 20:32:29 2017 +0000
@@ -536,7 +536,7 @@
 }
  
 // Function which makes it possible to lower the end-effector to pick up a piece
-void zakker(){                                 // hierdoor wacht dit programma totdat de beweging klaar is 
+void zakker(){                                  
     dLod=sqrt(pow(Lou,2)+pow((277.85),2))-Lou;    //dit is wat je motoren moeten doen om te zakken
     dLbd=sqrt(pow(Lbu,2)+pow((277.85),2))-Lbu;    // dat laatste getal moet nog aangepast worden
     dLrd=sqrt(pow(Lru,2)+pow((277.85),2))-Lru;
@@ -558,15 +558,24 @@
 
 
 void tiller(){
-    int reference_o = hcounto-12487;
-    int reference_b = hcountb-8148;
-    int reference_r = hcountr-7386;
+    int stijg_o = -12487;       //eerst hcounto minus 12487?
+    int stijg_b = -8148;
+    int stijg_r = -7386;
+    int lou_t = sqrt(pow((91),2)+pow((278),2));         // (Pex - Pox)^2 + (Pey - Poy)^2
+    int lou_b = sqrt(pow((91),2)+pow((278-887),2));
+    int lou_r = sqrt(pow((91-768),2)+pow((278-443),2));
+    int roto_t = lou_t/omtrekklosje;
+    int roto_b = lou_b/omtrekklosje;
+    int roto_r = lou_r/omtrekklosje;  
+    
+    counto = stijg_o + roto_t*4200;
+    countb = stijg_b + roto_b*4200;
+    countr = stijg_r + roto_r*4200;
     pc.printf("Tiller");
-/*    Vex = 20;
-    Vey = 20;*/
     controlmotor1.attach(&MotorController1, 0.01);
     controlmotor2.attach(&MotorController2, 0.01);
     controlmotor3.attach(&MotorController3, 0.01);
+    pc.printf("Tiller done");
     }
    
 void zakken_threshold() {