a
Dependencies: mbed Brushless_DC_Lib_ MPU6050 AR-610_Lib_
main.cpp@2:1f0765d254d1, 2019-03-13 (annotated)
- Committer:
- lordofthestorm12
- Date:
- Wed Mar 13 18:20:03 2019 +0000
- Revision:
- 2:1f0765d254d1
- Parent:
- 0:38cb99a6dea6
DA Quadro DiArbeit
Who changed what in which revision?
User | Revision | Line number | New 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 | } |