Diff: main.cpp

Revision:

7:9d4313510646

Parent:

6:179752756e9f

Child:

8:d25ecdcdbeb5

--- a/main.cpp	Sat Oct 13 11:12:22 2012 +0000
+++ b/main.cpp	Mon Oct 15 17:23:06 2012 +0000
@@ -1,11 +1,12 @@
-#include "mbed.h" // Standar Library
-#include "LED.h" // LEDs
-#include "L3G4200D.h" // Gyro (Gyroscope)
-#include "ADXL345.h" // Acc (Accelerometer)
-#include "HMC5883.h" // Comp (Compass)
-#include "BMP085.h" // Alt (Altitude sensor)
-#include "Servo.h" // Motor
-#include "terminal.h"
+#include "mbed.h"       // Standard Library
+#include "LED.h"        // LEDs framework for blinking ;)
+#include "L3G4200D.h"   // Gyro (Gyroscope)
+#include "ADXL345.h"    // Acc (Accelerometer)
+#include "HMC5883.h"    // Comp (Compass)
+#include "BMP085.h"     // Alt (Altitude sensor)
+#include "RC_Channel.h" // RemoteControl Chnnels with PPM
+#include "Servo.h"      // Motor PPM
+#include "PC.h"         // Serial Port via USB for debugging
 
 Timer GlobalTimer;
 
@@ -15,9 +16,12 @@
 ADXL345     Acc(p28, p27);
 HMC5883     Comp(p28, p27, GlobalTimer);
 BMP085      Alt(p28, p27);
-Servo     Motor(p20);
+//Servo       Motor_front(p18);
+Servo       Motor_left(p17);
+Servo       Motor_right(p19);
+Servo       Motor_back(p20);
 
-terminal       pc(USBTX, USBRX);        //Achtung: Treiber auf PC fuer Serial-mbed installieren. hier herunterladen https://mbed.org/media/downloads/drivers/mbedWinSerial_16466.exe
+PC          pc(USBTX, USBRX);        //Achtung: Treiber auf PC fuer Serial-mbed installieren. hier herunterladen https://mbed.org/media/downloads/drivers/mbedWinSerial_16466.exe
 
 #define PI             3.1415926535897932384626433832795
 #define Rad2Deg        57.295779513082320876798154814105
@@ -40,7 +44,9 @@
     float Comp_angle = 0;
     float tempangle = 0;
     
-    Motor.initialize();
+    Motor_left.initialize();
+    Motor_right.initialize();
+    Motor_back.initialize();
     //Kompass kalibrieren  --> Problem fremde Magnetfelder!
     //Comp.AutoCalibration = 1;
     short MagRawMin[3]= {-400, -400, -400};     //Gespeicherte Werte verwenden
@@ -67,8 +73,8 @@
         Comp_angle = Comp.getAngle(Comp.Mag[0], Comp.Mag[1]);
         
         // meassure dt
-        dt = GlobalTimer.read_us() - time_loop; // Zeit in us seit letzter loop
-        time_loop = GlobalTimer.read_us(); // setze aktuelle zeit f�r n�chste messung
+        dt = GlobalTimer.read_us() - time_loop; // time in us since last loop
+        time_loop = GlobalTimer.read_us();      // set new time for next measurement
         
         // calculate angles for roll, pitch an yaw
         angle[0] += (Acc_angle[0] - angle[0])/50 + Gyro_data[0] *dt/15000000.0;
@@ -77,23 +83,28 @@
         tempangle -= Gyro_data[2] *dt/15000000.0;
         
         // LCD output
-        pc.locate(10,5); // first line
-        pc.printf("Y:%2.1f %2.1fm   ", angle[2], Alt.CalcAltitude(Alt.Pressure));
-        //LCD.printf("%2.1f %2.1f %2.1f   ", Comp.RawMag[0], Comp.RawMag[1], Comp.RawMag[2]);
-        pc.locate(10,8); // second line
+        pc.locate(10,5);
+        pc.printf("dt:%d %6.1fm   ", dt, Alt.CalcAltitude(Alt.Pressure));
+        pc.locate(10,8);
         pc.printf("Roll:%6.1f Pitch:%6.1f Yaw:%6.1f    ", angle[0], angle[1], angle[2]);
-        //LCD.printf("R:%2.1f P:%2.1f  ", Comp_angle, tempangle);
-        //LCD.printf("%2.1f %2.1f %2.1f   ", Comp.Mag[0], Comp.Mag[1], Comp.Mag[2]);
+        pc.locate(10,10);
+        pc.printf("Debug_Yaw:  Comp:%6.1f Gyro:%6.1f  ", Comp_angle, tempangle);
+        pc.locate(10,12);
+        pc.printf("Comp_Raw: %6.1f %6.1f %6.1f   ", Comp.RawMag[0], Comp.RawMag[1], Comp.RawMag[2]);
+        pc.locate(10,13);
+        pc.printf("Comp_Mag: %6.1f %6.1f %6.1f   ", Comp.Mag[0], Comp.Mag[1], Comp.Mag[2]);
         
-        Motor = 1000 + 5*abs(angle[1]); // set new motor speed
+        // Read RC data
+        //RC[0].read() // TODO: RC daten lesen und einberechnen!
+        
+        // PID Regelung
         
-        //LED hin und her
-        int ledset = 0;
-        if (angle[0] < 0) ledset += 1; else ledset += 8; 
-        if (angle[1] < 0) ledset += 2; else ledset += 4;
-        LEDs = ledset;
+        // set new motor speeds
+        Motor_left = 1000 + 5*abs(angle[1]);
+        Motor_right = 1000 + 5*abs(angle[1]);
+        Motor_back = 1000 + 5*abs(angle[1]);
         
-        //LEDs.rollnext();
-        //wait(0.1);
+        LEDs.rollnext();
+        wait(0.1);
     }
 }