Active Aerodynamics and Drag Reduction System / Mbed 2 deprecated ActiveAeroAndDRS

Source code for Active Aerodynamics and Drag Reduction System

Dependencies:   mbed Servo mbed-rtos LSM9DS1_Library_cal MPL3115A2

main.cpp@1:8e8aac99a366, 2020-04-21 (annotated)

Committer:
abir77935
Date:
Tue Apr 21 06:55:43 2020 +0000
Revision:
1:8e8aac99a366
Parent:
0:04fef978a0ab
Child:
2:426f26e9801d
added Uart code for Bluetooth interactability

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cheryldocherty 0:04fef978a0ab 1 #include "mbed.h"
cheryldocherty 0:04fef978a0ab 2 #include "rtos.h"
cheryldocherty 0:04fef978a0ab 3 #include "Servo.h"
cheryldocherty 0:04fef978a0ab 4 #include "LSM9DS1.h"
cheryldocherty 0:04fef978a0ab 5 #define PI 3.14159 // Used in IMU code
cheryldocherty 0:04fef978a0ab 6 #define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA (Used in IMU code)
cheryldocherty 0:04fef978a0ab 7
cheryldocherty 0:04fef978a0ab 8
cheryldocherty 0:04fef978a0ab 9 // OBJECTS
cheryldocherty 0:04fef978a0ab 10 Servo servoFrontLeft(p21);
cheryldocherty 0:04fef978a0ab 11 Servo servoFrontRight(p22);
cheryldocherty 0:04fef978a0ab 12 Servo servoBackLeft(p23);
cheryldocherty 0:04fef978a0ab 13 Servo servoBackRight(p24);
cheryldocherty 0:04fef978a0ab 14 LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);
cheryldocherty 0:04fef978a0ab 15 Serial blue(p28,p27); // bluetooth
cheryldocherty 0:04fef978a0ab 16 //BusOut myled(LED1,LED2,LED3,LED4); //bluetooth debugging
cheryldocherty 0:04fef978a0ab 17 Thread thread1;
cheryldocherty 0:04fef978a0ab 18 Serial pc(USBTX, USBRX); // Debugging
cheryldocherty 0:04fef978a0ab 19
cheryldocherty 0:04fef978a0ab 20
cheryldocherty 0:04fef978a0ab 21 // VARIABLE DECLARATIONS
cheryldocherty 0:04fef978a0ab 22 volatile char bluetoothRead = 0;
abir77935 1:8e8aac99a366 23 volatile float xAccel = 0;
abir77935 1:8e8aac99a366 24 volatile float yAccel = 0;
abir77935 1:8e8aac99a366 25 volatile float zAccel = 0;
abir77935 1:8e8aac99a366 26 volatile float tuning = 1;
cheryldocherty 0:04fef978a0ab 27
cheryldocherty 0:04fef978a0ab 28
cheryldocherty 0:04fef978a0ab 29 // FUNCTION DECLARATIONS
cheryldocherty 0:04fef978a0ab 30 void bluetooth_recv()
cheryldocherty 0:04fef978a0ab 31 {
cheryldocherty 0:04fef978a0ab 32 bluetoothRead = blue.getc();
cheryldocherty 0:04fef978a0ab 33 /*if (blue.getc()=='!') {
cheryldocherty 0:04fef978a0ab 34 if (blue.getc()=='B') { //button data
cheryldocherty 0:04fef978a0ab 35 bluetoothRead = blue.getc(); //button number
cheryldocherty 0:04fef978a0ab 36 if ((bluetoothRead>='1')&&(bluetoothRead<='4')) //is a number button 1..4
cheryldocherty 0:04fef978a0ab 37 myled[bluetoothRead-'1']=blue.getc()-'0'; //turn on/off that num LED
cheryldocherty 0:04fef978a0ab 38 }
cheryldocherty 0:04fef978a0ab 39 } */
cheryldocherty 0:04fef978a0ab 40 }
cheryldocherty 0:04fef978a0ab 41
cheryldocherty 0:04fef978a0ab 42 // IMU - caluclate pitch and roll
cheryldocherty 0:04fef978a0ab 43 void printAttitude(float ax, float ay, float az, float mx, float my, float mz)
cheryldocherty 0:04fef978a0ab 44 {
cheryldocherty 0:04fef978a0ab 45 float roll = atan2(ay, az);
cheryldocherty 0:04fef978a0ab 46 float pitch = atan2(-ax, sqrt(ay * ay + az * az));
cheryldocherty 0:04fef978a0ab 47 // touchy trig stuff to use arctan to get compass heading (scale is 0..360)
cheryldocherty 0:04fef978a0ab 48 mx = -mx;
cheryldocherty 0:04fef978a0ab 49 float heading;
cheryldocherty 0:04fef978a0ab 50 if (my == 0.0)
cheryldocherty 0:04fef978a0ab 51 heading = (mx < 0.0) ? 180.0 : 0.0;
cheryldocherty 0:04fef978a0ab 52 else
cheryldocherty 0:04fef978a0ab 53 heading = atan2(mx, my)*360.0/(2.0*PI);
cheryldocherty 0:04fef978a0ab 54 //pc.printf("heading atan=%f \n\r",heading);
cheryldocherty 0:04fef978a0ab 55 heading -= DECLINATION; //correct for geo location
cheryldocherty 0:04fef978a0ab 56 if(heading>180.0) heading = heading - 360.0;
cheryldocherty 0:04fef978a0ab 57 else if(heading<-180.0) heading = 360.0 + heading;
cheryldocherty 0:04fef978a0ab 58 else if(heading<0.0) heading = 360.0 + heading;
cheryldocherty 0:04fef978a0ab 59 // Convert everything from radians to degrees:
cheryldocherty 0:04fef978a0ab 60 //heading *= 180.0 / PI;
cheryldocherty 0:04fef978a0ab 61 pitch *= 180.0 / PI;
cheryldocherty 0:04fef978a0ab 62 roll *= 180.0 / PI;
cheryldocherty 0:04fef978a0ab 63 pc.printf("Pitch: %f, Roll: %f degress\n\r",pitch,roll);
cheryldocherty 0:04fef978a0ab 64 pc.printf("Magnetic Heading: %f degress\n\r",heading);
cheryldocherty 0:04fef978a0ab 65 }
cheryldocherty 0:04fef978a0ab 66
cheryldocherty 0:04fef978a0ab 67 // IMU - read and display magnetometer, gyroscope, acceleration values
cheryldocherty 0:04fef978a0ab 68 void getIMUData()
cheryldocherty 0:04fef978a0ab 69 {
cheryldocherty 0:04fef978a0ab 70 while(!IMU.magAvailable(X_AXIS));
cheryldocherty 0:04fef978a0ab 71 IMU.readMag();
cheryldocherty 0:04fef978a0ab 72 while(!IMU.accelAvailable());
cheryldocherty 0:04fef978a0ab 73 IMU.readAccel();
cheryldocherty 0:04fef978a0ab 74 while(!IMU.gyroAvailable());
cheryldocherty 0:04fef978a0ab 75 IMU.readGyro();
cheryldocherty 0:04fef978a0ab 76 pc.printf(" X axis Y axis Z axis\n\r");
cheryldocherty 0:04fef978a0ab 77 pc.printf("gyro: %9f %9f %9f in deg/s\n\r", IMU.calcGyro(IMU.gx), IMU.calcGyro(IMU.gy), IMU.calcGyro(IMU.gz));
cheryldocherty 0:04fef978a0ab 78 pc.printf("accel: %9f %9f %9f in Gs\n\r", IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az));
cheryldocherty 0:04fef978a0ab 79 pc.printf("mag: %9f %9f %9f in gauss\n\r", IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
cheryldocherty 0:04fef978a0ab 80 printAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx),
cheryldocherty 0:04fef978a0ab 81 IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
abir77935 1:8e8aac99a366 82
abir77935 1:8e8aac99a366 83 zAccel = IMU.calcAccel(IMU.az));//setting global variable for storage of z-Axis acceleration
abir77935 1:8e8aac99a366 84 yAccel = IMU.calcAccel(IMU.ay));
abir77935 1:8e8aac99a366 85 xAccel = IMU.calcAccel(IMU.ax));
cheryldocherty 0:04fef978a0ab 86 }
cheryldocherty 0:04fef978a0ab 87
cheryldocherty 0:04fef978a0ab 88
cheryldocherty 0:04fef978a0ab 89 // THREADS
cheryldocherty 0:04fef978a0ab 90 void thread1Name() {
cheryldocherty 0:04fef978a0ab 91 while (true) {
cheryldocherty 0:04fef978a0ab 92 // things
cheryldocherty 0:04fef978a0ab 93 Thread::wait(100);
cheryldocherty 0:04fef978a0ab 94 }
cheryldocherty 0:04fef978a0ab 95 }
cheryldocherty 0:04fef978a0ab 96
abir77935 1:8e8aac99a366 97 //Prints the acceraltion data to the bluetooth UART window
abir77935 1:8e8aac99a366 98 void blue_rec()
abir77935 1:8e8aac99a366 99 {
abir77935 1:8e8aac99a366 100 while(1){
abir77935 1:8e8aac99a366 101 dev.putc(zAccel);
abir77935 1:8e8aac99a366 102 Thread::wait(1000);
abir77935 1:8e8aac99a366 103 }
abir77935 1:8e8aac99a366 104 }
abir77935 1:8e8aac99a366 105
abir77935 1:8e8aac99a366 106 /*
abir77935 1:8e8aac99a366 107 //take in data from UART window on bluetooth to adjust tuning variables
abir77935 1:8e8aac99a366 108 void blue_tuning()
abir77935 1:8e8aac99a366 109 {
abir77935 1:8e8aac99a366 110 while(dev.readable()) {
abir77935 1:8e8aac99a366 111 tuning = dev.getc();
abir77935 1:8e8aac99a366 112 }
abir77935 1:8e8aac99a366 113 }
abir77935 1:8e8aac99a366 114 */
cheryldocherty 0:04fef978a0ab 115
cheryldocherty 0:04fef978a0ab 116 int main() {
cheryldocherty 0:04fef978a0ab 117 // initialise IMU
cheryldocherty 0:04fef978a0ab 118 IMU.begin();
cheryldocherty 0:04fef978a0ab 119 if (!IMU.begin()) {
cheryldocherty 0:04fef978a0ab 120 pc.printf("Failed to communicate with LSM9DS1.\n");
cheryldocherty 0:04fef978a0ab 121 }
cheryldocherty 0:04fef978a0ab 122 IMU.calibrate(1);
cheryldocherty 0:04fef978a0ab 123 IMU.calibrateMag(0);
cheryldocherty 0:04fef978a0ab 124
cheryldocherty 0:04fef978a0ab 125 // Start threads
cheryldocherty 0:04fef978a0ab 126 thread1.start(thread1Name);
cheryldocherty 0:04fef978a0ab 127
abir77935 1:8e8aac99a366 128 blue.baud(9600); //set baud rate for UART window
abir77935 1:8e8aac99a366 129 blue.attach(&blue_rec, Serial::RxIrq);
abir77935 1:8e8aac99a366 130
cheryldocherty 0:04fef978a0ab 131 while(1) {
cheryldocherty 0:04fef978a0ab 132 // things
cheryldocherty 0:04fef978a0ab 133 Thread::wait(1000);
cheryldocherty 0:04fef978a0ab 134 }
cheryldocherty 0:04fef978a0ab 135 }