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@0:04fef978a0ab, 2020-04-13 (annotated)

Committer:
cheryldocherty
Date:
Mon Apr 13 14:04:34 2020 +0000
Revision:
0:04fef978a0ab
Child:
1:8e8aac99a366
libraries and functions only

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;
cheryldocherty 0:04fef978a0ab 23
cheryldocherty 0:04fef978a0ab 24
cheryldocherty 0:04fef978a0ab 25 // FUNCTION DECLARATIONS
cheryldocherty 0:04fef978a0ab 26 void bluetooth_recv()
cheryldocherty 0:04fef978a0ab 27 {
cheryldocherty 0:04fef978a0ab 28 bluetoothRead = blue.getc();
cheryldocherty 0:04fef978a0ab 29 /*if (blue.getc()=='!') {
cheryldocherty 0:04fef978a0ab 30 if (blue.getc()=='B') { //button data
cheryldocherty 0:04fef978a0ab 31 bluetoothRead = blue.getc(); //button number
cheryldocherty 0:04fef978a0ab 32 if ((bluetoothRead>='1')&&(bluetoothRead<='4')) //is a number button 1..4
cheryldocherty 0:04fef978a0ab 33 myled[bluetoothRead-'1']=blue.getc()-'0'; //turn on/off that num LED
cheryldocherty 0:04fef978a0ab 34 }
cheryldocherty 0:04fef978a0ab 35 } */
cheryldocherty 0:04fef978a0ab 36 }
cheryldocherty 0:04fef978a0ab 37
cheryldocherty 0:04fef978a0ab 38 // IMU - caluclate pitch and roll
cheryldocherty 0:04fef978a0ab 39 void printAttitude(float ax, float ay, float az, float mx, float my, float mz)
cheryldocherty 0:04fef978a0ab 40 {
cheryldocherty 0:04fef978a0ab 41 float roll = atan2(ay, az);
cheryldocherty 0:04fef978a0ab 42 float pitch = atan2(-ax, sqrt(ay * ay + az * az));
cheryldocherty 0:04fef978a0ab 43 // touchy trig stuff to use arctan to get compass heading (scale is 0..360)
cheryldocherty 0:04fef978a0ab 44 mx = -mx;
cheryldocherty 0:04fef978a0ab 45 float heading;
cheryldocherty 0:04fef978a0ab 46 if (my == 0.0)
cheryldocherty 0:04fef978a0ab 47 heading = (mx < 0.0) ? 180.0 : 0.0;
cheryldocherty 0:04fef978a0ab 48 else
cheryldocherty 0:04fef978a0ab 49 heading = atan2(mx, my)*360.0/(2.0*PI);
cheryldocherty 0:04fef978a0ab 50 //pc.printf("heading atan=%f \n\r",heading);
cheryldocherty 0:04fef978a0ab 51 heading -= DECLINATION; //correct for geo location
cheryldocherty 0:04fef978a0ab 52 if(heading>180.0) heading = heading - 360.0;
cheryldocherty 0:04fef978a0ab 53 else if(heading<-180.0) heading = 360.0 + heading;
cheryldocherty 0:04fef978a0ab 54 else if(heading<0.0) heading = 360.0 + heading;
cheryldocherty 0:04fef978a0ab 55 // Convert everything from radians to degrees:
cheryldocherty 0:04fef978a0ab 56 //heading *= 180.0 / PI;
cheryldocherty 0:04fef978a0ab 57 pitch *= 180.0 / PI;
cheryldocherty 0:04fef978a0ab 58 roll *= 180.0 / PI;
cheryldocherty 0:04fef978a0ab 59 pc.printf("Pitch: %f, Roll: %f degress\n\r",pitch,roll);
cheryldocherty 0:04fef978a0ab 60 pc.printf("Magnetic Heading: %f degress\n\r",heading);
cheryldocherty 0:04fef978a0ab 61 }
cheryldocherty 0:04fef978a0ab 62
cheryldocherty 0:04fef978a0ab 63 // IMU - read and display magnetometer, gyroscope, acceleration values
cheryldocherty 0:04fef978a0ab 64 void getIMUData()
cheryldocherty 0:04fef978a0ab 65 {
cheryldocherty 0:04fef978a0ab 66 while(!IMU.magAvailable(X_AXIS));
cheryldocherty 0:04fef978a0ab 67 IMU.readMag();
cheryldocherty 0:04fef978a0ab 68 while(!IMU.accelAvailable());
cheryldocherty 0:04fef978a0ab 69 IMU.readAccel();
cheryldocherty 0:04fef978a0ab 70 while(!IMU.gyroAvailable());
cheryldocherty 0:04fef978a0ab 71 IMU.readGyro();
cheryldocherty 0:04fef978a0ab 72 pc.printf(" X axis Y axis Z axis\n\r");
cheryldocherty 0:04fef978a0ab 73 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 74 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 75 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 76 printAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx),
cheryldocherty 0:04fef978a0ab 77 IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
cheryldocherty 0:04fef978a0ab 78 }
cheryldocherty 0:04fef978a0ab 79
cheryldocherty 0:04fef978a0ab 80
cheryldocherty 0:04fef978a0ab 81 // THREADS
cheryldocherty 0:04fef978a0ab 82 void thread1Name() {
cheryldocherty 0:04fef978a0ab 83 while (true) {
cheryldocherty 0:04fef978a0ab 84 // things
cheryldocherty 0:04fef978a0ab 85 Thread::wait(100);
cheryldocherty 0:04fef978a0ab 86 }
cheryldocherty 0:04fef978a0ab 87 }
cheryldocherty 0:04fef978a0ab 88
cheryldocherty 0:04fef978a0ab 89
cheryldocherty 0:04fef978a0ab 90 int main() {
cheryldocherty 0:04fef978a0ab 91 // initialise IMU
cheryldocherty 0:04fef978a0ab 92 IMU.begin();
cheryldocherty 0:04fef978a0ab 93 if (!IMU.begin()) {
cheryldocherty 0:04fef978a0ab 94 pc.printf("Failed to communicate with LSM9DS1.\n");
cheryldocherty 0:04fef978a0ab 95 }
cheryldocherty 0:04fef978a0ab 96 IMU.calibrate(1);
cheryldocherty 0:04fef978a0ab 97 IMU.calibrateMag(0);
cheryldocherty 0:04fef978a0ab 98
cheryldocherty 0:04fef978a0ab 99 // Start threads
cheryldocherty 0:04fef978a0ab 100 thread1.start(thread1Name);
cheryldocherty 0:04fef978a0ab 101
cheryldocherty 0:04fef978a0ab 102 while(1) {
cheryldocherty 0:04fef978a0ab 103 // things
cheryldocherty 0:04fef978a0ab 104 Thread::wait(1000);
cheryldocherty 0:04fef978a0ab 105 }
cheryldocherty 0:04fef978a0ab 106 }