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?
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; |
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 | } |