Diff: main.cpp

Revision:

23:91a67476fc2b

Parent:

22:cdaa5c1208a4

Child:

24:50235511956c

--- a/main.cpp	Wed Nov 01 16:50:57 2017 +0000
+++ b/main.cpp	Wed Nov 01 17:24:50 2017 +0000
@@ -327,9 +327,9 @@
 float          Psty;
 float          T=0.02;//seconds
 
-float          kpo = 0.1;
-float          kpb = 0.1;
-float          kpr = 0.1;
+float          kpo = 21;
+float          kpb = 21;
+float          kpr = 21;
  
 float          speedfactor1;
 float          speedfactor2;
@@ -370,7 +370,7 @@
         motorValue1 = 0;
         }
     else {
-    motorValue1 = 0.05*P1(error_o, kpo);
+    motorValue1 = P1(error_o, kpo)/4200;
     }    
     
     
@@ -401,7 +401,7 @@
         motorValue2 = 0;
         }
     else {
-    motorValue2 = 0.05*P2(error_b, kpb);
+    motorValue2 = P2(error_b, kpb)/4200;
     }    
         
     
@@ -432,7 +432,7 @@
         
         }
     else {
-    motorValue3 = 0.05*P3(error_r, kpr);
+    motorValue3 = P3(error_r, kpr)/4200;
     }    
             
     if (motorValue3 <=0) motor3DirectionPin=0;
@@ -533,7 +533,8 @@
 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  
+    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;