Quadrirotor
Dependencies: CommonTypes ESC Matrix PID Servo kalman mbed-rtos mbed
Fork of Nucleo_MPU_9250 by
main.cpp
- Committer:
- AlanHuchin
- Date:
- 2018-06-26
- Revision:
- 0:89cf0851969b
File content as of revision 0:89cf0851969b:
#include "mbed.h" #include "math.h" #include "rtos.h" #include "Servo.h" #include "CID10DOF.h" #include <string> #include "stdio.h" #include "MathFuncs.h" #define double_SIZE sizeof(double) Serial pc(USBTX, USBRX); Serial telem(PC_0, PC_1); CID10DOF CID10DOF(D14,D15,D2,D4,D5,D7); AnalogIn Analog_I(A0); DigitalOut led1(D6); DigitalOut led2(D3); Timer t; double bias[3] = {0.0,0.0,0.0}, setpoint[3]= {0.0,0.0,0.0}, speed[4], actSpeed[4]; double ypr[4],M[5],Diff[3]; void getData(void const *argument); void PID(void const *argument); void motors(void const *argument); int main() { pc.baud(115200); //telem.baud(57600); led2 = !led2; CID10DOF.MPU9250init(); CID10DOF.PID_Config(setpoint, bias); CID10DOF.Calibrate_Motors(); for (int i = 0; i < 4; i++){ //initialise speed to be 0.3 speed[i] = MIN_TRHUST; } led2 = !led2; Thread thread3(getData, NULL, osPriorityRealtime, DEFAULT_STACK_SIZE); Thread thread4(PID, NULL, osPriorityRealtime, DEFAULT_STACK_SIZE); Thread thread5(motors, NULL, osPriorityRealtime, DEFAULT_STACK_SIZE); t.start(); while(1) { led1 = !led1; Thread::wait(17); //pc.printf("Y:%f,P:%f,R:%f,PID_P:%.5f,PID_R:%.5f,M1:%f,M2:%f \n\r",ypr[0],ypr[1],ypr[2],Diff[1],Diff[2],actSpeed[0],actSpeed[1]); pc.printf("%f, %f, %f, %f, %f, %f\n\r",ypr[1],ypr[2],0.0,t.read(),ypr[1]-setpoint[1],setpoint[1]); if(t>10){ setpoint[1] = 5.0; } if(t>15){ setpoint[1] = 0.0; } if(t>20){ setpoint[1] = -5; } if(t>25){ setpoint[1] = 0.0; } } } void PID(void const *argument) { while(true){ CID10DOF.PID_Run(ypr, setpoint, Diff); actSpeed[0] = MIN_TRHUST - Diff[2] ; actSpeed[1] = MIN_TRHUST + Diff[2] ; if(actSpeed[0]< MIN_TRHUST){actSpeed[0] = MIN_TRHUST;} if(actSpeed[0]> 1.0){actSpeed[0] = MAX_TRHUST;} if(actSpeed[1]< MIN_TRHUST){actSpeed[1] = MIN_TRHUST;} if(actSpeed[1]> 1.0){actSpeed[1] = MAX_TRHUST;} Thread::wait(10); } } void getData(void const *argument) { while(true){ CID10DOF.MPU9250ReadAxis(ypr); Thread::wait(5); } } void motors(void const *argument) { while(true){ CID10DOF.run (actSpeed); Thread::wait(20); } }