Project Football / Mbed 2 deprecated Nucleo_MPU_9250_

as

Dependencies:   CommonTypes ESC Matrix PID Servo kalman mbed-rtos mbed

Fork of Nucleo_MPU_9250_ by Alan Huchin Herrera

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?

UserRevisionLine numberNew 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