Takeuchi Kouichi
/
25h0922_IMU2_V1
Tamagawaseiki IMU2 Ver1
Fork of AQM0802_test by
main.cpp
- Committer:
- takeuchi
- Date:
- 2013-09-22
- Revision:
- 1:20b0a21f3b43
- Parent:
- 0:0543430a6bd1
File content as of revision 1:20b0a21f3b43:
// IMU2 Ver1 // Subete Suuji hyouji #include "mbed.h" #include "I2cLCD.h" I2cLCD lcd(p28,p27,p26); //sda scl reset Serial IMU(p9,p10);// p9=Tx,p10=Rx DigitalOut mled0(LED1); DigitalOut mled1(LED2); DigitalIn tsw(p11); #define ON 1 #define OFF 0 #define XON 0 #define XOFF 1 float rx,px,hx; void offset_can(){ int i; lcd.cls(); IMU.printf("$TSC,OFC,10*CC\r\n"); IMU.printf("$TSC,HRST*75\r\n"); lcd.printf("Offset cansel\n"); for(i=0;i<5;i++){ lcd.printf("."); wait(0.5); } lcd.cls(); } void IMU_get(){ int i; unsigned char c1,c2; while(IMU.getc()!='$'){ } for(i=1;i<=8;i++){ c1=IMU.getc(); } for(i=1;i<=20;i++){ c1=IMU.getc(); } c1=IMU.getc(); c2=IMU.getc(); rx=float((c1 << 8) | c2); c1=IMU.getc(); c2=IMU.getc(); px=float((c1 << 8) | c2); c1=IMU.getc(); c2=IMU.getc(); hx=float((c1 << 8) | c2); } int main() { IMU.baud(119200); lcd.cls(); lcd.locate(0,0); lcd.printf("IMU2 Test03\n"); lcd.printf("Init now..."); IMU.printf("$TSC,HRST*75\r\n"); wait(0.1); IMU.printf("$TSC,RAW,10\r\n"); wait(0.1); lcd.cls(); wait(0.1); mled0=OFF; while (1) { if(tsw==XON){ mled0=ON; offset_can(); } mled0=OFF; IMU_get(); lcd.locate(0,0); lcd.printf("r:%5.1f,p:%5.1f",180*rx/32768,180*px/32768); lcd.locate(0,1); lcd.printf("h:%5.1f",180*hx/32768); }//while }//main