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?
User | Revision | Line number | New 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 | } |