Diff: Source/main.cpp

Revision:

3:cc828af4a4c6

Parent:

1:8c4f93e10af3

--- a/Source/main.cpp	Thu Oct 19 11:52:10 2017 +0000
+++ b/Source/main.cpp	Thu Sep 20 07:43:09 2018 +0000
@@ -9,28 +9,6 @@
  */
 #define PI 3.1415926536
 
-/* Choose PID-T1 parameters VELOCITY Controller */
-//P-Part
-#define P_gain   1.666666666666668//2.5
-#define P_b01    1.0
-#define P_b11   -0.843146028951280
-#define P_a11   -0.738576714918798
-//I-Part
-#define I_gain   0.050505050505053
-#define I_b11    1.0
-#define I_a11   -1.0
-//Pre-Gain
-#define k        0.053348382301167*2
-
-/* Choose P parameter POSITION Controller */
-#define KpPosition  25*1.5
-
-/* Choose setposition for the gimbal */
-#define SOLLPOSITION 0 // 30° = -0.523598775598299 //[rad]
-
-
-/* Assign a unique ID to the sensors */
-//This board/chip uses I2C 7-bit addresses 0x19 & 0x1E & 0x6B
 
 Adafruit_9DOF                 dof   = Adafruit_9DOF();
 
@@ -41,8 +19,7 @@
 Adafruit_L3GD20_Unified gyro        = Adafruit_L3GD20_Unified(30303);
 
 //Debug/Communication with PC through
-Serial pc(PA_0, PA_1, 115200); //921600; 115200
-Serial serial1(USBTX, USBRX, 9600);
+Serial pc(SERIAL_TX, SERIAL_RX,115200);        // serial connection via USB - programmer
 
 // Magnetometer correction
 // mounted in gimbal
@@ -84,7 +61,7 @@
 //Timer for controller interrupt
 // the controll loop will be atached to the timer interrupt
 Ticker  ControllerLoopTimer;
-void updateControllers(void);
+void updateAHRS(void);
 
 //Sensor events, contains the last read sensor values
 sensors_event_t mag_event;
@@ -120,13 +97,13 @@
 
 void initSensors()
 {
-
+ pc.printf(("Init...\r\n"));
   if(!accel.begin())
   {
  
    /* There was a problem detecting the LSM303 ... check your connections */
     
-    pc.printf(("Ooops, no LSM303 detected ... Check your wiring!"));
+    pc.printf(("Ooops, no LSM303 detected ... Check your wiring!\r\n"));
 
     while(1);
 
@@ -136,7 +113,7 @@
 
     /* There was a problem detecting the LSM303 ... check your connections */
 
-    pc.printf("Ooops, no LSM303 detected ... Check your wiring!");
+    pc.printf("Ooops, no LSM303 detected ... Check your wiring!\r\n");
 
     while(1);
 
@@ -149,7 +126,7 @@
 
     /* There was a problem detecting the LSM303 ... check your connections */
 
-    pc.printf("Ooops, no L3GD20 detected ... Check your wiring!");
+    pc.printf("Ooops, no L3GD20 detected ... Check your wiring!\r\n");
 
     while(1);
 
@@ -178,29 +155,35 @@
 {
     //init system
     setup();      
+    
     //set PWM frequncy to 1 kHZ
-    SollStrom.period(0.001);
-    //set Current to 0 Ampère
-    SollStrom.write(0.5);
-    //enable motor
-    motorEnable.write(1);     
-    
-    //wait for start (user button press)
-    while( SystemEnable )
-    {
-        serial1.printf("Wait for system enable! \r\n");
-    }
-    while( !SystemEnable ){}
-    //attach controller loop to timer interrupt
-    ControllerLoopTimer.attach(&updateControllers, 0.003030303030303f); //Assume Ts = 1/330;
+    wait_ms(500);
+    pc.printf("Started! \r\n");
+    //ControllerLoopTimer.attach(&updateControllers, 1.0f); //Assume Ts = 1/100;
     //controller on -> LED on
-    SystemEnableLED = 1;
-    //start timer for sample time estimation
     t.start();
     //do as long user button d'ont get pressed
-    while ( SystemEnable )
+    while ( 1 )// schafft etwa 400Hz
     {
-        //read raw sensor data
+        LoopCounter++;
+        updateAHRS();
+        if(LoopCounter == 100)
+        {
+          LoopCounter = 0;
+         //pc.printf("R %3.2f P %3.2f Y %3.2f \r\n",filter.getRoll(),filter.getPitch(),filter.getYaw());
+         //pc.printf("R %2.4f P %2.4f Y %2.4f \r\n",gyro_event.gyro.x,gyro_event.gyro.y,gyro_event.gyro.z);
+         filter.getRoll();
+         pc.printf("A %f B %f C %f D %f\r\n",filter.Q[0],filter.Q[1],filter.Q[2],filter.Q[3]);
+        }
+        //wait_ms(100);
+
+    }
+}
+
+
+void updateAHRS(void)
+{
+       
         mag.getEvent(&mag_event);
         accel.getEvent(&accel_event);
         gyro.getEvent(&gyro_event);
@@ -233,121 +216,6 @@
         //Perform Madgwick-filter update
         filter.update(   CorrectedGyrometer[0]/ PI * 180, -CorrectedGyrometer[1]/ PI * 180, -CorrectedGyrometer[2]/ PI * 180 ,
                         -CorrectedAccelerometer[0] ,       CorrectedAccelerometer[1] ,       CorrectedAccelerometer[2] ,
-                         CorrectedMagnetometer[0] ,       -CorrectedMagnetometer[1],        -CorrectedMagnetometer[2]);
-
-        LoopCounter++;
-        if(LoopCounter == 12000)
-        {
-          eMaxPos = 0;
-          eMaxNeg = 0;
-          serial1.printf("Error reseted");   
-        }
-        //serial1.printf("Roll: %f Grad \r\n",filter.getRoll());
-
-    }
-    //Stop timer
-    TotalTime = t.read();
-    //disable motor
-    motorEnable.write(0);
-    //set Current to 0 Ampère
-    SollStrom.write(0.5);
-    //Switch controller off
-    ControllerLoopTimer.detach();
-    //controller off -> LED off
-    SystemEnableLED = 0;
-    //Print mean loop time ->sample frequency of the madgwick-filter
-    serial1.printf("Mean loop time: %f [s] \r\n", TotalTime/LoopCounter);
-    serial1.printf("Max pos Fehler: %f [°] \r\n", eMaxPos/PI*180);
-    serial1.printf("Max neg Fehler: %f [°] \r\n", eMaxNeg/PI*180);    
-    serial1.printf("Finished!");
-}
-
-
-
-/**************************************************************************
-/
-/
-    @brief  Regler (Geschwindigkeits- und Positionsregler)
-
-/
-**************************************************************************/
-//-----------------------------------------------------------------------//
-//                                                                       //
-//                                                                       //
-//                                     controller (PID-T1)               //
-//ThetaDes[rad]  ____              ____/                                 //
-//  ------>O--->| Kx |----->O---->| Cv |---> I_des [A]                   //
-//         ^-    ----       ^-     ----                                  //
-//         |   Pos.Cntrl.   |   Speed Cntrl.                             //
-//         |                |                                            //
-//         |                |                                            //
-//         |                +---- Speed Omega [rad/s]                    //
-//         |                                                             //
-//         +--------------------- Position Theta [rad]                   //
-//                                                                       //
-//-----------------------------------------------------------------------//
-
-// Cv is split into a prop. and an integrating part (limiting, windup)
-//   e    ___             ____ 
-//  ---->| k |---+------>| Cp |--------->O---->
-//        ---    |        ----           ^
-//               |   ____    ________    |
-//               +->| k_I|->| z/(z-1)|---+
-//                   ----    -------- \
-//                                     Integrator
-//
-// the anti-windup loop is not shon in the diagram above! (see documentation
-// for detail)
-void updateControllers(void)
-{
-        static float ek_1 = 0;
-        static float ukP_1 = 0;
-        static float ukI_1 = 0;
-        
-        float ePosition =  SOLLPOSITION - filter.getRollRadians();
-        
-        //Positionsregler
-        float stellgroessePosition = KpPosition*(ePosition);
-        if( ePosition > eMaxPos)
-        {
-            eMaxPos = ePosition;
-        }
-        if( ePosition < eMaxNeg )
-        {
-            eMaxNeg =  ePosition;
-        }
-
-        //Geschwindigkeitsregler
-        float ek = k*(stellgroessePosition - CorrectedGyrometer[0]);
-        //P-part
-        float ukP = P_gain*(P_b01*ek + P_b11*ek_1) - P_a11*ukP_1;
-        //I-Part
-        float stellgroesseStrom = ukP + ukI_1;
-        float ukI = I_gain*I_b11*ek_1 - I_a11*ukI_1;
-
-        //Limit +/- 0.5 Ampère
-        float uSaturated;
-        if(stellgroesseStrom > 0.5)
-        {
-            SollStrom.write( 0.8999999 );
-            uSaturated = 0.5; 
-        }
-        else if(  stellgroesseStrom < -0.5 )
-        {
-            SollStrom.write( 0.1000001 );
-            uSaturated = -0.5;
-        }
-        else
-        {
-            SollStrom.write(0.5f+stellgroesseStrom*0.4f/0.5f);
-            uSaturated = stellgroesseStrom;
-        }
-        
-        //Anti-Windup (back calculation)
-        float deltaI = ( -stellgroesseStrom + uSaturated );
-        ukI_1 = ukI + deltaI*I_gain;
-        ek_1 = ek;
-        ukP_1 = ukP;
-
-
+                         CorrectedMagnetometer[0] ,       -CorrectedMagnetometer[1],        -CorrectedMagnetometer[2]); 
+    
 }
\ No newline at end of file