Diff: main.cpp

Revision:

9:2779500685cb

Parent:

8:c573f315319a

Child:

10:12f9371f3e0f

--- a/main.cpp	Thu May 12 20:10:25 2016 +0000
+++ b/main.cpp	Sun May 15 16:15:44 2016 +0000
@@ -1,35 +1,27 @@
 #include "mbed.h"
 #include "MPU6050.h"
+#include "PID.h"
 #include "brushlessMotor.h"
 
 Serial pc(USBTX, USBRX);       // Debug serial connectie (voor de computer)
 MPU6050 mpu6050;               // mpu6050 object voor onze gyroscoop
-Ticker filter;                 // een Ticker object voor onze filter --> een ticker is een functie die x keer per seconde uitgevoerd gaat worden
-Ticker gimbal;                 // ticker voor de gimball
-Ticker speed;                  // ticker voor de snelheids controle
+
+PIDControll pitchMotorPID; //PID controle aanmaken voor de pitchmotor
+brushlessMotor pitchMotor(D5, D6, D7); //motoraansturing per motor
+Ticker pitchMotorTicker; //ticker (functie die x keer per seconde gaat uitgevoerd worden)
+void pitchMotorFunction(); //functie voor deze ticker (deze functie vullen we later in)
 
-//Functie prototypes, zodat het programma weet dat we deze functies later gaan "invullen"
-void compFilter();
-void gimbalPID();
-void speedControl();
+PIDControll rollMotorPID; //idem voor roll motor
+brushlessMotor rollMotor(D8, D9, D10);
+Ticker rollMotorTicker;
+void rollMotorFunction();
+
+Ticker updatePID;                 // een Ticker object voor onze filter --> een ticker is een functie die x keer per seconde uitgevoerd gaat worden
+void updatePIDfunctie();
 
 //variabelen nodig voor ons programma
 float pitchAngle = 0;
-float rollAngle = 0;
-bool dir;           // links of rechts
-bool stop;          // motor moet stoppen?
-float delay;        // delay tussen onze stappen
-
-//PID
-float Kp = 10;
-float Ki = 0.0001; 
-float Kd = 5;
-float set_point = 0;         // camera hoek
-float proportional = 0;
-float last_proportional =0;
-float integral = 0;
-float derivative = 0;
-float errorPID = 0;          
+float rollAngle = 0;         
 
 int main()
 {
@@ -40,8 +32,7 @@
     mpu6050.init();                              // Initializeer de gyroscoop
     pc.printf("MPU6050 is klaar \r\n\r\n");
    
-    filter.attach(&compFilter, 0.005);       // Elke 5ms onze complementary Filter aanroepen
-    gimbal.attach(&gimbalPID,  0.005);       // Gimbal aansturing ook elke 5ms aanroepen
+    updatePID.attach(&updatePIDfunctie, 0.005);       // Elke 5ms onze complementary Filter aanroepen
     
     while(1)
     {          
@@ -51,38 +42,32 @@
 }
 
 //elke 5ms de filter functie van de mpu6050 library aanroepen, berekent de pitch en de roll en voert deze in bij de variabelen pitchAngle en rollAngle
-void compFilter() {mpu6050.complementaryFilter(&pitchAngle, &rollAngle);}
-
-//elke 5ms updaten we ook de gimball
-void gimbalPID()
-{
-    proportional = set_point - rollAngle;        
-    integral += proportional; 
-    derivative = proportional - last_proportional;
-    last_proportional = proportional;
+void updatePIDfunctie() {
+    mpu6050.complementaryFilter(&pitchAngle, &rollAngle); //lees de pitchangle en de rollangle uit vanuit de gyroscoop
     
-    errorPID = (Kp * proportional) + (Ki * integral) + (Kd * derivative); 
-    (errorPID > 0)?(dir = 1):(dir = 0);
+    //voer de PID regelaar uit
+    pitchMotorPID.PIDaanpassing(pitchAngle);
+    rollMotorPID.PIDaanpassing(rollAngle);
     
-    // errorPID is restricted between -400 and 400 
-    if(errorPID > 400)
-        errorPID = 400;
-    else if(errorPID < -400)
-        errorPID = -400;   
-   
-    stop = 0;   
-    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(stop)  // if the gimbal is within noise margin, dont move.
-        speed.detach();
+    //pitch aanpassingen nodig?
+    if(pitchMotorPID.stop)  // if the gimbal is within noise margin, dont move.
+        pitchMotorTicker.detach();
     else    
-        speed.attach(&speedControl, delay);  
+        pitchMotorTicker.attach(&pitchMotorFunction, pitchMotorPID.delay);  
+    
+    //roll motor aanpassingen nodig?
+    if(rollMotorPID.stop)  // if the gimbal is within noise margin, dont move.
+        rollMotorTicker.detach();
+    else    
+        rollMotorTicker.attach(&rollMotorFunction, rollMotorPID.delay);  
+
 }
 
-void speedControl() 
-{    
-    brushlessControl(dir, 0);    
+
+void pitchMotorFunction(){
+    pitchMotor.brushlessControl(rollMotorPID.dir, 0);    
+}
+
+void rollMotorFunction(){
+     rollMotor.brushlessControl(rollMotorPID.dir, 0);    
 }
\ No newline at end of file