a
Dependencies: mbed Brushless_DC_Lib_ MPU6050 AR-610_Lib_
main.cpp
- Committer:
- lordofthestorm12
- Date:
- 2019-03-13
- Revision:
- 2:1f0765d254d1
- Parent:
- 0:38cb99a6dea6
File content as of revision 2:1f0765d254d1:
#include "mbed.h" #include "AR610_LIB.h" #include "Brushless_DC_LIB.h" #include "MPU6050.h" Serial pc(USBTX, USBRX); BusOut LED(LED1, LED2, LED3, LED4); MPU6050 Gyro(p9, p10); int main() { // I. Initprocedure... // I~SEC bool SEC_MOTOR_FREE = false; // I~a Reciever Init Receiver_obj Throttle(p15), Aile(p16),Elev(p17), Rudd(p18); // I~b Brushless Init (criticle procedure, [set for 4sec on 1kppm for Motorrealease...]) wait(2); LED = 0x06; Motor_obj Brshlss(p24, p23, p22, p21); wait_ms(4000); // I~c Gyro Init and set Offset... float GyroData[3]; float G_Offset[3]; bool Test = Gyro.testConnection(); if( Test == true ){ LED = 0x0f; wait(2); SEC_MOTOR_FREE = true; Gyro.getGyro(GyroData); // GyroOffSet G_Offset[0] = GyroData[0]; G_Offset[1] = GyroData[1]; G_Offset[2] = GyroData[2]; }else{ SEC_MOTOR_FREE = false; Brshlss.setAllMotor(1000,1000,1000,1000); if(SEC_MOTOR_FREE == false){ while(true){ LED =0x0f; wait_ms(100); LED = 0x00; wait_ms(100); } } } LED = 0x09; int CurThrVal; while(true){ Gyro.getGyro(GyroData); CurThrVal = Throttle.CurVal(); if(CurThrVal >= 1100 && CurThrVal <= 2000){ 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)); } else { Brshlss.setAllMotor(1000, 1000, 1000, 1000); } /* XY Achse Regler 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)); X Achse Regler Brshlss.setAllMotor(1200 - (GyroData[0] * 15),1200 + (GyroData[0] * 15),1200 - (GyroData[0] * 15),1200 + (GyroData[0] * 15)); wait(0.1); Gyro.getGyro(GyroData); pc.printf("X: %f | Y: %f | Z: %f \n", GyroData[0], GyroData[1], GyroData[2]); */ } }