as
Dependencies: CommonTypes ESC Matrix PID Servo kalman mbed-rtos mbed
Fork of Nucleo_MPU_9250_ by
Diff: main.cpp
- Revision:
- 0:89cf0851969b
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Tue Jun 26 18:24:45 2018 +0000 @@ -0,0 +1,119 @@ +#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); + } +} + +