Markus Tschuchnik / Mbed 2 deprecated DAQuadrocopter

a

Dependencies:   mbed Brushless_DC_Lib_ MPU6050 AR-610_Lib_

main.cpp@3:9c5cecd78963, 2019-03-13 (annotated)

Committer:
lordofthestorm12
Date:
Wed Mar 13 18:23:09 2019 +0000
Revision:
3:9c5cecd78963
Parent:
2:1f0765d254d1 
haha

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lordofthestorm12 0:38cb99a6dea6 1 #include "mbed.h"
lordofthestorm12 0:38cb99a6dea6 2 #include "AR610_LIB.h"
lordofthestorm12 0:38cb99a6dea6 3 #include "Brushless_DC_LIB.h"
lordofthestorm12 0:38cb99a6dea6 4 #include "MPU6050.h"
lordofthestorm12 0:38cb99a6dea6 5
lordofthestorm12 0:38cb99a6dea6 6 Serial pc(USBTX, USBRX);
lordofthestorm12 0:38cb99a6dea6 7 BusOut LED(LED1, LED2, LED3, LED4);
lordofthestorm12 0:38cb99a6dea6 8 MPU6050 Gyro(p9, p10);
lordofthestorm12 0:38cb99a6dea6 9
lordofthestorm12 0:38cb99a6dea6 10 int main() {
lordofthestorm12 0:38cb99a6dea6 11
lordofthestorm12 0:38cb99a6dea6 12 // I. Initprocedure...
lordofthestorm12 0:38cb99a6dea6 13
lordofthestorm12 0:38cb99a6dea6 14 // I~SEC
lordofthestorm12 0:38cb99a6dea6 15
lordofthestorm12 0:38cb99a6dea6 16 bool SEC_MOTOR_FREE = false;
lordofthestorm12 0:38cb99a6dea6 17
lordofthestorm12 0:38cb99a6dea6 18 // I~a Reciever Init
lordofthestorm12 0:38cb99a6dea6 19 Receiver_obj Throttle(p15), Aile(p16),Elev(p17), Rudd(p18);
lordofthestorm12 0:38cb99a6dea6 20
lordofthestorm12 0:38cb99a6dea6 21 // I~b Brushless Init (criticle procedure, [set for 4sec on 1kppm for Motorrealease...])
lordofthestorm12 0:38cb99a6dea6 22
lordofthestorm12 2:1f0765d254d1 23 wait(2);
lordofthestorm12 0:38cb99a6dea6 24 LED = 0x06;
lordofthestorm12 2:1f0765d254d1 25 Motor_obj Brshlss(p24, p23, p22, p21);
lordofthestorm12 2:1f0765d254d1 26 wait_ms(4000);
lordofthestorm12 0:38cb99a6dea6 27
lordofthestorm12 0:38cb99a6dea6 28 // I~c Gyro Init and set Offset...
lordofthestorm12 0:38cb99a6dea6 29
lordofthestorm12 0:38cb99a6dea6 30 float GyroData[3];
lordofthestorm12 0:38cb99a6dea6 31 float G_Offset[3];
lordofthestorm12 0:38cb99a6dea6 32
lordofthestorm12 0:38cb99a6dea6 33
lordofthestorm12 0:38cb99a6dea6 34 bool Test = Gyro.testConnection();
lordofthestorm12 0:38cb99a6dea6 35 if( Test == true ){
lordofthestorm12 0:38cb99a6dea6 36 LED = 0x0f;
lordofthestorm12 0:38cb99a6dea6 37 wait(2);
lordofthestorm12 0:38cb99a6dea6 38
lordofthestorm12 0:38cb99a6dea6 39 SEC_MOTOR_FREE = true;
lordofthestorm12 0:38cb99a6dea6 40
lordofthestorm12 0:38cb99a6dea6 41 Gyro.getGyro(GyroData);
lordofthestorm12 0:38cb99a6dea6 42
lordofthestorm12 0:38cb99a6dea6 43 // GyroOffSet
lordofthestorm12 0:38cb99a6dea6 44
lordofthestorm12 0:38cb99a6dea6 45 G_Offset[0] = GyroData[0];
lordofthestorm12 0:38cb99a6dea6 46 G_Offset[1] = GyroData[1];
lordofthestorm12 0:38cb99a6dea6 47 G_Offset[2] = GyroData[2];
lordofthestorm12 0:38cb99a6dea6 48
lordofthestorm12 0:38cb99a6dea6 49 }else{
lordofthestorm12 0:38cb99a6dea6 50 SEC_MOTOR_FREE = false;
lordofthestorm12 0:38cb99a6dea6 51 Brshlss.setAllMotor(1000,1000,1000,1000);
lordofthestorm12 0:38cb99a6dea6 52 if(SEC_MOTOR_FREE == false){
lordofthestorm12 0:38cb99a6dea6 53 while(true){
lordofthestorm12 0:38cb99a6dea6 54 LED =0x0f;
lordofthestorm12 0:38cb99a6dea6 55 wait_ms(100);
lordofthestorm12 0:38cb99a6dea6 56 LED = 0x00;
lordofthestorm12 0:38cb99a6dea6 57 wait_ms(100);
lordofthestorm12 0:38cb99a6dea6 58 }
lordofthestorm12 0:38cb99a6dea6 59 }
lordofthestorm12 2:1f0765d254d1 60
lordofthestorm12 0:38cb99a6dea6 61 }
lordofthestorm12 0:38cb99a6dea6 62
lordofthestorm12 0:38cb99a6dea6 63 LED = 0x09;
lordofthestorm12 2:1f0765d254d1 64 int CurThrVal;
lordofthestorm12 0:38cb99a6dea6 65
lordofthestorm12 0:38cb99a6dea6 66
lordofthestorm12 0:38cb99a6dea6 67 while(true){
lordofthestorm12 2:1f0765d254d1 68
lordofthestorm12 2:1f0765d254d1 69 Gyro.getGyro(GyroData);
lordofthestorm12 2:1f0765d254d1 70
lordofthestorm12 2:1f0765d254d1 71 CurThrVal = Throttle.CurVal();
lordofthestorm12 2:1f0765d254d1 72 if(CurThrVal >= 1100 && CurThrVal <= 2000){
lordofthestorm12 2:1f0765d254d1 73 Brshlss.setAllMotor(CurThrVal + (GyroData[1] * 15) - (GyroData[0] * 15), CurThrVal + (GyroData[1] * 15) + (GyroData[0] * 15), CurThrVal - (GyroData[1] * 15) - (GyroData[0] * 15), CurThrVal - (GyroData[1] * 15) + (GyroData[0] * 15));
lordofthestorm12 2:1f0765d254d1 74 } else {
lordofthestorm12 2:1f0765d254d1 75 Brshlss.setAllMotor(1000, 1000, 1000, 1000);
lordofthestorm12 2:1f0765d254d1 76 }
lordofthestorm12 2:1f0765d254d1 77
lordofthestorm12 2:1f0765d254d1 78
lordofthestorm12 2:1f0765d254d1 79
lordofthestorm12 2:1f0765d254d1 80
lordofthestorm12 2:1f0765d254d1 81 /*
lordofthestorm12 2:1f0765d254d1 82
lordofthestorm12 2:1f0765d254d1 83 XY Achse Regler
lordofthestorm12 2:1f0765d254d1 84
lordofthestorm12 2:1f0765d254d1 85 Brshlss.setAllMotor(1200 + (GyroData[1] * 15) - (GyroData[0] * 15), 1200 + (GyroData[1] * 15) + (GyroData[0] * 15), 1200 - (GyroData[1] * 15) - (GyroData[0] * 15), 1200 - (GyroData[1] * 15) + (GyroData[0] * 15));
lordofthestorm12 2:1f0765d254d1 86
lordofthestorm12 2:1f0765d254d1 87 X Achse Regler
lordofthestorm12 2:1f0765d254d1 88
lordofthestorm12 2:1f0765d254d1 89 Brshlss.setAllMotor(1200 - (GyroData[0] * 15),1200 + (GyroData[0] * 15),1200 - (GyroData[0] * 15),1200 + (GyroData[0] * 15));
lordofthestorm12 2:1f0765d254d1 90
lordofthestorm12 2:1f0765d254d1 91
lordofthestorm12 2:1f0765d254d1 92 wait(0.1);
lordofthestorm12 2:1f0765d254d1 93 Gyro.getGyro(GyroData);
lordofthestorm12 2:1f0765d254d1 94 pc.printf("X: %f | Y: %f | Z: %f \n", GyroData[0], GyroData[1], GyroData[2]);
lordofthestorm12 2:1f0765d254d1 95
lordofthestorm12 2:1f0765d254d1 96
lordofthestorm12 2:1f0765d254d1 97 */
lordofthestorm12 0:38cb99a6dea6 98
lordofthestorm12 2:1f0765d254d1 99
lordofthestorm12 2:1f0765d254d1 100
lordofthestorm12 0:38cb99a6dea6 101
lordofthestorm12 0:38cb99a6dea6 102
lordofthestorm12 0:38cb99a6dea6 103 }
lordofthestorm12 0:38cb99a6dea6 104
lordofthestorm12 0:38cb99a6dea6 105 }