Sound update
Dependencies: 4DGL-uLCD-SE Physac-MBED PinDetect SDFileSystem mbed-rtos mbed
Diff: main.cpp
- Revision:
- 7:d27f97ac2bea
- Parent:
- 0:da114b98e013
- Child:
- 8:005b0a85be70
diff -r da114b98e013 -r d27f97ac2bea main.cpp --- a/main.cpp Mon Nov 14 20:23:43 2022 +0000 +++ b/main.cpp Tue Nov 15 04:20:38 2022 +0000 @@ -1,12 +1,71 @@ +// uLCD-144-G2 demo program for uLCD-4GL LCD driver library +// +#include <cmath> #include "mbed.h" +#include "uLCD_4DGL.h" +#include "LSM9DS1.h" +#define PI 3.14159 + +uLCD_4DGL uLCD(p9,p10,p30); +LSM9DS1 imu(p28, p27, 0xD6, 0x3C); +Serial pc(USBTX, USBRX); -DigitalOut myled(LED1); +void getAttitude(LSM9DS1& imu, float& roll, float& pitch, float& yaw) +{ + float ax = imu.calcAccel(imu.ax), + ay = imu.calcAccel(imu.ay), + az = imu.calcAccel(imu.az), + mx = -imu.calcMag(imu.mx), + my = imu.calcMag(imu.my), + mz = imu.calcMag(imu.mz); + + pitch = atan2(-ax, sqrt(ay * ay + az * az)); + + if (az == 0.0) roll = (ay < 0.0) ? 180 : 0.0; + else roll = atan2(ay, az); + + if (my == 0.0) yaw = (mx < 0.0) ? 180.0 : 0.0; + else yaw = atan2(mx, my); + + if (yaw > 180.0) yaw -=PI; + else if (yaw < -180.0) yaw += PI; + +} -int main() { - while(1) { - myled = 1; - wait(0.2); - myled = 0; - wait(0.2); +int main() +{ + imu.begin(); + if (!imu.begin()) { + pc.printf("Failed to communicate with LSM9DS1.\n"); + } + imu.calibrate(1); + + + while (true) { + // Read IMU + while(!imu.accelAvailable()); + imu.readAccel(); + + while (!imu.magAvailable()); + imu.readMag(); + + + + float roll, pitch, yaw; + getAttitude(imu, roll, pitch, yaw); + + pc.printf("%7f %7f %7f\r\n", roll, pitch, yaw); + + + uLCD.cls(); + + float x = 64 + roll * 60; + float y = 64 + pitch * 60; + float dx = 12 * sin(yaw); + float dy = 12 * cos(yaw); + + uLCD.circle(x, y, 2, GREEN); + uLCD.line(x-dx, y-dy, x+dx, y+dy, GREEN); + wait(.05); } }