Richard Kuo / Mbed OS NTOUEE-mbed-I2C_MPU6500_LCD

mbed I2C IMU+LCD

main.cpp@2:6b9ce9b7f08a, 2016-10-20 (annotated)

Committer:
rkuo2000
Date:
Thu Oct 20 13:26:43 2016 +0000
Revision:
2:6b9ce9b7f08a
Parent:
1:efa4c7817836 
mbed I2C IMU+LCD

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rkuo2000 2:6b9ce9b7f08a 1 // using NuMaker-PFM-NUC472 I2C LCD+IMU control the direction of moving pointer
rkuo2000 0:73a95126993e 2 #include "mbed.h"
rkuo2000 1:efa4c7817836 3 #include "math.h"
rkuo2000 0:73a95126993e 4 #include "mpu6500.h"
rkuo2000 2:6b9ce9b7f08a 5 #include "ssd1306.h"
rkuo2000 2:6b9ce9b7f08a 6 #include "draw2D.h"
rkuo2000 0:73a95126993e 7
rkuo2000 2:6b9ce9b7f08a 8 #define PI 3.1415926535
rkuo2000 1:efa4c7817836 9
rkuo2000 2:6b9ce9b7f08a 10 #define X0 64 // initial point
rkuo2000 2:6b9ce9b7f08a 11 #define Y0 32 // initial point
rkuo2000 0:73a95126993e 12
rkuo2000 2:6b9ce9b7f08a 13 I2C i2c0(PC_9 , PA_15); // I2C0_SDA, I2C0_SCL
rkuo2000 2:6b9ce9b7f08a 14 I2C i2c1(PD_12, PD_10); // I2C1_SDA, I2C1_SCL
rkuo2000 2:6b9ce9b7f08a 15
rkuo2000 2:6b9ce9b7f08a 16 MPU6500 IMU; // IMU connected on I2C0 (on-board MPU6500)
rkuo2000 2:6b9ce9b7f08a 17 SSD1306 LCD; // LCD connected on I2C1
rkuo2000 2:6b9ce9b7f08a 18 Draw2D D2D; // Draw2D library
rkuo2000 0:73a95126993e 19
rkuo2000 0:73a95126993e 20 int main() {
rkuo2000 2:6b9ce9b7f08a 21 int accX, accY, accZ;
rkuo2000 2:6b9ce9b7f08a 22 float theta, psi;
rkuo2000 2:6b9ce9b7f08a 23
rkuo2000 2:6b9ce9b7f08a 24 int dirX, dirY;
rkuo2000 2:6b9ce9b7f08a 25 int movX, movY;
rkuo2000 2:6b9ce9b7f08a 26 int x, y, r;
rkuo2000 2:6b9ce9b7f08a 27
rkuo2000 2:6b9ce9b7f08a 28 i2c0.frequency(400000);
rkuo2000 2:6b9ce9b7f08a 29 i2c1.frequency(400000);
rkuo2000 1:efa4c7817836 30
rkuo2000 0:73a95126993e 31 IMU.initialize();
rkuo2000 2:6b9ce9b7f08a 32 LCD.initialize();
rkuo2000 2:6b9ce9b7f08a 33 LCD.clearscreen();
rkuo2000 2:6b9ce9b7f08a 34
rkuo2000 2:6b9ce9b7f08a 35 x = X0; // cross center x
rkuo2000 2:6b9ce9b7f08a 36 y = Y0; // cross center y
rkuo2000 2:6b9ce9b7f08a 37 r = 2; // cross length
rkuo2000 0:73a95126993e 38
rkuo2000 2:6b9ce9b7f08a 39 movX = 2; // x movement
rkuo2000 2:6b9ce9b7f08a 40 movY = 2; // y movement
rkuo2000 2:6b9ce9b7f08a 41 dirX = 1; // x direction
rkuo2000 2:6b9ce9b7f08a 42 dirY = 1; // y direction
rkuo2000 2:6b9ce9b7f08a 43
rkuo2000 0:73a95126993e 44 while(true) {
rkuo2000 2:6b9ce9b7f08a 45 D2D.drawLine(x-2, y, x+2, y, FG_COLOR, BG_COLOR); // draw a line
rkuo2000 2:6b9ce9b7f08a 46 D2D.drawLine(x, y-2, x, y+2, FG_COLOR, BG_COLOR); // draw a line
rkuo2000 2:6b9ce9b7f08a 47
rkuo2000 2:6b9ce9b7f08a 48 Thread::wait(10);
rkuo2000 2:6b9ce9b7f08a 49
rkuo2000 2:6b9ce9b7f08a 50 D2D.drawLine(x-2, y, x+2, y, BG_COLOR, BG_COLOR); // draw a line
rkuo2000 2:6b9ce9b7f08a 51 D2D.drawLine(x, y-2, x, y+2, BG_COLOR, BG_COLOR); // draw a line
rkuo2000 2:6b9ce9b7f08a 52
rkuo2000 2:6b9ce9b7f08a 53 // read Accelerometer to calculate tilt angle
rkuo2000 0:73a95126993e 54 accX = IMU.getAccelXvalue();
rkuo2000 0:73a95126993e 55 accY = IMU.getAccelYvalue();
rkuo2000 0:73a95126993e 56 accZ = IMU.getAccelZvalue();
rkuo2000 2:6b9ce9b7f08a 57 theta = atan(accX / sqrt(pow(accY,2.0) + pow(accZ,2.0))) *180 /PI;
rkuo2000 2:6b9ce9b7f08a 58 psi = atan(accY / sqrt(pow(accZ,2.0) + pow(accX,2.0))) *180 /PI;
rkuo2000 2:6b9ce9b7f08a 59
rkuo2000 2:6b9ce9b7f08a 60 if (theta<-20) dirX= 1;
rkuo2000 2:6b9ce9b7f08a 61 else if (theta> 20) dirX=-1;
rkuo2000 2:6b9ce9b7f08a 62 else dirX= 0;
rkuo2000 2:6b9ce9b7f08a 63 if (psi <-20) dirY= 1;
rkuo2000 2:6b9ce9b7f08a 64 else if (psi > 20) dirY=-1;
rkuo2000 2:6b9ce9b7f08a 65 else dirY= 0;
rkuo2000 2:6b9ce9b7f08a 66
rkuo2000 2:6b9ce9b7f08a 67 x = x + dirX * movX; // change x of circle center
rkuo2000 2:6b9ce9b7f08a 68 y = y + dirY * movY; // change y of circle center
rkuo2000 2:6b9ce9b7f08a 69
rkuo2000 2:6b9ce9b7f08a 70 // boundary check for changing direction
rkuo2000 2:6b9ce9b7f08a 71 if ((x-r) <=0) {dirX= 1; x= x + dirX*movX;}
rkuo2000 2:6b9ce9b7f08a 72 else if ((x+r) >=LCD_Xmax) {dirX=-1; x= x + dirX*movX;}
rkuo2000 2:6b9ce9b7f08a 73
rkuo2000 2:6b9ce9b7f08a 74 if ((y-r) <=0) {dirY= 1; y= y + dirY*movY;}
rkuo2000 2:6b9ce9b7f08a 75 else if ((y+r) >=LCD_Ymax) {dirY=-1; y= y + dirY*movY;}
rkuo2000 2:6b9ce9b7f08a 76 }
rkuo2000 0:73a95126993e 77 }