Diff: main.cpp

Revision:

26:3e421de274e9

Parent:

25:c1eab0cc297e

--- a/main.cpp	Thu Nov 02 10:24:47 2017 +0000
+++ b/main.cpp	Tue Nov 07 13:42:13 2017 +0000
@@ -44,6 +44,7 @@
 float          currentTimeTL;
  
 InterruptIn     button(SW2); // Wordt uiteindelijk vervangen door EMG
+InterruptIn     button2(SW3);
  
 Timer           t;
 float          speedfactor; // = 0.01; snelheid in, zonder potmeter gebruik            <- waarom is dit zo?
@@ -56,7 +57,6 @@
 // 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
@@ -308,9 +308,7 @@
 float          countzo;
 float          countzb;
 float          countzr;
-int error_o;
-int error_b;
-int error_r;
+
 float          hcounto;
 float          dcounto;
 float          hcountb;
@@ -357,11 +355,11 @@
     int         reference_o = (int) (counto-hcounto);
     int         position_o = Encoder1.getPulses();
     
-    error_o = reference_o - position_o;
+    int 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 (-10<error_o && error_o<10){
+    if (-20<error_o && error_o<20){
         motorValue1 = 0;
         }
     else {
@@ -388,11 +386,11 @@
     int reference_b = (int) (-(countb-hcountb));
     int position_b = Encoder2.getPulses();
 
-    error_b = reference_b - position_b;
+    int error_b = reference_b - position_b;
     
     //pc.printf("Position_b = %i         reference_b=%i         Error_b=%i      " ,position_b,reference_b,error_b);
         
-    if (-10<error_b && error_b<10){
+    if (-20<error_b && error_b<20){
         motorValue2 = 0;
         }
     else {
@@ -417,12 +415,12 @@
     int reference_r = (int) (-(countr-hcountr));
     int position_r = Encoder3.getPulses();
         
-    error_r = reference_r - position_r;
+    int 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);
     
     
-    if (-10<error_r && error_r<10){
+    if (-20<error_r && error_r<20){
         motorValue3 = 0;
         
         }
@@ -459,13 +457,15 @@
     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;
 }
@@ -495,29 +495,6 @@
  //   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(){
@@ -530,14 +507,12 @@
     }*/
     Pst();
  //   pc.printf("Vex=%.2f \r\n Vey=%.2f \r\n",Vex,Vey);
-    if((fabs(Vex)<0.01f)&&(fabs(Vey)<0.01f)) {//(error_o<10)&&(error_b<10)&&(error_r<10)){
+    if((fabs(Vex)<0.01f)&&(fabs(Vey)<0.01f)){
         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();
@@ -547,11 +522,31 @@
     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(127.85,2))-Lou;    //dit is wat je motoren moeten doen om te zakken
+    dLbd=sqrt(pow(Lbu,2)+pow(127.85,2))-Lbu;    // dat laatste getal moet nog aangepast worden
+    dLrd=sqrt(pow(Lru,2)+pow(127.85,2))-Lru;
+    rotzo=dLod/omtrekklosje;
+    rotzb=dLbd/omtrekklosje;
+    rotzr=dLrd/omtrekklosje;
+    countzo=rotzo*4200;
+    countzb=rotzb*4200;
+    countzr=rotzr*4200;
+    counto=-countzo;
+    countb=-countzb;
+    countr=-countzr;
+    
+    pc.printf("o=%.2fb=%.2fr=%.2f",countzo,countzb,countzr);       // hier moet komen te staan hoe het zakken gaat
+    }
+}
 
 void tiller(){
-    int counto = -12487;
-    int countb = -8148;
-    int countr = -7386;
+    int reference_o = hcounto-12487;
+    int reference_b = hcountb-8148;
+    int reference_r = hcountr-7386;
     pc.printf("Tiller");
 /*    Vex = 20;
     Vey = 20;*/
@@ -589,7 +584,7 @@
         controlmotor1.attach(&MotorController1, 0.01);
         controlmotor2.attach(&MotorController2, 0.01);
         controlmotor3.attach(&MotorController3, 0.01);
-        zakker();
+        button2.fall(zakker);
         wait(5.0f);
         }