Diff: main.cpp

Revision:

25:c1eab0cc297e

Parent:

24:50235511956c

Child:

26:3e421de274e9

--- a/main.cpp	Thu Nov 02 09:03:07 2017 +0000
+++ b/main.cpp	Thu Nov 02 10:24:47 2017 +0000
@@ -56,6 +56,7 @@
 // Boolean needed to know if new input coordinates have to be given 
 bool            Move_done = false;
 bool            Input_done = true;
+bool            zakker_done = false;
  
 /* Defining all the different BiQuad filters, which contain a Notch filter,
 High-pass filter and Low-pass filter. The Notch filter cancels all frequencies
@@ -307,7 +308,9 @@
 float          countzo;
 float          countzb;
 float          countzr;
-
+int error_o;
+int error_b;
+int error_r;
 float          hcounto;
 float          dcounto;
 float          hcountb;
@@ -354,11 +357,11 @@
     int         reference_o = (int) (counto-hcounto);
     int         position_o = Encoder1.getPulses();
     
-    int error_o = reference_o - position_o;
+    error_o = reference_o - position_o;
     
-    //pc.printf("Position_o = %i          reference_o=%i              Error_o=%i\n\r" ,position_o,reference_o,error_o);
+    pc.printf("Position_o = %i          reference_o=%i              Error_o=%i\n\r" ,position_o,reference_o,error_o);
     
-    if (-100<error_o && error_o<100){
+    if (-10<error_o && error_o<10){
         motorValue1 = 0;
         }
     else {
@@ -385,11 +388,11 @@
     int reference_b = (int) (-(countb-hcountb));
     int position_b = Encoder2.getPulses();
 
-    int error_b = reference_b - position_b;
+    error_b = reference_b - position_b;
     
     //pc.printf("Position_b = %i         reference_b=%i         Error_b=%i      " ,position_b,reference_b,error_b);
         
-    if (-100<error_b && error_b<100){
+    if (-10<error_b && error_b<10){
         motorValue2 = 0;
         }
     else {
@@ -414,12 +417,12 @@
     int reference_r = (int) (-(countr-hcountr));
     int position_r = Encoder3.getPulses();
         
-    int error_r = reference_r - position_r;
+    error_r = reference_r - position_r;
     
-    pc.printf("Position_r = %i          reference_r=%i              Error_r=%i\n\r" ,position_r,reference_r,error_r);
+    //pc.printf("Position_r = %i          reference_r=%i              Error_r=%i\n\r" ,position_r,reference_r,error_r);
     
     
-    if (-100<error_r && error_r<100){
+    if (-10<error_r && error_r<10){
         motorValue3 = 0;
         
         }
@@ -456,15 +459,13 @@
     rotb=Lbu/omtrekklosje; 
     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;
     //pc.printf("dcountb = %f \n\r",dcountb);
     countr=rotr*4200;
-    dcountr=countr-hcountr;
+ 
  
     return 0;
 }
@@ -494,6 +495,29 @@
  //   pc.printf("x=%.2f \n\r y=%.2f \n\r",Psx,Psy);
     return 0;
 }
+
+// Function which makes it possible to lower the end-effector to pick up a piece
+void zakker(){
+    if(Move_done==true){    //misschien moet je hier als voorwaarden een delta is 1 zetten                               // hierdoor wacht dit programma totdat de beweging klaar is 
+     
+    dLod=sqrt(pow(Lou,2)+pow(397.85,2))-Lou;    //dit is wat je motoren moeten doen om te zakken
+    dLbd=sqrt(pow(Lbu,2)+pow(397.85,2))-Lbu;
+    dLrd=sqrt(pow(Lru,2)+pow(397.85,2))-Lru;
+    rotzo=dLod/omtrekklosje;
+    rotzb=dLbd/omtrekklosje;
+    rotzr=dLrd/omtrekklosje;
+    countzo=rotzo*4200;
+    countzb=rotzb*4200;
+    countzr=rotzr*4200;
+    counto=countzo+hcounto-3000;
+    countb=countzb+hcountb-3000;
+    countr=countzr+hcountr-3000;
+        
+    pc.printf("o=%f     b=%f      r=%f",counto,countb,countr);       // hier moet komen te staan hoe het zakken gaat
+    zakker_done=true;
+    }
+}
+
  
 // Rekent dit de snelheid uit waarmee de motoren bewegen?
 void Ve(){
@@ -506,12 +530,14 @@
     }*/
     Pst();
  //   pc.printf("Vex=%.2f \r\n Vey=%.2f \r\n",Vex,Vey);
-    if((fabs(Vex)<0.01f)&&(fabs(Vey)<0.01f)){
+    if((fabs(Vex)<0.01f)&&(fabs(Vey)<0.01f)) {//(error_o<10)&&(error_b<10)&&(error_r<10)){
         Move_done=true;
         loop.detach();
+        pc.printf("movedone is troe");
+        zakker();
         }
 }
- 
+
 // Calculating the desired position, so that the motors can go here
 int calculator(){
     Ps();
@@ -521,31 +547,11 @@
     return 0;
 }
  
-// Function which makes it possible to lower the end-effector to pick up a piece
-void zakker(){
-    while(1){
-        wait(1);
-    if(Move_done==true){    //misschien moet je hier als voorwaarden een delta is 1 zetten                               // hierdoor wacht dit programma totdat de beweging klaar is 
-     
-    dLod=sqrt(pow(Lou,2)+pow(397.85,2))-Lou;    //dit is wat je motoren moeten doen om te zakken
-    dLbd=sqrt(pow(Lbu,2)+pow(397.85,2))-Lbu;
-    dLrd=sqrt(pow(Lru,2)+pow(397.85,2))-Lru;
-    rotzo=dLod/omtrekklosje;
-    rotzb=dLbd/omtrekklosje;
-    rotzr=dLrd/omtrekklosje;
-    countzo=rotzo*4200;
-    countzb=rotzb*4200;
-    countzr=rotzr*4200;
-    
-    //pc.printf("o=%.2fb=%.2fr=%.2f",countzo,countzb,countzr);       // hier moet komen te staan hoe het zakken gaat
-    }
-    }
-}
 
 void tiller(){
-    int reference_o = hcounto-12487;
-    int reference_b = hcountb-8148;
-    int reference_r = hcountr-7386;
+    int counto = -12487;
+    int countb = -8148;
+    int countr = -7386;
     pc.printf("Tiller");
 /*    Vex = 20;
     Vey = 20;*/
@@ -583,7 +589,7 @@
         controlmotor1.attach(&MotorController1, 0.01);
         controlmotor2.attach(&MotorController2, 0.01);
         controlmotor3.attach(&MotorController3, 0.01);
-        //zakker();
+        zakker();
         wait(5.0f);
         }