Madeline Kistler / Mbed 2 deprecated Kistler_A9_IMU

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed LSM9DS11

main.cpp@1:d90cd9d60f7e, 2020-11-30 (annotated)

Committer:
mkistler
Date:
Mon Nov 30 21:31:20 2020 +0000
Revision:
1:d90cd9d60f7e
Parent:
0:eb5dec59e660 
Final code.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mkistler 0:eb5dec59e660 1 #include "LSM9DS1.h"
mkistler 0:eb5dec59e660 2
mkistler 0:eb5dec59e660 3 DigitalOut myled(LED1);
mkistler 0:eb5dec59e660 4 Serial pc(USBTX, USBRX);
mkistler 1:d90cd9d60f7e 5 LSM9DS1 lol(p28, p27, 0xD6, 0x3C); // IMU pins.
mkistler 1:d90cd9d60f7e 6 Timer t; // Timer to collect time series.
mkistler 0:eb5dec59e660 7
mkistler 1:d90cd9d60f7e 8 PwmOut servo(p21); // Servo pin
mkistler 0:eb5dec59e660 9
mkistler 0:eb5dec59e660 10 float PI = 3.14159265358979323846f;
mkistler 1:d90cd9d60f7e 11 float angle; // Angle of the servo
mkistler 1:d90cd9d60f7e 12 float pulseWidth; // Pulse width and other pulse variables help communicate to the servo the correct angle through the PMW
mkistler 0:eb5dec59e660 13 float pulseCoeff = 10.0;
mkistler 0:eb5dec59e660 14 float pulseOffset = 400;
mkistler 0:eb5dec59e660 15
mkistler 0:eb5dec59e660 16 void mag_correction(float mx, float my, float mz, float mag_c[3])
mkistler 0:eb5dec59e660 17 {
mkistler 1:d90cd9d60f7e 18 float bias[3] = {0.2230,0.8809,0.1835}; // These matrices for bias and scale were found from the previous calibration
mkistler 0:eb5dec59e660 19 float scale[3][3] = {{0.9918,0.0913,0.0922},
mkistler 0:eb5dec59e660 20 {0.0913,1.6211,0.1597},
mkistler 0:eb5dec59e660 21 {0.0922,0.1597,0.6479}
mkistler 0:eb5dec59e660 22 };
mkistler 1:d90cd9d60f7e 23 // Full completing the calibration using the values of bias and scale.
mkistler 0:eb5dec59e660 24 mag_c[0] = (mx - bias[0]) *scale[0][0] + (my - bias[1]) *scale[1][0] + (mz - bias[2]) *scale[2][0];
mkistler 0:eb5dec59e660 25 mag_c[1] = (mx - bias[0]) *scale[0][1] + (my - bias[1]) *scale[1][1] + (mz - bias[2]) *scale[2][1];
mkistler 0:eb5dec59e660 26 mag_c[2] = (mx - bias[0]) *scale[0][2] + (my - bias[1]) *scale[1][2] + (mz - bias[2]) *scale[2][2];
mkistler 0:eb5dec59e660 27 }
mkistler 0:eb5dec59e660 28
mkistler 0:eb5dec59e660 29 int main()
mkistler 0:eb5dec59e660 30 {
mkistler 0:eb5dec59e660 31
mkistler 1:d90cd9d60f7e 32 float roll, pitch, yaw; // Creating variables
mkistler 0:eb5dec59e660 33 float accel[3], mag[3], gyro[3];
mkistler 0:eb5dec59e660 34
mkistler 1:d90cd9d60f7e 35 lol.begin(); // Starting the connecting to the imu
mkistler 1:d90cd9d60f7e 36 if(!lol.begin()) { // If it didn't connect, print the script below.
mkistler 0:eb5dec59e660 37 pc.printf("Failed to communicate with LSM9DS1.\n");
mkistler 0:eb5dec59e660 38 }
mkistler 0:eb5dec59e660 39 lol.calibrate(true);
mkistler 0:eb5dec59e660 40 pc.printf("Gyro bias = %f,%f,%f\r\n", lol.gBias[0], lol.gBias[1], lol.gBias[2]);
mkistler 0:eb5dec59e660 41 pc.printf("accel bias = %f,%f,%f\r\n", lol.aBias[0], lol.aBias[1], lol.aBias[2]);
mkistler 0:eb5dec59e660 42 wait(1);
mkistler 0:eb5dec59e660 43 t.start();
mkistler 0:eb5dec59e660 44 while(1) {
mkistler 1:d90cd9d60f7e 45 lol.readMag(); // reading the different values from the IMU
mkistler 0:eb5dec59e660 46 lol.readGyro();
mkistler 0:eb5dec59e660 47 lol.readAccel();
mkistler 1:d90cd9d60f7e 48 accel[0] = lol.calcAccel(lol.ax); // Putting the acceleration values all into one matrix
mkistler 0:eb5dec59e660 49 accel[1] = lol.calcAccel(lol.ay);
mkistler 0:eb5dec59e660 50 accel[2] = - lol.calcAccel(lol.az);
mkistler 1:d90cd9d60f7e 51 gyro[0] = lol.calcGyro(lol.gx); // Putting all the gyro values all into one matrix
mkistler 0:eb5dec59e660 52 gyro[1] = lol.calcGyro(lol.gy);
mkistler 0:eb5dec59e660 53 gyro[2] = lol.calcGyro(lol.gz);
mkistler 0:eb5dec59e660 54
mkistler 1:d90cd9d60f7e 55 mag_correction(lol.calcMag(lol.mx), lol.calcMag(lol.my), lol.calcMag(lol.mz), mag); // Correcting mag.
mkistler 1:d90cd9d60f7e 56 mag[2] = -mag[2]; // Making mag negative.
mkistler 1:d90cd9d60f7e 57
mkistler 1:d90cd9d60f7e 58 // Calculating roll pitch and yaw from the values above.
mkistler 0:eb5dec59e660 59 roll = atan2(accel[1], accel[2]/abs(accel[2])*(sqrt((accel[0]*accel[0])+(accel[2]*accel[2]))));
mkistler 0:eb5dec59e660 60 pitch = -atan2(-accel[0], (sqrt((accel[1]*accel[1])+(accel[2]*accel[2]))));
mkistler 0:eb5dec59e660 61 float Yh = (mag[1]*cos(roll)) - (mag[2]*sin(roll));
mkistler 0:eb5dec59e660 62 float Xh = (mag[0]*cos(pitch))+(mag[1]*sin(roll)*sin(pitch)) + (mag[2]*cos(roll) * sin(pitch));
mkistler 0:eb5dec59e660 63 yaw = atan2(Yh, Xh);
mkistler 0:eb5dec59e660 64 pitch *= 180.0f / PI;
mkistler 0:eb5dec59e660 65 yaw *= 180.0f / PI;
mkistler 0:eb5dec59e660 66 roll *= 180.0f / PI;
mkistler 0:eb5dec59e660 67
mkistler 1:d90cd9d60f7e 68 if(yaw<=0) { // Reseting yaw roll and pitch to 360 if it ever gets below zero.
mkistler 0:eb5dec59e660 69 yaw = yaw+360;
mkistler 0:eb5dec59e660 70 }
mkistler 0:eb5dec59e660 71 if(roll<=0) {
mkistler 0:eb5dec59e660 72 roll = roll+360;
mkistler 0:eb5dec59e660 73 }
mkistler 0:eb5dec59e660 74 if(pitch<=0) {
mkistler 0:eb5dec59e660 75 pitch = pitch+360;
mkistler 0:eb5dec59e660 76 }
mkistler 0:eb5dec59e660 77
mkistler 1:d90cd9d60f7e 78 angle = yaw/4; // Setting the angle of the servo to be proportional to the angle of the yaw
mkistler 1:d90cd9d60f7e 79 if (angle < 0) { // Ensuring the angle cannot exceed 190 or go below 0.
mkistler 0:eb5dec59e660 80 angle = 0;
mkistler 0:eb5dec59e660 81 } else if (angle > 90) {
mkistler 0:eb5dec59e660 82 angle = 90.0;
mkistler 0:eb5dec59e660 83 }
mkistler 1:d90cd9d60f7e 84 pulseWidth = pulseCoeff * angle + pulseOffset; // Calculting the pulse width from the angle value
mkistler 1:d90cd9d60f7e 85 servo.pulsewidth(pulseWidth/1000000.000); // Setting the physical servo to the right angle.
mkistler 0:eb5dec59e660 86
mkistler 0:eb5dec59e660 87
mkistler 1:d90cd9d60f7e 88 // Printing the values to coolterm
mkistler 0:eb5dec59e660 89 pc.printf("$IMU,3,11,%f,%3.3f, %3.3f, %3.3f, %3.3f, %3.3f, %3.3f, %3.3f, %3.3f, %3.3f, %3.3f;\r\n", t.read(), lol.calcGyro(lol.gx), lol.calcGyro(lol.gy), lol.calcGyro(lol.gz), lol.calcAccel(lol.ax), lol.calcAccel(lol.ay), lol.calcAccel(lol.az), roll, pitch, yaw, pulseWidth);
mkistler 0:eb5dec59e660 90 }
mkistler 0:eb5dec59e660 91 }