SURIYON TANSURIYAVONG / Mbed 2 deprecated mbed_blinky

The example program for mbed pin-compatible platforms

Dependencies:   BM1422AGMV KX224-1053 mbed

main.cpp@25:1ab0189cdaaf, 19 months ago (annotated)

Committer:
suriyon
Date:
Thu Sep 22 23:04:14 2022 +0000
Revision:
25:1ab0189cdaaf
Parent:
24:7f14b70fc9ef 
sssssssssssssssss

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dan 0:7dec7e9ac085 1 #include "mbed.h"
suriyon 25:1ab0189cdaaf 2 #include "BM1422AGMV.h"
suriyon 25:1ab0189cdaaf 3 #include "KX224.h"
suriyon 25:1ab0189cdaaf 4 //#include "wire.h"
dan 0:7dec7e9ac085 5
dan 0:7dec7e9ac085 6 DigitalOut myled(LED1);
dan 0:7dec7e9ac085 7
suriyon 25:1ab0189cdaaf 8 //https://www.google.com/search?q=bm1422agmv+arduino&rlz=1C1SQJL_enTH863TH863&oq=BM1422&aqs=chrome.5.69i57j0i19j69i59l2j0i19l2j69i60l2.3962j0j7&sourceid=chrome&ie=UTF-8#imgrc=llWSwJWhYP3wyM
suriyon 25:1ab0189cdaaf 9
suriyon 25:1ab0189cdaaf 10 I2C i2c(I2C_SDA, I2C_SCL); // setup i2c (SDA,SCL)
suriyon 25:1ab0189cdaaf 11
suriyon 25:1ab0189cdaaf 12 BM1422AGMV bm1422(I2C_SDA, I2C_SCL);
suriyon 25:1ab0189cdaaf 13 KX224 accelerometer(I2C_SDA, I2C_SCL);
suriyon 25:1ab0189cdaaf 14
suriyon 25:1ab0189cdaaf 15 //BM1422AGMV bm1422(i2c,0x0E);
suriyon 25:1ab0189cdaaf 16 Serial pc(USBTX, USBRX);
suriyon 25:1ab0189cdaaf 17
suriyon 25:1ab0189cdaaf 18 int _px = 6.3;//-44.5; //中点x
suriyon 25:1ab0189cdaaf 19 int _py = -24.8;//-38.5; //中点y
suriyon 25:1ab0189cdaaf 20
suriyon 25:1ab0189cdaaf 21 /** 角度を求める **/
suriyon 25:1ab0189cdaaf 22 double getDirection(double x, double y){
suriyon 25:1ab0189cdaaf 23 double dir = 0;
suriyon 25:1ab0189cdaaf 24 dir = atan((y - _py)/(x - _px));
suriyon 25:1ab0189cdaaf 25 return dir;
suriyon 25:1ab0189cdaaf 26 }
suriyon 25:1ab0189cdaaf 27
dan 0:7dec7e9ac085 28 int main() {
suriyon 25:1ab0189cdaaf 29 //bm1422.initialize();
suriyon 25:1ab0189cdaaf 30 pc.baud(115200); // baud rate: 9600
suriyon 25:1ab0189cdaaf 31 pc.printf("test\n");
suriyon 25:1ab0189cdaaf 32 float mag[3];
suriyon 25:1ab0189cdaaf 33 float acc[3];
suriyon 25:1ab0189cdaaf 34 int ret;
suriyon 25:1ab0189cdaaf 35
suriyon 25:1ab0189cdaaf 36 double rd;
suriyon 25:1ab0189cdaaf 37 int x = mag[0];
suriyon 25:1ab0189cdaaf 38 int y = mag[1];
suriyon 25:1ab0189cdaaf 39 int z = mag[2];
suriyon 25:1ab0189cdaaf 40
suriyon 25:1ab0189cdaaf 41 //Initialize KX224-1053
suriyon 25:1ab0189cdaaf 42 ret = accelerometer.initialize();
suriyon 25:1ab0189cdaaf 43 if (ret != 0) {
suriyon 25:1ab0189cdaaf 44 printf("\r\nKX224-1053 : Failed initialization \n");
suriyon 25:1ab0189cdaaf 45 }
suriyon 25:1ab0189cdaaf 46
dan 0:7dec7e9ac085 47 while(1) {
suriyon 25:1ab0189cdaaf 48 accelerometer.get_val(&acc[0]);
suriyon 25:1ab0189cdaaf 49 bm1422.get_val(mag);
suriyon 25:1ab0189cdaaf 50 rd = getDirection(mag[0],mag[1]);
suriyon 25:1ab0189cdaaf 51 x = mag[0];
suriyon 25:1ab0189cdaaf 52 y = mag[1];
suriyon 25:1ab0189cdaaf 53 z = mag[2];
suriyon 25:1ab0189cdaaf 54
suriyon 25:1ab0189cdaaf 55 printf("%7.2f %7.2f %7.2f %7.2f %7.2f %7.2f\r\n", mag[0], mag[1], mag[2],acc[0], acc[1], acc[2]);
suriyon 25:1ab0189cdaaf 56
suriyon 25:1ab0189cdaaf 57 //磁北ではないときステッピングモーターを回転させる
suriyon 25:1ab0189cdaaf 58 //ステッピングモーターの回転数に応じてrdに余裕を持たせています
suriyon 25:1ab0189cdaaf 59 if(rd <= 0.3 && rd >= -0.3 && x > _px){
suriyon 25:1ab0189cdaaf 60 //磁北の状態//Serial.println("North!");//https://deviceplus.jp/arduino/entry041/
suriyon 25:1ab0189cdaaf 61 pc.printf("%f %d %d %d North! \n",rd,x,y,z);
suriyon 25:1ab0189cdaaf 62 myled = 1;
suriyon 25:1ab0189cdaaf 63
suriyon 25:1ab0189cdaaf 64 }else{
suriyon 25:1ab0189cdaaf 65 pc.printf("%f %d %d %d - \n",rd,x,y,z);
suriyon 25:1ab0189cdaaf 66 myled = 0;
suriyon 25:1ab0189cdaaf 67 }
suriyon 25:1ab0189cdaaf 68 /*
suriyon 25:1ab0189cdaaf 69 else if(rd > 0){
suriyon 25:1ab0189cdaaf 70 moveStepper(2,-1);
suriyon 25:1ab0189cdaaf 71 }
suriyon 25:1ab0189cdaaf 72 else{
suriyon 25:1ab0189cdaaf 73 moveStepper(2);
suriyon 25:1ab0189cdaaf 74 }
suriyon 25:1ab0189cdaaf 75 */
suriyon 25:1ab0189cdaaf 76
suriyon 25:1ab0189cdaaf 77 //myled = 1;
suriyon 25:1ab0189cdaaf 78 //wait(.1);
suriyon 25:1ab0189cdaaf 79 //myled = 0;
suriyon 25:1ab0189cdaaf 80 //wait(.1);
stevep 4:81cea7a352b0 81 }
dan 0:7dec7e9ac085 82 }