Takeuchi Kouichi
/
25h0922_IMU2_V1
Tamagawaseiki IMU2 Ver1
Fork of AQM0802_test by
main.cpp@1:20b0a21f3b43, 2013-09-22 (annotated)
- Committer:
- takeuchi
- Date:
- Sun Sep 22 03:04:05 2013 +0000
- Revision:
- 1:20b0a21f3b43
- Parent:
- 0:0543430a6bd1
Tamagawaseiki IMU2 Ver1
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
takeuchi | 1:20b0a21f3b43 | 1 | // IMU2 Ver1 |
takeuchi | 1:20b0a21f3b43 | 2 | // Subete Suuji hyouji |
takeuchi | 1:20b0a21f3b43 | 3 | |
takeuchi | 1:20b0a21f3b43 | 4 | #include "mbed.h" |
takeuchi | 1:20b0a21f3b43 | 5 | #include "I2cLCD.h" |
ytsuboi | 0:0543430a6bd1 | 6 | |
takeuchi | 1:20b0a21f3b43 | 7 | I2cLCD lcd(p28,p27,p26); //sda scl reset |
takeuchi | 1:20b0a21f3b43 | 8 | Serial IMU(p9,p10);// p9=Tx,p10=Rx |
ytsuboi | 0:0543430a6bd1 | 9 | |
takeuchi | 1:20b0a21f3b43 | 10 | DigitalOut mled0(LED1); |
takeuchi | 1:20b0a21f3b43 | 11 | DigitalOut mled1(LED2); |
takeuchi | 1:20b0a21f3b43 | 12 | DigitalIn tsw(p11); |
ytsuboi | 0:0543430a6bd1 | 13 | |
takeuchi | 1:20b0a21f3b43 | 14 | #define ON 1 |
takeuchi | 1:20b0a21f3b43 | 15 | #define OFF 0 |
takeuchi | 1:20b0a21f3b43 | 16 | #define XON 0 |
takeuchi | 1:20b0a21f3b43 | 17 | #define XOFF 1 |
ytsuboi | 0:0543430a6bd1 | 18 | |
takeuchi | 1:20b0a21f3b43 | 19 | float rx,px,hx; |
ytsuboi | 0:0543430a6bd1 | 20 | |
takeuchi | 1:20b0a21f3b43 | 21 | void offset_can(){ |
takeuchi | 1:20b0a21f3b43 | 22 | int i; |
takeuchi | 1:20b0a21f3b43 | 23 | lcd.cls(); |
takeuchi | 1:20b0a21f3b43 | 24 | IMU.printf("$TSC,OFC,10*CC\r\n"); |
takeuchi | 1:20b0a21f3b43 | 25 | IMU.printf("$TSC,HRST*75\r\n"); |
takeuchi | 1:20b0a21f3b43 | 26 | lcd.printf("Offset cansel\n"); |
takeuchi | 1:20b0a21f3b43 | 27 | for(i=0;i<5;i++){ |
takeuchi | 1:20b0a21f3b43 | 28 | lcd.printf("."); |
takeuchi | 1:20b0a21f3b43 | 29 | wait(0.5); |
takeuchi | 1:20b0a21f3b43 | 30 | } |
takeuchi | 1:20b0a21f3b43 | 31 | lcd.cls(); |
ytsuboi | 0:0543430a6bd1 | 32 | } |
ytsuboi | 0:0543430a6bd1 | 33 | |
takeuchi | 1:20b0a21f3b43 | 34 | void IMU_get(){ |
takeuchi | 1:20b0a21f3b43 | 35 | int i; |
takeuchi | 1:20b0a21f3b43 | 36 | unsigned char c1,c2; |
ytsuboi | 0:0543430a6bd1 | 37 | |
takeuchi | 1:20b0a21f3b43 | 38 | while(IMU.getc()!='$'){ |
takeuchi | 1:20b0a21f3b43 | 39 | } |
takeuchi | 1:20b0a21f3b43 | 40 | for(i=1;i<=8;i++){ |
takeuchi | 1:20b0a21f3b43 | 41 | c1=IMU.getc(); |
takeuchi | 1:20b0a21f3b43 | 42 | } |
takeuchi | 1:20b0a21f3b43 | 43 | for(i=1;i<=20;i++){ |
takeuchi | 1:20b0a21f3b43 | 44 | c1=IMU.getc(); |
ytsuboi | 0:0543430a6bd1 | 45 | } |
takeuchi | 1:20b0a21f3b43 | 46 | c1=IMU.getc(); |
takeuchi | 1:20b0a21f3b43 | 47 | c2=IMU.getc(); |
takeuchi | 1:20b0a21f3b43 | 48 | rx=float((c1 << 8) | c2); |
takeuchi | 1:20b0a21f3b43 | 49 | c1=IMU.getc(); |
takeuchi | 1:20b0a21f3b43 | 50 | c2=IMU.getc(); |
takeuchi | 1:20b0a21f3b43 | 51 | px=float((c1 << 8) | c2); |
takeuchi | 1:20b0a21f3b43 | 52 | c1=IMU.getc(); |
takeuchi | 1:20b0a21f3b43 | 53 | c2=IMU.getc(); |
takeuchi | 1:20b0a21f3b43 | 54 | hx=float((c1 << 8) | c2); |
ytsuboi | 0:0543430a6bd1 | 55 | } |
ytsuboi | 0:0543430a6bd1 | 56 | |
ytsuboi | 0:0543430a6bd1 | 57 | |
ytsuboi | 0:0543430a6bd1 | 58 | int main() { |
takeuchi | 1:20b0a21f3b43 | 59 | |
takeuchi | 1:20b0a21f3b43 | 60 | IMU.baud(119200); |
takeuchi | 1:20b0a21f3b43 | 61 | lcd.cls(); |
takeuchi | 1:20b0a21f3b43 | 62 | lcd.locate(0,0); |
takeuchi | 1:20b0a21f3b43 | 63 | lcd.printf("IMU2 Test03\n"); |
takeuchi | 1:20b0a21f3b43 | 64 | lcd.printf("Init now..."); |
takeuchi | 1:20b0a21f3b43 | 65 | IMU.printf("$TSC,HRST*75\r\n"); |
takeuchi | 1:20b0a21f3b43 | 66 | wait(0.1); |
takeuchi | 1:20b0a21f3b43 | 67 | IMU.printf("$TSC,RAW,10\r\n"); |
takeuchi | 1:20b0a21f3b43 | 68 | wait(0.1); |
takeuchi | 1:20b0a21f3b43 | 69 | |
takeuchi | 1:20b0a21f3b43 | 70 | lcd.cls(); |
takeuchi | 1:20b0a21f3b43 | 71 | wait(0.1); |
takeuchi | 1:20b0a21f3b43 | 72 | mled0=OFF; |
takeuchi | 1:20b0a21f3b43 | 73 | |
takeuchi | 1:20b0a21f3b43 | 74 | while (1) { |
takeuchi | 1:20b0a21f3b43 | 75 | if(tsw==XON){ |
takeuchi | 1:20b0a21f3b43 | 76 | mled0=ON; |
takeuchi | 1:20b0a21f3b43 | 77 | offset_can(); |
ytsuboi | 0:0543430a6bd1 | 78 | } |
takeuchi | 1:20b0a21f3b43 | 79 | mled0=OFF; |
takeuchi | 1:20b0a21f3b43 | 80 | |
takeuchi | 1:20b0a21f3b43 | 81 | IMU_get(); |
takeuchi | 1:20b0a21f3b43 | 82 | |
takeuchi | 1:20b0a21f3b43 | 83 | lcd.locate(0,0); |
takeuchi | 1:20b0a21f3b43 | 84 | lcd.printf("r:%5.1f,p:%5.1f",180*rx/32768,180*px/32768); |
takeuchi | 1:20b0a21f3b43 | 85 | lcd.locate(0,1); |
takeuchi | 1:20b0a21f3b43 | 86 | lcd.printf("h:%5.1f",180*hx/32768); |
takeuchi | 1:20b0a21f3b43 | 87 | |
takeuchi | 1:20b0a21f3b43 | 88 | }//while |
takeuchi | 1:20b0a21f3b43 | 89 | }//main |