Diff: main.cpp

Revision:

10:12f9371f3e0f

Parent:

9:2779500685cb

--- a/main.cpp	Sun May 15 16:15:44 2016 +0000
+++ b/main.cpp	Mon May 16 09:50:44 2016 +0000
@@ -31,12 +31,14 @@
     pc.printf("Calibratie compleet \r\n"); 
     mpu6050.init();                              // Initializeer de gyroscoop
     pc.printf("MPU6050 is klaar \r\n\r\n");
-   
+
     updatePID.attach(&updatePIDfunctie, 0.005);       // Elke 5ms onze complementary Filter aanroepen
     
     while(1)
     {          
-       pc.printf("Roll: %.1f, Pitch: %.1f\r\n",rollAngle,pitchAngle); //DEBUG
+       pc.printf("Roll: %.1f, Pitch: %.1f\r\n",rollAngle, pitchAngle); //DEBUG
+       pc.printf("pitch: dir: %d, delay: %f\r\n",pitchMotorPID.dir, pitchMotorPID.delay); //DEBUG
+       pc.printf("roll: dir: %d, delay: %f\r\n",rollMotorPID.dir, rollMotorPID.delay); //DEBUG
        wait_ms(500); //500ms wachten
     }    
 }
@@ -50,22 +52,24 @@
     rollMotorPID.PIDaanpassing(rollAngle);
     
     //pitch aanpassingen nodig?
-    if(pitchMotorPID.stop)  // if the gimbal is within noise margin, dont move.
+    if(pitchMotorPID.stop){  // if the gimbal is within noise margin, dont move.
         pitchMotorTicker.detach();
-    else    
+    }else{
         pitchMotorTicker.attach(&pitchMotorFunction, pitchMotorPID.delay);  
+    }
     
     //roll motor aanpassingen nodig?
-    if(rollMotorPID.stop)  // if the gimbal is within noise margin, dont move.
+    if(rollMotorPID.stop){  // if the gimbal is within noise margin, dont move.
         rollMotorTicker.detach();
-    else    
-        rollMotorTicker.attach(&rollMotorFunction, rollMotorPID.delay);  
+    } else { 
+        rollMotorTicker.attach(&rollMotorFunction, rollMotorPID.delay); 
+    } 
 
 }
 
 
 void pitchMotorFunction(){
-    pitchMotor.brushlessControl(rollMotorPID.dir, 0);    
+    pitchMotor.brushlessControl(pitchMotorPID.dir, 0);    
 }
 
 void rollMotorFunction(){