Diff: main.cpp

Revision:

3:874424bbe577

Parent:

2:f3043132a959

--- a/main.cpp	Sat Apr 09 19:17:26 2016 +0000
+++ b/main.cpp	Wed Apr 13 21:11:55 2016 +0000
@@ -7,17 +7,21 @@
 
 #define Rad2Dree       57.295779513082320876798154814105f
 
-#define PID_L_KP        0.0275f              /* Proporcional */ //0.015f
-#define PID_L_KI        0.000125f              /* Integral */
-#define PID_L_KD        0.075f              /* Derivative */
+#define PID_L_KP       0.0275f              /* Proporcional */ //0.015f
+#define PID_L_KI       0.0f                 /* Integral */
+#define PID_L_KD       0.0f                  /* Derivative */
 
 #define PID_R_KP       0.0275f              /* Proporcional */ //0.015f
-#define PID_R_KI       0.000125f              /* Integral */
-#define PID_R_KD       0.075f              /* Derivative */
+#define PID_R_KI       0.0f                 /* Integral */
+#define PID_R_KD       0.0f                 /* Derivative */
 
-#define L_SP            PI/2
+#define L_SP           PI/2
 #define R_SP           PI/2
 
+#define YOFF  100.0    // Y-Offset
+#define XOFF  2.0      // X-Offset
+#define WOFF  12.0     // Winkel-Offset
+
 DigitalOut led_red(LED_RED);
 DigitalOut led_green(LED_GREEN);
 DigitalIn sw2(SW2);
@@ -30,6 +34,11 @@
 LSM303DLHC imuR(PTC11,PTC10);
 L3GD20H gyroR(PTC11,PTC10);
 
+PwmOut M1(D13);
+PwmOut M2(D12);
+DigitalOut M1D(D11);
+DigitalOut M2D(D10);
+
 Timer GlobalTime;
 Timer ProgramTimer;
 
@@ -70,6 +79,7 @@
 void GyrRaw2DL(short gyr[3]);
 void AccRaw2GR(int acc[3]);
 void GyrRaw2DR(short gyr[3]);
+double calcHeading(int *mag);
 
 int main() {
     arm_pid_instance_f32 L_PID;
@@ -91,10 +101,16 @@
     GlobalTime.start();
     imuL.init();
     imuR.init();
+    M1.period(1.0f/1000.0f);               //Comparten el mismo timer
+    M1.pulsewidth(0.0f/1000.0f);
+    M2.pulsewidth(0.0f/1000.0f);
+    
     led_green = 1;
     led_red = 1;
+    M1D = 0;
+    M2D = 0;
     pc.baud(115200);
-    pc.printf("Hello World from FRDM-K64F board.\r\n");
+    
     kalman_init(&filter_pitch_L, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); 
     kalman_init(&filter_roll_L, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
     kalman_init(&filter_pitch_R, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); 
@@ -106,11 +122,13 @@
 
     while (true) {
         imuL.readAcc(accL);
+        imuL.readMag(magL);
         AccRaw2GL(accL);
         gyroL.read(gyrL);
         GyrRaw2DL(gyrL);
         
         imuR.readAcc(accR);
+        imuR.readMag(magR);
         AccRaw2GR(accR);
         gyroR.read(gyrR);
         GyrRaw2DR(gyrR);
@@ -134,6 +152,9 @@
         angleR[0] = kalman_get_angle(&filter_pitch_R);
         angleR[1] = kalman_get_angle(&filter_roll_R);
         
+        angleL[2] = calcHeading(magL);
+        angleR[2] = calcHeading(magR);
+        
         L_error = angleL[0] - L_SP;
         R_error = angleR[0] - R_SP;
         
@@ -142,8 +163,8 @@
         
         timer = ProgramTimer.read_us();
          
-        printf("IMUL\tPitch=%6.2f\tRoll=%6.2f\r\n",Rad2Dree*angleL[0],Rad2Dree*angleL[1]);
-        printf("IMIR\tPitch=%6.2f\tRoll=%6.2f\r\n",Rad2Dree*angleR[0],Rad2Dree*angleR[1]);
+        printf("IMUL\tPitch=%6.2f\tRoll=%6.2f\tYaw=%6.2f\r\n",Rad2Dree*angleL[0],Rad2Dree*angleL[1],angleL[2]);
+        //printf("IMIR\tPitch=%6.2f\tRoll=%6.2f\tYaw=%6.2f\r\n",Rad2Dree*angleR[0],Rad2Dree*angleR[1],angleR[2]);
         wait(0.2);
     }
 }
@@ -170,4 +191,30 @@
     GyrR.x = gyr[0]/225.0f; 
     GyrR.y = gyr[1]/225.0f; 
     GyrR.z = gyr[2]/225.0f;
+}
+
+double calcHeading(int *mag)
+{
+    double x,y;
+    double hdg;
+    
+    x = mag[0] + XOFF;                  // X-Achsen Ausgleich
+    y = mag[1] + YOFF;                  // Y-Achsen Ausgleich
+    
+    hdg = atan(y/x);
+    hdg *= Rad2Dree;                     // Umrechnung von Bogen- nach Winkelmass
+    if (x > 0)
+    {
+        if(y>0) hdg = hdg;              // Korrektur 1. Quadrant
+        else    hdg = 360.0 + hdg;      // Korrektur 4. Quadrant
+    }
+    else
+    {   
+        if(y>0) hdg = 180.0 + hdg;      // Korrektur 2. Quadrant
+        else hdg = 180.0 + hdg;         // Korrektur 3. Quadrant
+    }
+    
+    hdg -= WOFF;                        // Korrektur Winkel (Vorsicht: Bei Winkeln < WOFF wird hdg negativ!)
+    if (hdg <0) hdg = 360.0 + hdg;        // Winkel soll nicht negativ werden! (Bsp.: -5° => 355°)
+    return (hdg);
 }
\ No newline at end of file