as
Dependencies: CommonTypes ESC Matrix PID Servo kalman mbed-rtos mbed
Fork of Nucleo_MPU_9250_ by
main.cpp@0:89cf0851969b, 2018-06-26 (annotated)
- Committer:
- AlanHuchin
- Date:
- Tue Jun 26 18:24:45 2018 +0000
- Revision:
- 0:89cf0851969b
hello
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
AlanHuchin | 0:89cf0851969b | 1 | #include "mbed.h" |
AlanHuchin | 0:89cf0851969b | 2 | #include "math.h" |
AlanHuchin | 0:89cf0851969b | 3 | #include "rtos.h" |
AlanHuchin | 0:89cf0851969b | 4 | #include "Servo.h" |
AlanHuchin | 0:89cf0851969b | 5 | #include "CID10DOF.h" |
AlanHuchin | 0:89cf0851969b | 6 | #include <string> |
AlanHuchin | 0:89cf0851969b | 7 | #include "stdio.h" |
AlanHuchin | 0:89cf0851969b | 8 | #include "MathFuncs.h" |
AlanHuchin | 0:89cf0851969b | 9 | |
AlanHuchin | 0:89cf0851969b | 10 | |
AlanHuchin | 0:89cf0851969b | 11 | #define double_SIZE sizeof(double) |
AlanHuchin | 0:89cf0851969b | 12 | |
AlanHuchin | 0:89cf0851969b | 13 | |
AlanHuchin | 0:89cf0851969b | 14 | Serial pc(USBTX, USBRX); |
AlanHuchin | 0:89cf0851969b | 15 | Serial telem(PC_0, PC_1); |
AlanHuchin | 0:89cf0851969b | 16 | CID10DOF CID10DOF(D14,D15,D2,D4,D5,D7); |
AlanHuchin | 0:89cf0851969b | 17 | AnalogIn Analog_I(A0); |
AlanHuchin | 0:89cf0851969b | 18 | DigitalOut led1(D6); |
AlanHuchin | 0:89cf0851969b | 19 | DigitalOut led2(D3); |
AlanHuchin | 0:89cf0851969b | 20 | Timer t; |
AlanHuchin | 0:89cf0851969b | 21 | |
AlanHuchin | 0:89cf0851969b | 22 | double bias[3] = {0.0,0.0,0.0}, setpoint[3]= {0.0,0.0,0.0}, speed[4], actSpeed[4]; |
AlanHuchin | 0:89cf0851969b | 23 | double ypr[4],M[5],Diff[3]; |
AlanHuchin | 0:89cf0851969b | 24 | |
AlanHuchin | 0:89cf0851969b | 25 | |
AlanHuchin | 0:89cf0851969b | 26 | void getData(void const *argument); |
AlanHuchin | 0:89cf0851969b | 27 | void PID(void const *argument); |
AlanHuchin | 0:89cf0851969b | 28 | void motors(void const *argument); |
AlanHuchin | 0:89cf0851969b | 29 | |
AlanHuchin | 0:89cf0851969b | 30 | |
AlanHuchin | 0:89cf0851969b | 31 | int main() { |
AlanHuchin | 0:89cf0851969b | 32 | |
AlanHuchin | 0:89cf0851969b | 33 | pc.baud(115200); |
AlanHuchin | 0:89cf0851969b | 34 | //telem.baud(57600); |
AlanHuchin | 0:89cf0851969b | 35 | |
AlanHuchin | 0:89cf0851969b | 36 | led2 = !led2; |
AlanHuchin | 0:89cf0851969b | 37 | |
AlanHuchin | 0:89cf0851969b | 38 | CID10DOF.MPU9250init(); |
AlanHuchin | 0:89cf0851969b | 39 | CID10DOF.PID_Config(setpoint, bias); |
AlanHuchin | 0:89cf0851969b | 40 | CID10DOF.Calibrate_Motors(); |
AlanHuchin | 0:89cf0851969b | 41 | |
AlanHuchin | 0:89cf0851969b | 42 | for (int i = 0; i < 4; i++){ //initialise speed to be 0.3 |
AlanHuchin | 0:89cf0851969b | 43 | speed[i] = MIN_TRHUST; |
AlanHuchin | 0:89cf0851969b | 44 | } |
AlanHuchin | 0:89cf0851969b | 45 | |
AlanHuchin | 0:89cf0851969b | 46 | led2 = !led2; |
AlanHuchin | 0:89cf0851969b | 47 | |
AlanHuchin | 0:89cf0851969b | 48 | Thread thread3(getData, NULL, osPriorityRealtime, DEFAULT_STACK_SIZE); |
AlanHuchin | 0:89cf0851969b | 49 | Thread thread4(PID, NULL, osPriorityRealtime, DEFAULT_STACK_SIZE); |
AlanHuchin | 0:89cf0851969b | 50 | Thread thread5(motors, NULL, osPriorityRealtime, DEFAULT_STACK_SIZE); |
AlanHuchin | 0:89cf0851969b | 51 | |
AlanHuchin | 0:89cf0851969b | 52 | t.start(); |
AlanHuchin | 0:89cf0851969b | 53 | |
AlanHuchin | 0:89cf0851969b | 54 | while(1) { |
AlanHuchin | 0:89cf0851969b | 55 | |
AlanHuchin | 0:89cf0851969b | 56 | led1 = !led1; |
AlanHuchin | 0:89cf0851969b | 57 | Thread::wait(17); |
AlanHuchin | 0:89cf0851969b | 58 | //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]); |
AlanHuchin | 0:89cf0851969b | 59 | pc.printf("%f, %f, %f, %f, %f, %f\n\r",ypr[1],ypr[2],0.0,t.read(),ypr[1]-setpoint[1],setpoint[1]); |
AlanHuchin | 0:89cf0851969b | 60 | |
AlanHuchin | 0:89cf0851969b | 61 | if(t>10){ |
AlanHuchin | 0:89cf0851969b | 62 | setpoint[1] = 5.0; |
AlanHuchin | 0:89cf0851969b | 63 | } |
AlanHuchin | 0:89cf0851969b | 64 | |
AlanHuchin | 0:89cf0851969b | 65 | if(t>15){ |
AlanHuchin | 0:89cf0851969b | 66 | setpoint[1] = 0.0; |
AlanHuchin | 0:89cf0851969b | 67 | } |
AlanHuchin | 0:89cf0851969b | 68 | |
AlanHuchin | 0:89cf0851969b | 69 | if(t>20){ |
AlanHuchin | 0:89cf0851969b | 70 | setpoint[1] = -5; |
AlanHuchin | 0:89cf0851969b | 71 | } |
AlanHuchin | 0:89cf0851969b | 72 | |
AlanHuchin | 0:89cf0851969b | 73 | if(t>25){ |
AlanHuchin | 0:89cf0851969b | 74 | setpoint[1] = 0.0; |
AlanHuchin | 0:89cf0851969b | 75 | } |
AlanHuchin | 0:89cf0851969b | 76 | |
AlanHuchin | 0:89cf0851969b | 77 | } |
AlanHuchin | 0:89cf0851969b | 78 | |
AlanHuchin | 0:89cf0851969b | 79 | } |
AlanHuchin | 0:89cf0851969b | 80 | |
AlanHuchin | 0:89cf0851969b | 81 | |
AlanHuchin | 0:89cf0851969b | 82 | void PID(void const *argument) |
AlanHuchin | 0:89cf0851969b | 83 | { |
AlanHuchin | 0:89cf0851969b | 84 | while(true){ |
AlanHuchin | 0:89cf0851969b | 85 | |
AlanHuchin | 0:89cf0851969b | 86 | CID10DOF.PID_Run(ypr, setpoint, Diff); |
AlanHuchin | 0:89cf0851969b | 87 | |
AlanHuchin | 0:89cf0851969b | 88 | actSpeed[0] = MIN_TRHUST - Diff[2] ; |
AlanHuchin | 0:89cf0851969b | 89 | actSpeed[1] = MIN_TRHUST + Diff[2] ; |
AlanHuchin | 0:89cf0851969b | 90 | |
AlanHuchin | 0:89cf0851969b | 91 | if(actSpeed[0]< MIN_TRHUST){actSpeed[0] = MIN_TRHUST;} |
AlanHuchin | 0:89cf0851969b | 92 | if(actSpeed[0]> 1.0){actSpeed[0] = MAX_TRHUST;} |
AlanHuchin | 0:89cf0851969b | 93 | if(actSpeed[1]< MIN_TRHUST){actSpeed[1] = MIN_TRHUST;} |
AlanHuchin | 0:89cf0851969b | 94 | if(actSpeed[1]> 1.0){actSpeed[1] = MAX_TRHUST;} |
AlanHuchin | 0:89cf0851969b | 95 | Thread::wait(10); |
AlanHuchin | 0:89cf0851969b | 96 | } |
AlanHuchin | 0:89cf0851969b | 97 | } |
AlanHuchin | 0:89cf0851969b | 98 | |
AlanHuchin | 0:89cf0851969b | 99 | |
AlanHuchin | 0:89cf0851969b | 100 | |
AlanHuchin | 0:89cf0851969b | 101 | void getData(void const *argument) |
AlanHuchin | 0:89cf0851969b | 102 | { |
AlanHuchin | 0:89cf0851969b | 103 | while(true){ |
AlanHuchin | 0:89cf0851969b | 104 | |
AlanHuchin | 0:89cf0851969b | 105 | CID10DOF.MPU9250ReadAxis(ypr); |
AlanHuchin | 0:89cf0851969b | 106 | Thread::wait(5); |
AlanHuchin | 0:89cf0851969b | 107 | } |
AlanHuchin | 0:89cf0851969b | 108 | } |
AlanHuchin | 0:89cf0851969b | 109 | |
AlanHuchin | 0:89cf0851969b | 110 | void motors(void const *argument) |
AlanHuchin | 0:89cf0851969b | 111 | { |
AlanHuchin | 0:89cf0851969b | 112 | while(true){ |
AlanHuchin | 0:89cf0851969b | 113 | |
AlanHuchin | 0:89cf0851969b | 114 | CID10DOF.run (actSpeed); |
AlanHuchin | 0:89cf0851969b | 115 | Thread::wait(20); |
AlanHuchin | 0:89cf0851969b | 116 | } |
AlanHuchin | 0:89cf0851969b | 117 | } |
AlanHuchin | 0:89cf0851969b | 118 | |
AlanHuchin | 0:89cf0851969b | 119 |