Jay Balar / Mbed 2 deprecated 4180_Project_3

Sound update

Dependencies:   4DGL-uLCD-SE Physac-MBED PinDetect SDFileSystem mbed-rtos mbed

main.cpp@7:d27f97ac2bea, 22 months ago (annotated)

Committer:
jstephens78
Date:
Tue Nov 15 04:20:38 2022 +0000
Revision:
7:d27f97ac2bea
Parent:
0:da114b98e013
Child:
8:005b0a85be70
Accelerometer-based control testing

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jstephens78 7:d27f97ac2bea 1 // uLCD-144-G2 demo program for uLCD-4GL LCD driver library
jstephens78 7:d27f97ac2bea 2 //
jstephens78 7:d27f97ac2bea 3 #include <cmath>
jstephens78 0:da114b98e013 4 #include "mbed.h"
jstephens78 7:d27f97ac2bea 5 #include "uLCD_4DGL.h"
jstephens78 7:d27f97ac2bea 6 #include "LSM9DS1.h"
jstephens78 7:d27f97ac2bea 7 #define PI 3.14159
jstephens78 7:d27f97ac2bea 8
jstephens78 7:d27f97ac2bea 9 uLCD_4DGL uLCD(p9,p10,p30);
jstephens78 7:d27f97ac2bea 10 LSM9DS1 imu(p28, p27, 0xD6, 0x3C);
jstephens78 7:d27f97ac2bea 11 Serial pc(USBTX, USBRX);
jstephens78 0:da114b98e013 12
jstephens78 7:d27f97ac2bea 13 void getAttitude(LSM9DS1& imu, float& roll, float& pitch, float& yaw)
jstephens78 7:d27f97ac2bea 14 {
jstephens78 7:d27f97ac2bea 15 float ax = imu.calcAccel(imu.ax),
jstephens78 7:d27f97ac2bea 16 ay = imu.calcAccel(imu.ay),
jstephens78 7:d27f97ac2bea 17 az = imu.calcAccel(imu.az),
jstephens78 7:d27f97ac2bea 18 mx = -imu.calcMag(imu.mx),
jstephens78 7:d27f97ac2bea 19 my = imu.calcMag(imu.my),
jstephens78 7:d27f97ac2bea 20 mz = imu.calcMag(imu.mz);
jstephens78 7:d27f97ac2bea 21
jstephens78 7:d27f97ac2bea 22 pitch = atan2(-ax, sqrt(ay * ay + az * az));
jstephens78 7:d27f97ac2bea 23
jstephens78 7:d27f97ac2bea 24 if (az == 0.0) roll = (ay < 0.0) ? 180 : 0.0;
jstephens78 7:d27f97ac2bea 25 else roll = atan2(ay, az);
jstephens78 7:d27f97ac2bea 26
jstephens78 7:d27f97ac2bea 27 if (my == 0.0) yaw = (mx < 0.0) ? 180.0 : 0.0;
jstephens78 7:d27f97ac2bea 28 else yaw = atan2(mx, my);
jstephens78 7:d27f97ac2bea 29
jstephens78 7:d27f97ac2bea 30 if (yaw > 180.0) yaw -=PI;
jstephens78 7:d27f97ac2bea 31 else if (yaw < -180.0) yaw += PI;
jstephens78 7:d27f97ac2bea 32
jstephens78 7:d27f97ac2bea 33 }
jstephens78 0:da114b98e013 34
jstephens78 7:d27f97ac2bea 35 int main()
jstephens78 7:d27f97ac2bea 36 {
jstephens78 7:d27f97ac2bea 37 imu.begin();
jstephens78 7:d27f97ac2bea 38 if (!imu.begin()) {
jstephens78 7:d27f97ac2bea 39 pc.printf("Failed to communicate with LSM9DS1.\n");
jstephens78 7:d27f97ac2bea 40 }
jstephens78 7:d27f97ac2bea 41 imu.calibrate(1);
jstephens78 7:d27f97ac2bea 42
jstephens78 7:d27f97ac2bea 43
jstephens78 7:d27f97ac2bea 44 while (true) {
jstephens78 7:d27f97ac2bea 45 // Read IMU
jstephens78 7:d27f97ac2bea 46 while(!imu.accelAvailable());
jstephens78 7:d27f97ac2bea 47 imu.readAccel();
jstephens78 7:d27f97ac2bea 48
jstephens78 7:d27f97ac2bea 49 while (!imu.magAvailable());
jstephens78 7:d27f97ac2bea 50 imu.readMag();
jstephens78 7:d27f97ac2bea 51
jstephens78 7:d27f97ac2bea 52
jstephens78 7:d27f97ac2bea 53
jstephens78 7:d27f97ac2bea 54 float roll, pitch, yaw;
jstephens78 7:d27f97ac2bea 55 getAttitude(imu, roll, pitch, yaw);
jstephens78 7:d27f97ac2bea 56
jstephens78 7:d27f97ac2bea 57 pc.printf("%7f %7f %7f\r\n", roll, pitch, yaw);
jstephens78 7:d27f97ac2bea 58
jstephens78 7:d27f97ac2bea 59
jstephens78 7:d27f97ac2bea 60 uLCD.cls();
jstephens78 7:d27f97ac2bea 61
jstephens78 7:d27f97ac2bea 62 float x = 64 + roll * 60;
jstephens78 7:d27f97ac2bea 63 float y = 64 + pitch * 60;
jstephens78 7:d27f97ac2bea 64 float dx = 12 * sin(yaw);
jstephens78 7:d27f97ac2bea 65 float dy = 12 * cos(yaw);
jstephens78 7:d27f97ac2bea 66
jstephens78 7:d27f97ac2bea 67 uLCD.circle(x, y, 2, GREEN);
jstephens78 7:d27f97ac2bea 68 uLCD.line(x-dx, y-dy, x+dx, y+dy, GREEN);
jstephens78 7:d27f97ac2bea 69 wait(.05);
jstephens78 0:da114b98e013 70 }
jstephens78 0:da114b98e013 71 }