Diff: Source/main.cpp

Revision:

1:8c4f93e10af3

Parent:

0:772bf4786416

Child:

3:bd353b8184cc

--- a/Source/main.cpp	Sat Mar 21 12:33:05 2015 +0000
+++ b/Source/main.cpp	Thu Oct 19 11:50:04 2017 +0000
@@ -2,10 +2,35 @@
 #define _MBED_
 #include "Adafruit_9DOF.h"
 #include "Serial_base.h"
+#include "MadgwickAHRS.h"
 
+/**
+ * Defines
+ */
+#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();
 
@@ -13,11 +38,77 @@
 
 Adafruit_LSM303_Mag_Unified   mag   = Adafruit_LSM303_Mag_Unified(30302);
 
+Adafruit_L3GD20_Unified gyro        = Adafruit_L3GD20_Unified(30303);
 
-/* Update this with the correct SLP for accurate altitude measurements */
+//Debug/Communication with PC through
+Serial pc(PA_0, PA_1, 115200); //921600; 115200
+Serial serial1(USBTX, USBRX, 9600);
+
+// Magnetometer correction
+// mounted in gimbal
+
+//Cal for Nickachse
+//double RotMag [3][3]     = {{0.947568, -0.0262851, 0.0355492}, 
+//                           {-0.0262851, 0.98413,   0.014105}, 
+//                            {0.0355492, 0.014105,  0.970769}};
+                         
+//double offsetsMag [3]    = {   -22.1205, -13.9738, -2.55159   };
+
+double offsetsMag[3] = {23.5297, -7.51693, 17.5943};
+double RotMag[3][3] = {{0.678312, -0.0371453, 0.0139236}, {-0.0371453, 0.993299, 0.0229827}, {0.0139236, 0.0229827, 0.809984}};
+
+//double gainsMag [3]      = {    39.1659,  38.1534,   35.7245   };
+//double refMag            =  47.9824; //nT
+
+//Accelerometer correction
+
+double RotAccel [3][3]     = { {-0.1251,   -0.7810,   -0.6118},
+                              { 0.0409,    0.6121,   -0.7879},
+                              { 0.9913,   -0.1238,   -0.0466}  };  
+                         
+                         
+double offsetsAccel [3]    = {   -0.215747,   0.509946,   -0.235360   };
+double gainsAccel   [3]    = {    0.978548,   0.976637,    0.939921   };
+
+
+//Gyrometer correction
+double offsetsGyro [3]     = { -0.0107709,     0.00729576,   0.0225833   };
 
-float seaLevelPressure = SENSORS_PRESSURE_SEALEVELHPA;
+//Filter for orientation estimation
+Madgwick filter;
+
+DigitalIn CPUEnable(D11);
+DigitalOut motorEnable(D12, 0);
+PwmOut SollStrom(PB_3);    //in [A]
+
+//Timer for controller interrupt
+// the controll loop will be atached to the timer interrupt
+Ticker  ControllerLoopTimer;
+void updateControllers(void);
+
+//Sensor events, contains the last read sensor values
+sensors_event_t mag_event;
+sensors_event_t accel_event;
+sensors_event_t gyro_event;
 
+//Sensor values after the calibration/correction is applied, (x,y,z)
+double CorrectedMagnetometer[3];
+double CorrectedAccelerometer[3];
+double CorrectedGyrometer[3];
+
+//"User Interface", Start/Stop controller
+DigitalIn SystemEnable(USER_BUTTON);
+//Status LED, on when controller on
+DigitalOut SystemEnableLED(LED3, 0);
+
+//variables for loop time estimation
+uint32_t LoopCounter = 0;
+Timer t;
+float TotalTime;
+
+//maximaler Fehler
+float eMaxPos = 0;
+float eMaxNeg = 0;
 
 /**************************************************************************
 /
@@ -35,7 +126,7 @@
  
    /* There was a problem detecting the LSM303 ... check your connections */
     
-    s_com->println(("Ooops, no LSM303 detected ... Check your wiring!"));
+    pc.printf(("Ooops, no LSM303 detected ... Check your wiring!"));
 
     while(1);
 
@@ -45,104 +136,218 @@
 
     /* There was a problem detecting the LSM303 ... check your connections */
 
-    s_com->println("Ooops, no LSM303 detected ... Check your wiring!");
+    pc.printf("Ooops, no LSM303 detected ... Check your wiring!");
+
+    while(1);
+
+  }
+  
+  gyroRange_t rng = GYRO_RANGE_2000DPS; //set gyro range to +/-2000dps
+  
+  if(!gyro.begin(rng))
+  {
+
+    /* There was a problem detecting the LSM303 ... check your connections */
+
+    pc.printf("Ooops, no L3GD20 detected ... Check your wiring!");
 
     while(1);
 
   }
 
 }
-/**************************************************************************/
+/**************************************************************************
 
+    @brief  system initialisation
 
 
-/**************************************************************************/
-
-void setup(void)
-{
+**************************************************************************/
 
-  
-
-  s_com->println(("Adafruit 9 DOF Pitch/Roll/Heading Example"));
- s_com->println("");
-
+void setup(void) 
+{
+  pc.printf(("Adafruit 9 DOF Pitch/Roll/Heading Example \n"));
+  pc.printf("\n");
   
   /* Initialise the sensors */
-
   initSensors();
 
 }
 
 
+int main()
+{
+    //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;
+    //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 )
+    {
+        //read raw sensor data
+        mag.getEvent(&mag_event);
+        accel.getEvent(&accel_event);
+        gyro.getEvent(&gyro_event);
+        
+        //Magnetometer correction
+        mag_event.magnetic.x = ((double)mag_event.magnetic.x) - offsetsMag[0];
+        mag_event.magnetic.y = ((double)mag_event.magnetic.y) - offsetsMag[1];
+        mag_event.magnetic.z = ((double)mag_event.magnetic.z) - offsetsMag[2];
+          
+        CorrectedMagnetometer[0] = ((double)mag_event.magnetic.x)*RotMag[0][0]  +  ((double)mag_event.magnetic.y)*RotMag[0][1]  +  ((double)mag_event.magnetic.z)*RotMag[0][2];
+        CorrectedMagnetometer[1] = ((double)mag_event.magnetic.x)*RotMag[1][0]  +  ((double)mag_event.magnetic.y)*RotMag[1][1]  +  ((double)mag_event.magnetic.z)*RotMag[1][2];
+        CorrectedMagnetometer[2] = ((double)mag_event.magnetic.x)*RotMag[2][0]  +  ((double)mag_event.magnetic.y)*RotMag[2][1]  +  ((double)mag_event.magnetic.z)*RotMag[2][2];
+
+        
+        //Accelerometer correction
+        CorrectedAccelerometer[0] = ((double)accel_event.acceleration.x) - offsetsAccel[0];
+        CorrectedAccelerometer[1] = ((double)accel_event.acceleration.y) - offsetsAccel[1];
+        CorrectedAccelerometer[2] = ((double)accel_event.acceleration.z) - offsetsAccel[2];
+        
+        CorrectedAccelerometer[0] = CorrectedAccelerometer[0] * gainsAccel[0];
+        CorrectedAccelerometer[1] = CorrectedAccelerometer[1] * gainsAccel[1];
+        CorrectedAccelerometer[2] = CorrectedAccelerometer[2] * gainsAccel[2];
+        
+        
+        //Gyrometer correction
+        CorrectedGyrometer[0] = ((double)gyro_event.gyro.x)  - offsetsGyro[0];
+        CorrectedGyrometer[1] = ((double)gyro_event.gyro.y)  - offsetsGyro[1];
+        CorrectedGyrometer[2] = ((double)gyro_event.gyro.z)  - offsetsGyro[2];
+        
+        //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  Constantly check the roll/pitch/heading/altitude/temperature
-
-**************************************************************************/
-
-void loop(void)
-{
+    @brief  Regler (Geschwindigkeits- und Positionsregler)
 
-  sensors_event_t accel_event;
-
-  sensors_event_t mag_event;
-
-  sensors_vec_t   orientation;
-
+/
+**************************************************************************/
+//-----------------------------------------------------------------------//
+//                                                                       //
+//                                                                       //
+//                                     controller (PID-T1)               //
+//ThetaDes[rad]  ____              ____/                                 //
+//  ------>O--->| Kx |----->O---->| Cv |---> I_des [A]                   //
+//         ^-    ----       ^-     ----                                  //
+//         |   Pos.Cntrl.   |   Speed Cntrl.                             //
+//         |                |                                            //
+//         |                |                                            //
+//         |                +---- Speed Omega [rad/s]                    //
+//         |                                                             //
+//         +--------------------- Position Theta [rad]                   //
+//                                                                       //
+//-----------------------------------------------------------------------//
 
-  /* Calculate pitch and roll from the raw accelerometer data */
-
-  accel.getEvent(&accel_event);
-
-  if (dof.accelGetOrientation(&accel_event, &orientation))
-  {
+// 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;
+        }
 
-    /* 'orientation' should have valid .roll and .pitch fields */
-
-    s_com->print(("Roll: "));
+        //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;
 
-    s_com->print(orientation.roll);
-
-    s_com->print(("; "));
-
-    s_com->print(("Pitch: "));
+        //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;
 
 
-    s_com->print(orientation.pitch);
-
-    s_com->print(("; "));
-
-  }
-
-  
-  /* Calculate the heading using the magnetometer */
-
-  mag.getEvent(&mag_event);
-
-  if (dof.magGetOrientation(SENSOR_AXIS_Z, &mag_event, &orientation))
-  {
-
-    /* 'orientation' should have valid .heading data now */
-
-    s_com->print(("Heading: "));
-
-    s_com->print(orientation.heading);
-
-    s_com->print(("; "));
-
-  }
-
-
-  s_com->println((""));
-
-  wait(0.1);
-
-}
-    
-int main()
-{
-    setup();
-    while(1)
-        loop();
 }
\ No newline at end of file