Markus Tschuchnik / Mbed 2 deprecated DAQuadrocopter

a

Dependencies:   mbed Brushless_DC_Lib_ MPU6050 AR-610_Lib_

main.cpp@0:38cb99a6dea6, 2019-03-06 (annotated)

Committer:
lordofthestorm12
Date:
Wed Mar 06 17:27:22 2019 +0000
Revision:
0:38cb99a6dea6
Child:
2:1f0765d254d1
CopterQuadro hoit

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 0:38cb99a6dea6 23 LED = 0x06;
lordofthestorm12 0:38cb99a6dea6 24 Motor_obj Brshlss(p21, p22, p23, p24);
lordofthestorm12 0:38cb99a6dea6 25 Brshlss.setAllMotor(1000,1000,1000,1000);
lordofthestorm12 0:38cb99a6dea6 26 wait(2);
lordofthestorm12 0:38cb99a6dea6 27
lordofthestorm12 0:38cb99a6dea6 28 LED = 0x00;
lordofthestorm12 0:38cb99a6dea6 29
lordofthestorm12 0:38cb99a6dea6 30 // I~c Gyro Init and set Offset...
lordofthestorm12 0:38cb99a6dea6 31
lordofthestorm12 0:38cb99a6dea6 32 float GyroData[3];
lordofthestorm12 0:38cb99a6dea6 33 float G_Offset[3];
lordofthestorm12 0:38cb99a6dea6 34
lordofthestorm12 0:38cb99a6dea6 35
lordofthestorm12 0:38cb99a6dea6 36 bool Test = Gyro.testConnection();
lordofthestorm12 0:38cb99a6dea6 37 if( Test == true ){
lordofthestorm12 0:38cb99a6dea6 38 LED = 0x0f;
lordofthestorm12 0:38cb99a6dea6 39 wait(2);
lordofthestorm12 0:38cb99a6dea6 40
lordofthestorm12 0:38cb99a6dea6 41 SEC_MOTOR_FREE = true;
lordofthestorm12 0:38cb99a6dea6 42
lordofthestorm12 0:38cb99a6dea6 43 Gyro.getGyro(GyroData);
lordofthestorm12 0:38cb99a6dea6 44
lordofthestorm12 0:38cb99a6dea6 45 // GyroOffSet
lordofthestorm12 0:38cb99a6dea6 46
lordofthestorm12 0:38cb99a6dea6 47 G_Offset[0] = GyroData[0];
lordofthestorm12 0:38cb99a6dea6 48 G_Offset[1] = GyroData[1];
lordofthestorm12 0:38cb99a6dea6 49 G_Offset[2] = GyroData[2];
lordofthestorm12 0:38cb99a6dea6 50
lordofthestorm12 0:38cb99a6dea6 51 }else{
lordofthestorm12 0:38cb99a6dea6 52 SEC_MOTOR_FREE = false;
lordofthestorm12 0:38cb99a6dea6 53 Brshlss.setAllMotor(1000,1000,1000,1000);
lordofthestorm12 0:38cb99a6dea6 54 if(SEC_MOTOR_FREE == false){
lordofthestorm12 0:38cb99a6dea6 55 while(true){
lordofthestorm12 0:38cb99a6dea6 56 LED =0x0f;
lordofthestorm12 0:38cb99a6dea6 57 wait_ms(100);
lordofthestorm12 0:38cb99a6dea6 58 LED = 0x00;
lordofthestorm12 0:38cb99a6dea6 59 wait_ms(100);
lordofthestorm12 0:38cb99a6dea6 60 }
lordofthestorm12 0:38cb99a6dea6 61 }
lordofthestorm12 0:38cb99a6dea6 62
lordofthestorm12 0:38cb99a6dea6 63 }
lordofthestorm12 0:38cb99a6dea6 64
lordofthestorm12 0:38cb99a6dea6 65 LED = 0x09;
lordofthestorm12 0:38cb99a6dea6 66
lordofthestorm12 0:38cb99a6dea6 67 // Variaben
lordofthestorm12 0:38cb99a6dea6 68 int throttleVal;
lordofthestorm12 0:38cb99a6dea6 69 int corval = 20;
lordofthestorm12 0:38cb99a6dea6 70
lordofthestorm12 0:38cb99a6dea6 71
lordofthestorm12 0:38cb99a6dea6 72 while(true){
lordofthestorm12 0:38cb99a6dea6 73
lordofthestorm12 0:38cb99a6dea6 74 Brshlss.setAllMotor(Throttle.CurVal(),Throttle.CurVal(),Throttle.CurVal(),Throttle.CurVal());
lordofthestorm12 0:38cb99a6dea6 75 pc.printf("%d", Throttle.CurVal());
lordofthestorm12 0:38cb99a6dea6 76 wait(1);
lordofthestorm12 0:38cb99a6dea6 77
lordofthestorm12 0:38cb99a6dea6 78 /*
lordofthestorm12 0:38cb99a6dea6 79
lordofthestorm12 0:38cb99a6dea6 80 LED = 0x01;
lordofthestorm12 0:38cb99a6dea6 81 Brshlss.setAllMotor(1300,0,0,0);
lordofthestorm12 0:38cb99a6dea6 82 wait(1);
lordofthestorm12 0:38cb99a6dea6 83 LED = 0x02;
lordofthestorm12 0:38cb99a6dea6 84 Brshlss.setAllMotor(0,1300,0,0);
lordofthestorm12 0:38cb99a6dea6 85 wait(1);
lordofthestorm12 0:38cb99a6dea6 86 LED = 0x04;
lordofthestorm12 0:38cb99a6dea6 87 Brshlss.setAllMotor(0,0,1300,0);
lordofthestorm12 0:38cb99a6dea6 88 wait(1);
lordofthestorm12 0:38cb99a6dea6 89 LED = 0x08;
lordofthestorm12 0:38cb99a6dea6 90 Brshlss.setAllMotor(0,0,0,1300);
lordofthestorm12 0:38cb99a6dea6 91 wait(1);
lordofthestorm12 0:38cb99a6dea6 92
lordofthestorm12 0:38cb99a6dea6 93
lordofthestorm12 0:38cb99a6dea6 94
lordofthestorm12 0:38cb99a6dea6 95 Gyro.getGyro(GyroData);
lordofthestorm12 0:38cb99a6dea6 96 throttleVal = Throttle.CurVal();
lordofthestorm12 0:38cb99a6dea6 97
lordofthestorm12 0:38cb99a6dea6 98 Brshlss.setAllMotor(throttleVal + (int)(GyroData[1] * corval),throttleVal + (int)(GyroData[1] * corval) , throttleVal, throttleVal);
lordofthestorm12 0:38cb99a6dea6 99
lordofthestorm12 0:38cb99a6dea6 100 pc.printf(" MC: %d %d %d %d || GY: %f %d %d \n" , Brshlss.getMotorVal(LeftFront), Brshlss.getMotorVal(RightFront), Brshlss.getMotorVal(LeftBack), Brshlss.getMotorVal(RightBack), GyroData[0], (int)(GyroData[0] * corval),
lordofthestorm12 0:38cb99a6dea6 101 throttleVal );
lordofthestorm12 0:38cb99a6dea6 102 */
lordofthestorm12 0:38cb99a6dea6 103
lordofthestorm12 0:38cb99a6dea6 104 }
lordofthestorm12 0:38cb99a6dea6 105
lordofthestorm12 0:38cb99a6dea6 106 }