//

Dependencies:   MPU6050 brushlessMotor mbed

Fork of gimbalController_brushless_IMU by Baser Kandehir

Files at this revision

API Documentation at this revision

Comitter:
alfaleader
Date:
Mon May 16 09:50:44 2016 +0000
Parent:
9:2779500685cb
Commit message:
d

Changed in this revision

MPU6050.lib Show annotated file Show diff for this revision Revisions of this file
PID/PID.cpp Show annotated file Show diff for this revision Revisions of this file
PID/PID.h Show annotated file Show diff for this revision Revisions of this file
brushlessMotor.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r 2779500685cb -r 12f9371f3e0f MPU6050.lib
--- a/MPU6050.lib	Sun May 15 16:15:44 2016 +0000
+++ b/MPU6050.lib	Mon May 16 09:50:44 2016 +0000
@@ -1,1 +1,1 @@
-https://developer.mbed.org/users/alfaleader/code/MPU6050/#a8a0621e4732
+https://developer.mbed.org/users/alfaleader/code/MPU6050/#ae197c3fd843
diff -r 2779500685cb -r 12f9371f3e0f PID/PID.cpp
--- a/PID/PID.cpp	Sun May 15 16:15:44 2016 +0000
+++ b/PID/PID.cpp	Mon May 16 09:50:44 2016 +0000
@@ -2,6 +2,7 @@
 
 PIDControll::PIDControll() { 
     //variabelen instellen als we ons PIDControll object aanmaken
+    //TODO: finetunen, andere Kp Ki Kd waarden
     Kp = 10;
     Ki = 0.0001; 
     Kd = 5;
@@ -12,6 +13,7 @@
     derivative = 0;
     errorPID = 0;  
     dir = 0;  
+    delay = 0;
 }
 
 void PIDControll::PIDaanpassing(float angle){
@@ -24,14 +26,15 @@
     (errorPID > 0)?(dir = 1):(dir = 0);
     
     // errorPID is restricted between -400 and 400 
+    //TODO: andere maximum of minimum errorPID (verhogen? verlagen?)
     if(errorPID > 400)
         errorPID = 400;
     else if(errorPID < -400)
         errorPID = -400;   
    
    stop = 0;   
+   //TODO: finetunes: mss hoger of lager
    delay = 0.1/abs(errorPID);      // speed should be proportional to error, therefore time delay
                                      //between steps should be inverse proportional to error.
-   if (abs(errorPID)< Kp/2) stop = 1;  // 0.5 deg noise margin
-         
+   if (abs(errorPID)< Kp/2) stop = 1;  // 0.5 deg noise margin    
 }
\ No newline at end of file
diff -r 2779500685cb -r 12f9371f3e0f PID/PID.h
--- a/PID/PID.h	Sun May 15 16:15:44 2016 +0000
+++ b/PID/PID.h	Mon May 16 09:50:44 2016 +0000
@@ -20,7 +20,7 @@
     float derivative;
     float errorPID;     
     int dir;
-    int delay;  
+    float delay;  
     int stop;  
 };
 
diff -r 2779500685cb -r 12f9371f3e0f brushlessMotor.lib
--- a/brushlessMotor.lib	Sun May 15 16:15:44 2016 +0000
+++ b/brushlessMotor.lib	Mon May 16 09:50:44 2016 +0000
@@ -1,1 +1,1 @@
-https://developer.mbed.org/users/alfaleader/code/brushlessMotor/#71fb2b106f41
+https://developer.mbed.org/users/alfaleader/code/brushlessMotor/#027fe6d9a8a7
diff -r 2779500685cb -r 12f9371f3e0f main.cpp
--- 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(){