Diff: main.cpp

Revision:

14:291f9a29bd74

Parent:

13:3ad20c09b5b4

Child:

15:46acc9b5decf

--- a/main.cpp	Wed Nov 01 11:48:57 2017 +0000
+++ b/main.cpp	Wed Nov 01 12:14:13 2017 +0000
@@ -54,6 +54,7 @@
 
 // Boolean needed to know if new input coordinates have to be given 
 bool            Move_done = false;
+bool            Input_done = true;
  
 /* Defining all the different BiQuad filters, which contain a Notch filter,
 High-pass filter and Low-pass filter. The Notch filter cancels all frequencies
@@ -202,6 +203,7 @@
 // Couting system for values of X
 int tellerX(){
     if (Move_done == true) {
+        Input_done = false;
         t.reset();
         led_G=1; 
         led_B=1;
@@ -263,7 +265,7 @@
         led_B=1; 
         Yin_new = Yin;
         Yin = 0;
-                
+        Input_done = true;        
         Move_done = false;
         return Yin_new;
         
@@ -360,13 +362,13 @@
     
     int error_o = reference_o - position_o;
     
-    pc.printf("Position_o = %i \n\r reference_o=%i \n\r 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){
         motorValue1 = 0;
         }
     else {
-    motorValue1 = 0.1*P1(error_o, kpo);
+    motorValue1 = 0.001*P1(error_o, kpo);
     }    
     
     if (motorValue1 >=0) motor1DirectionPin=0;
@@ -389,8 +391,8 @@
 
     int error_b = reference_b - position_b;
     
-    pc.printf("position_b= %i", 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){
         motorValue2 = 0;
         }
@@ -418,7 +420,7 @@
         
     int error_r = reference_r - position_r;
     
-    pc.printf("position_r= %i", position_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){
@@ -557,10 +559,12 @@
     
 
 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);
+    if(Input_done==true){
+        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);
+        pc.printf("ik reset hcounts");
+        }
     }
     
 int main()
@@ -574,7 +578,6 @@
     while(true){
         sample_timer.attach(&EMG_sample, 0.002);
         wait(2.5f);
-        
         tellerX();
         tellerY();
         setcurrentposition();