Benjamin Smith / Mbed 2 deprecated imu_lab4

Demo for finding orientation (pitch,roll) and velocity

Dependencies:   4DGL-uLCD-SE imu_lab4 mbed

Dependents:   imu_lab4

main.cpp@1:fe0a0b7530b0, 2015-03-13 (annotated)

Committer:
bsmit170
Date:
Fri Mar 13 23:46:39 2015 +0000
Revision:
1:fe0a0b7530b0
asdfsa

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bsmit170 1:fe0a0b7530b0 1 // LSM9DS90/lcd Demo
bsmit170 1:fe0a0b7530b0 2 // ECE 4180 Lab Code Template
bsmit170 1:fe0a0b7530b0 3
bsmit170 1:fe0a0b7530b0 4 #include "mbed.h"
bsmit170 1:fe0a0b7530b0 5 #include "LSM9DS0.h"
bsmit170 1:fe0a0b7530b0 6 #include "uLCD_4DGL.h"
bsmit170 1:fe0a0b7530b0 7
bsmit170 1:fe0a0b7530b0 8 // uncomment this line to enable the lcd for Part 4 of the lab
bsmit170 1:fe0a0b7530b0 9
bsmit170 1:fe0a0b7530b0 10
bsmit170 1:fe0a0b7530b0 11 // SDO_XM and SDO_G are pulled up, so our addresses are:
bsmit170 1:fe0a0b7530b0 12 #define LSM9DS0_XM_ADDR 0x1D // Would be 0x1E if SDO_XM is LOW
bsmit170 1:fe0a0b7530b0 13 #define LSM9DS0_G_ADDR 0x6B // Would be 0x6A if SDO_G is LOW
bsmit170 1:fe0a0b7530b0 14
bsmit170 1:fe0a0b7530b0 15 // refresh time. set to 500 for part 2 and 50 for part 4
bsmit170 1:fe0a0b7530b0 16 #define REFRESH_TIME_MS 2000
bsmit170 1:fe0a0b7530b0 17
bsmit170 1:fe0a0b7530b0 18 // Verify that the pin assignments below match your breadboard
bsmit170 1:fe0a0b7530b0 19 LSM9DS0 imu(p9, p10, LSM9DS0_G_ADDR, LSM9DS0_XM_ADDR);
bsmit170 1:fe0a0b7530b0 20 Serial pc(USBTX, USBRX);
bsmit170 1:fe0a0b7530b0 21
bsmit170 1:fe0a0b7530b0 22
bsmit170 1:fe0a0b7530b0 23
bsmit170 1:fe0a0b7530b0 24 uLCD_4DGL lcd(p28, p27, p30);
bsmit170 1:fe0a0b7530b0 25
bsmit170 1:fe0a0b7530b0 26
bsmit170 1:fe0a0b7530b0 27 //Init Serial port and LSM9DS0 chip
bsmit170 1:fe0a0b7530b0 28 void setup()
bsmit170 1:fe0a0b7530b0 29 {
bsmit170 1:fe0a0b7530b0 30 #ifdef PART_4
bsmit170 1:fe0a0b7530b0 31 lcd.baudrate(3000000);
bsmit170 1:fe0a0b7530b0 32 lcd.background_color(0);
bsmit170 1:fe0a0b7530b0 33 lcd.cls();
bsmit170 1:fe0a0b7530b0 34
bsmit170 1:fe0a0b7530b0 35 lcd.printf("Initializing...");
bsmit170 1:fe0a0b7530b0 36 #endif
bsmit170 1:fe0a0b7530b0 37
bsmit170 1:fe0a0b7530b0 38 // Use the begin() function to initialize the LSM9DS0 library.
bsmit170 1:fe0a0b7530b0 39 // You can either call it with no parameters (the easy way):
bsmit170 1:fe0a0b7530b0 40 uint16_t status = imu.begin();
bsmit170 1:fe0a0b7530b0 41
bsmit170 1:fe0a0b7530b0 42 //Make sure communication is working
bsmit170 1:fe0a0b7530b0 43 lcd.printf("LSM9DS0 WHO_AM_I's returned: 0x%X\n", status);
bsmit170 1:fe0a0b7530b0 44 lcd.printf("Should be 0x49D4\n\n");
bsmit170 1:fe0a0b7530b0 45 wait(5.0);
bsmit170 1:fe0a0b7530b0 46 }
bsmit170 1:fe0a0b7530b0 47
bsmit170 1:fe0a0b7530b0 48
bsmit170 1:fe0a0b7530b0 49 float oldX = 0;
bsmit170 1:fe0a0b7530b0 50 float oldY = 0;
bsmit170 1:fe0a0b7530b0 51 float oldZ = 0;
bsmit170 1:fe0a0b7530b0 52 float x= 0;
bsmit170 1:fe0a0b7530b0 53 float y = 0;
bsmit170 1:fe0a0b7530b0 54 float z = 0;
bsmit170 1:fe0a0b7530b0 55
bsmit170 1:fe0a0b7530b0 56 float posx = 0;
bsmit170 1:fe0a0b7530b0 57 float velx = 0;
bsmit170 1:fe0a0b7530b0 58 float oldPosx = 0;
bsmit170 1:fe0a0b7530b0 59 float oldVelx = 0;
bsmit170 1:fe0a0b7530b0 60
bsmit170 1:fe0a0b7530b0 61 float posy = 0;
bsmit170 1:fe0a0b7530b0 62 float vely = 0;
bsmit170 1:fe0a0b7530b0 63 float oldPosy = 0;
bsmit170 1:fe0a0b7530b0 64 float oldVely = 0;
bsmit170 1:fe0a0b7530b0 65
bsmit170 1:fe0a0b7530b0 66 float posz = 0;
bsmit170 1:fe0a0b7530b0 67 float velz = 0;
bsmit170 1:fe0a0b7530b0 68 float oldPosz = 0;
bsmit170 1:fe0a0b7530b0 69 float oldVelz = 0;
bsmit170 1:fe0a0b7530b0 70
bsmit170 1:fe0a0b7530b0 71 int main()
bsmit170 1:fe0a0b7530b0 72 {
bsmit170 1:fe0a0b7530b0 73 setup(); //Setup sensor and Serial
bsmit170 1:fe0a0b7530b0 74 pc.printf("------ LSM0DS0 Demo -----------\n");
bsmit170 1:fe0a0b7530b0 75
bsmit170 1:fe0a0b7530b0 76 while (true)
bsmit170 1:fe0a0b7530b0 77 {
bsmit170 1:fe0a0b7530b0 78 lcd.cls();
bsmit170 1:fe0a0b7530b0 79
bsmit170 1:fe0a0b7530b0 80 imu.calcPitchRoll();
bsmit170 1:fe0a0b7530b0 81 lcd.printf("Pitch: ");
bsmit170 1:fe0a0b7530b0 82 lcd.printf("%2f\n",imu.pitch);
bsmit170 1:fe0a0b7530b0 83 lcd.printf("Roll: ");
bsmit170 1:fe0a0b7530b0 84 lcd.printf("%2f\n",imu.roll);
bsmit170 1:fe0a0b7530b0 85
bsmit170 1:fe0a0b7530b0 86
bsmit170 1:fe0a0b7530b0 87 lcd.printf("X Accel: %2f\n",imu.ax);
bsmit170 1:fe0a0b7530b0 88 lcd.printf(", ");
bsmit170 1:fe0a0b7530b0 89 lcd.printf("Y Accel:%2f\n",imu.ay);
bsmit170 1:fe0a0b7530b0 90 lcd.printf(", ");
bsmit170 1:fe0a0b7530b0 91 lcd.printf("Z Accel: %2f\n",imu.az);
bsmit170 1:fe0a0b7530b0 92
bsmit170 1:fe0a0b7530b0 93 velx = oldVelx + REFRESH_TIME_MS * oldX/1000;
bsmit170 1:fe0a0b7530b0 94 posx = oldPosx + REFRESH_TIME_MS * oldVelx/1000;
bsmit170 1:fe0a0b7530b0 95
bsmit170 1:fe0a0b7530b0 96 vely = oldVely + REFRESH_TIME_MS * oldY/1000;
bsmit170 1:fe0a0b7530b0 97 posy = oldPosy + REFRESH_TIME_MS * oldVely/1000;
bsmit170 1:fe0a0b7530b0 98
bsmit170 1:fe0a0b7530b0 99 velz = oldVelz + REFRESH_TIME_MS * oldZ/1000;
bsmit170 1:fe0a0b7530b0 100 posz = oldPosx + REFRESH_TIME_MS * oldVelz/1000;
bsmit170 1:fe0a0b7530b0 101
bsmit170 1:fe0a0b7530b0 102
bsmit170 1:fe0a0b7530b0 103 lcd.printf("X Vel: %2f\n",velx);
bsmit170 1:fe0a0b7530b0 104 lcd.printf(", ");
bsmit170 1:fe0a0b7530b0 105 lcd.printf("Y Vel:%2f\n",vely);
bsmit170 1:fe0a0b7530b0 106 lcd.printf(", ");
bsmit170 1:fe0a0b7530b0 107 lcd.printf("Z Vel: %2f\n",velz);
bsmit170 1:fe0a0b7530b0 108
bsmit170 1:fe0a0b7530b0 109
bsmit170 1:fe0a0b7530b0 110
bsmit170 1:fe0a0b7530b0 111
bsmit170 1:fe0a0b7530b0 112 oldVelx = velx;
bsmit170 1:fe0a0b7530b0 113 oldPosx = posx;
bsmit170 1:fe0a0b7530b0 114 oldVely = vely;
bsmit170 1:fe0a0b7530b0 115 oldPosy = posy;
bsmit170 1:fe0a0b7530b0 116 oldPosz = posz;
bsmit170 1:fe0a0b7530b0 117 oldVelz = velz;
bsmit170 1:fe0a0b7530b0 118 oldX = imu.ax;
bsmit170 1:fe0a0b7530b0 119 oldY = imu.ay;
bsmit170 1:fe0a0b7530b0 120 oldZ = imu.az;
bsmit170 1:fe0a0b7530b0 121
bsmit170 1:fe0a0b7530b0 122
bsmit170 1:fe0a0b7530b0 123 wait_ms(REFRESH_TIME_MS);
bsmit170 1:fe0a0b7530b0 124 }
bsmit170 1:fe0a0b7530b0 125 }