NOT FINISHED YET!!! My first try to get a self built fully working Quadrocopter based on an mbed, a self built frame and some other more or less cheap parts.
main.cpp@7:9d4313510646, 2012-10-15 (annotated)
- Committer:
- maetugr
- Date:
- Mon Oct 15 17:23:06 2012 +0000
- Revision:
- 7:9d4313510646
- Parent:
- 6:179752756e9f
- Child:
- 8:d25ecdcdbeb5
RC vorgelegt und vor Tickereinbau
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
maetugr | 7:9d4313510646 | 1 | #include "mbed.h" // Standard Library |
maetugr | 7:9d4313510646 | 2 | #include "LED.h" // LEDs framework for blinking ;) |
maetugr | 7:9d4313510646 | 3 | #include "L3G4200D.h" // Gyro (Gyroscope) |
maetugr | 7:9d4313510646 | 4 | #include "ADXL345.h" // Acc (Accelerometer) |
maetugr | 7:9d4313510646 | 5 | #include "HMC5883.h" // Comp (Compass) |
maetugr | 7:9d4313510646 | 6 | #include "BMP085.h" // Alt (Altitude sensor) |
maetugr | 7:9d4313510646 | 7 | #include "RC_Channel.h" // RemoteControl Chnnels with PPM |
maetugr | 7:9d4313510646 | 8 | #include "Servo.h" // Motor PPM |
maetugr | 7:9d4313510646 | 9 | #include "PC.h" // Serial Port via USB for debugging |
maetugr | 0:0c4fafa398b4 | 10 | |
maetugr | 2:93f703d2c4d7 | 11 | Timer GlobalTimer; |
maetugr | 2:93f703d2c4d7 | 12 | |
maetugr | 5:818c0668fd2d | 13 | // initialisation of hardware |
maetugr | 5:818c0668fd2d | 14 | LED LEDs; |
maetugr | 5:818c0668fd2d | 15 | L3G4200D Gyro(p28, p27); |
maetugr | 5:818c0668fd2d | 16 | ADXL345 Acc(p28, p27); |
maetugr | 5:818c0668fd2d | 17 | HMC5883 Comp(p28, p27, GlobalTimer); |
maetugr | 5:818c0668fd2d | 18 | BMP085 Alt(p28, p27); |
maetugr | 7:9d4313510646 | 19 | //Servo Motor_front(p18); |
maetugr | 7:9d4313510646 | 20 | Servo Motor_left(p17); |
maetugr | 7:9d4313510646 | 21 | Servo Motor_right(p19); |
maetugr | 7:9d4313510646 | 22 | Servo Motor_back(p20); |
maetugr | 5:818c0668fd2d | 23 | |
maetugr | 7:9d4313510646 | 24 | PC pc(USBTX, USBRX); //Achtung: Treiber auf PC fuer Serial-mbed installieren. hier herunterladen https://mbed.org/media/downloads/drivers/mbedWinSerial_16466.exe |
maetugr | 5:818c0668fd2d | 25 | |
maetugr | 2:93f703d2c4d7 | 26 | #define PI 3.1415926535897932384626433832795 |
maetugr | 2:93f703d2c4d7 | 27 | #define Rad2Deg 57.295779513082320876798154814105 |
maetugr | 2:93f703d2c4d7 | 28 | |
maetugr | 0:0c4fafa398b4 | 29 | int main() { |
maetugr | 6:179752756e9f | 30 | pc.baud(57600); |
maetugr | 6:179752756e9f | 31 | pc.cls(); |
maetugr | 6:179752756e9f | 32 | // init screen |
maetugr | 6:179752756e9f | 33 | pc.locate(10,5); |
maetugr | 6:179752756e9f | 34 | pc.printf("Flybed v0.2"); |
maetugr | 1:5a64632b1eb9 | 35 | LEDs.roll(2); |
maetugr | 5:818c0668fd2d | 36 | |
maetugr | 5:818c0668fd2d | 37 | // variables for loop |
maetugr | 5:818c0668fd2d | 38 | float Gyro_data[3]; |
maetugr | 5:818c0668fd2d | 39 | int Acc_data[3]; |
maetugr | 5:818c0668fd2d | 40 | unsigned long dt = 0; |
maetugr | 5:818c0668fd2d | 41 | unsigned long time_loop = 0; |
maetugr | 5:818c0668fd2d | 42 | float angle[3] = {0,0,0}; |
maetugr | 5:818c0668fd2d | 43 | float Acc_angle[2] = {0,0}; |
maetugr | 5:818c0668fd2d | 44 | float Comp_angle = 0; |
maetugr | 5:818c0668fd2d | 45 | float tempangle = 0; |
maetugr | 0:0c4fafa398b4 | 46 | |
maetugr | 7:9d4313510646 | 47 | Motor_left.initialize(); |
maetugr | 7:9d4313510646 | 48 | Motor_right.initialize(); |
maetugr | 7:9d4313510646 | 49 | Motor_back.initialize(); |
maetugr | 5:818c0668fd2d | 50 | //Kompass kalibrieren --> Problem fremde Magnetfelder! |
maetugr | 5:818c0668fd2d | 51 | //Comp.AutoCalibration = 1; |
maetugr | 5:818c0668fd2d | 52 | short MagRawMin[3]= {-400, -400, -400}; //Gespeicherte Werte verwenden |
maetugr | 5:818c0668fd2d | 53 | short MagRawMax[3]= {400, 400, 400}; |
maetugr | 5:818c0668fd2d | 54 | Comp.Calibrate(MagRawMin, MagRawMax); |
maetugr | 5:818c0668fd2d | 55 | //Comp.Calibrate(20); |
maetugr | 2:93f703d2c4d7 | 56 | |
maetugr | 5:818c0668fd2d | 57 | //Oversampling des Barometers setzen |
maetugr | 5:818c0668fd2d | 58 | Alt.oss = 0; |
maetugr | 4:e96b16ad986d | 59 | |
maetugr | 2:93f703d2c4d7 | 60 | GlobalTimer.start(); |
maetugr | 0:0c4fafa398b4 | 61 | while(1) { |
maetugr | 5:818c0668fd2d | 62 | // read data from sensors |
maetugr | 0:0c4fafa398b4 | 63 | Gyro.read(Gyro_data); |
maetugr | 2:93f703d2c4d7 | 64 | Acc.read(Acc_data); |
maetugr | 5:818c0668fd2d | 65 | Comp.Update(); |
maetugr | 5:818c0668fd2d | 66 | Alt.Update(); |
maetugr | 3:a97f1d874f4e | 67 | |
maetugr | 5:818c0668fd2d | 68 | // calculate the angles for roll and pitch from accelerometer |
maetugr | 4:e96b16ad986d | 69 | Acc_angle[0] = Rad2Deg * atan2((float)Acc_data[1], (float)Acc_data[2]); |
maetugr | 5:818c0668fd2d | 70 | Acc_angle[1] = 3 -Rad2Deg * atan2((float)Acc_data[0], (float)Acc_data[2]); // TODO Offset accelerometer einstellen |
maetugr | 5:818c0668fd2d | 71 | |
maetugr | 5:818c0668fd2d | 72 | //calculate angle for yaw from compass |
maetugr | 5:818c0668fd2d | 73 | Comp_angle = Comp.getAngle(Comp.Mag[0], Comp.Mag[1]); |
maetugr | 2:93f703d2c4d7 | 74 | |
maetugr | 5:818c0668fd2d | 75 | // meassure dt |
maetugr | 7:9d4313510646 | 76 | dt = GlobalTimer.read_us() - time_loop; // time in us since last loop |
maetugr | 7:9d4313510646 | 77 | time_loop = GlobalTimer.read_us(); // set new time for next measurement |
maetugr | 2:93f703d2c4d7 | 78 | |
maetugr | 5:818c0668fd2d | 79 | // calculate angles for roll, pitch an yaw |
maetugr | 4:e96b16ad986d | 80 | angle[0] += (Acc_angle[0] - angle[0])/50 + Gyro_data[0] *dt/15000000.0; |
maetugr | 4:e96b16ad986d | 81 | angle[1] += (Acc_angle[1] - angle[1])/50 + Gyro_data[1] *dt/15000000.0; |
maetugr | 5:818c0668fd2d | 82 | angle[2] += (Comp_angle - angle[2])/50 - Gyro_data[2] *dt/15000000.0; |
maetugr | 5:818c0668fd2d | 83 | tempangle -= Gyro_data[2] *dt/15000000.0; |
maetugr | 3:a97f1d874f4e | 84 | |
maetugr | 5:818c0668fd2d | 85 | // LCD output |
maetugr | 7:9d4313510646 | 86 | pc.locate(10,5); |
maetugr | 7:9d4313510646 | 87 | pc.printf("dt:%d %6.1fm ", dt, Alt.CalcAltitude(Alt.Pressure)); |
maetugr | 7:9d4313510646 | 88 | pc.locate(10,8); |
maetugr | 6:179752756e9f | 89 | pc.printf("Roll:%6.1f Pitch:%6.1f Yaw:%6.1f ", angle[0], angle[1], angle[2]); |
maetugr | 7:9d4313510646 | 90 | pc.locate(10,10); |
maetugr | 7:9d4313510646 | 91 | pc.printf("Debug_Yaw: Comp:%6.1f Gyro:%6.1f ", Comp_angle, tempangle); |
maetugr | 7:9d4313510646 | 92 | pc.locate(10,12); |
maetugr | 7:9d4313510646 | 93 | pc.printf("Comp_Raw: %6.1f %6.1f %6.1f ", Comp.RawMag[0], Comp.RawMag[1], Comp.RawMag[2]); |
maetugr | 7:9d4313510646 | 94 | pc.locate(10,13); |
maetugr | 7:9d4313510646 | 95 | pc.printf("Comp_Mag: %6.1f %6.1f %6.1f ", Comp.Mag[0], Comp.Mag[1], Comp.Mag[2]); |
maetugr | 2:93f703d2c4d7 | 96 | |
maetugr | 7:9d4313510646 | 97 | // Read RC data |
maetugr | 7:9d4313510646 | 98 | //RC[0].read() // TODO: RC daten lesen und einberechnen! |
maetugr | 7:9d4313510646 | 99 | |
maetugr | 7:9d4313510646 | 100 | // PID Regelung |
maetugr | 0:0c4fafa398b4 | 101 | |
maetugr | 7:9d4313510646 | 102 | // set new motor speeds |
maetugr | 7:9d4313510646 | 103 | Motor_left = 1000 + 5*abs(angle[1]); |
maetugr | 7:9d4313510646 | 104 | Motor_right = 1000 + 5*abs(angle[1]); |
maetugr | 7:9d4313510646 | 105 | Motor_back = 1000 + 5*abs(angle[1]); |
maetugr | 0:0c4fafa398b4 | 106 | |
maetugr | 7:9d4313510646 | 107 | LEDs.rollnext(); |
maetugr | 7:9d4313510646 | 108 | wait(0.1); |
maetugr | 0:0c4fafa398b4 | 109 | } |
maetugr | 0:0c4fafa398b4 | 110 | } |