Tamagawaseiki IMU2 Ver1

Dependencies:   mbed

Fork of AQM0802_test by Yoshihiro TSUBOI

Revision:
1:20b0a21f3b43
Parent:
0:0543430a6bd1
--- a/main.cpp	Wed Jun 19 15:58:03 2013 +0000
+++ b/main.cpp	Sun Sep 22 03:04:05 2013 +0000
@@ -1,208 +1,89 @@
-/*
- *  LCD module "AQM0802A-RN-GBW" sample program
- *
- *  AQM0802A-RN-GBW is an I2C text LCD panel (Controller: Sitronix ST7032i)
- *  I bought this from AKIZUKI DENSHI TSUSHO CO.,LTD.
- *  http://akizukidenshi.com/catalog/g/gP-06669/ (Online shop page (Japanese))
- *  http://akizukidenshi.com/download/ds/sitronix/st7032.pdf (datasheet of the chip)
- *
- *  Original Arduino version was developed by
- *  Copyright (c) 2013 Masahiro WAKAYAMA at SWITCH SCIENCE
- *
- *  Copyright (c) 2013 Yoshihiro TSUBOI
- *
- *  Released under the MIT License: http://mbed.org/license/mit
- *
- *  revision 1.0  19-June-2013   a. 1st release
- *
- */
- 
- #include "mbed.h"
+// IMU2 Ver1
+// Subete Suuji hyouji
+
+#include "mbed.h"
+#include "I2cLCD.h"
 
-I2C i2c(p28, p27); // sda, scl
-const int AQCM0802_addr = 0x7C;
+I2cLCD lcd(p28,p27,p26); //sda scl reset
+Serial IMU(p9,p10);// p9=Tx,p10=Rx 
 
-unsigned char mode;
-unsigned char contrast = 0; // 0-63
-unsigned char contrastFlag = false;
-int CGcounter;
-int FADEcounter;
-
-void lcd_cmd(char x) {
-  char data[2];
-  data[0] = 0x00; // CO = 0,RS = 0
-  data[1] = x;
-  i2c.write(AQCM0802_addr, data, 2);
-}
+DigitalOut mled0(LED1);
+DigitalOut mled1(LED2);
+DigitalIn tsw(p11);
 
-void lcd_contdata(char x) {
-  char data[2];
-  data[0] = 0xC0; //0b11000000 CO = 1, RS = 1
-  data[1] = x;
-  i2c.write(AQCM0802_addr, data, 2);
-}
+#define ON 1
+#define OFF 0
+#define XON 0
+#define XOFF 1
 
-void lcd_lastdata(char x) {
-  char data[2];
-  data[0] = 0x40; //0b11000000 CO = 0, RS = 1
-  data[1] = x;
-  i2c.write(AQCM0802_addr, data, 2);
-}
+float rx,px,hx;
 
-void lcd_printStr(const char *s) {
-  while(*s) {
-    if(*(s + 1)) {
-      lcd_contdata(*s);
-    } else {
-      lcd_lastdata(*s);
-    }
-    s++;
-  }
-}
-
-void lcd_printHex(unsigned char num) {
-  lcd_contdata(num);
-}
-
-void lcd_init() {
-  wait(0.04);
-  // LCD initialize
-  lcd_cmd(0x38); // function set
-  lcd_cmd(0x39); // function set
-  lcd_cmd(0x04); // EntryModeSet
-  lcd_cmd(0x14); // interval osc
-  lcd_cmd(0x70 | (contrast & 0xF)); // contrast Low
-  lcd_cmd(0x5C | ((contrast >> 4) & 0x3)); // contast High/icon/power
-  lcd_cmd(0x6C); // follower control
-  wait(0.2);
-  lcd_cmd(0x38); // function set
-  lcd_cmd(0x0C); // Display On
-  lcd_cmd(0x01); // Clear Display
-  wait(0.2); // need additional wait to Clear Display
-}
-
-void lcd_setCursor(unsigned char x,unsigned char y) {
-  lcd_cmd(0x80 | (y * 0x40 + x));
+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();
 }
 
-unsigned char cg[13 * 8] = {
-/*
-  0b00001111,0b00010000,0b00010000,0b00001110,0b00000001,0b00000001,0b00011110,0b00000000,
-  0b00010001,0b00010001,0b00010001,0b00010101,0b00010101,0b00010101,0b00001010,0b00000000,
-  0b00001110,0b00000100,0b00000100,0b00000100,0b00000100,0b00000100,0b00001110,0b00000000,
-  0b00011111,0b00000100,0b00000100,0b00000100,0b00000100,0b00000100,0b00000100,0b00000000,
-  0b00001110,0b00010001,0b00010000,0b00010000,0b00010000,0b00010001,0b00001110,0b00000000,
-  0b00010001,0b00010001,0b00010001,0b00011111,0b00010001,0b00010001,0b00010001,0b00000000,
-*/
-  0x0F,0x10,0x10,0x0E,0x01,0x01,0x1E,0x00, // S
-  0x11,0x11,0x11,0x15,0x15,0x15,0x0A,0x00, // W
-  0x0E,0x40,0x40,0x40,0x40,0x40,0x0E,0x00, // I
-  0x1F,0x40,0x40,0x40,0x40,0x40,0x40,0x00, // T
-  0x0E,0x11,0x10,0x10,0x10,0x11,0x0E,0x00, // C
-  0x11,0x11,0x11,0x1F,0x11,0x11,0x11,0x00, // H
-/*
-  0b00001111,0b00010000,0b00010000,0b00001110,0b00000001,0b00000001,0b00011110,0b00000000,
-  0b00001110,0b00010001,0b00010000,0b00010000,0b00010000,0b00010001,0b00001110,0b00000000,
-  0b00001110,0b00000100,0b00000100,0b00000100,0b00000100,0b00000100,0b00001110,0b00000000,
-  0b00011111,0b00010000,0b00010000,0b00011110,0b00010000,0b00010000,0b00011111,0b00000000,
-  0b00010001,0b00010001,0b00011001,0b00010101,0b00010011,0b00010001,0b00010001,0b00000000,
-  0b00001110,0b00010001,0b00010000,0b00010000,0b00010000,0b00010001,0b00001110,0b00000000,
-  0b00011111,0b00010000,0b00010000,0b00011110,0b00010000,0b00010000,0b00011111,0b00000000,
-*/
-  0x0F,0x10,0x10,0x0E,0x01,0x01,0x1E,0x00, // S
-  0x0E,0x11,0x10,0x10,0x10,0x11,0x0E,0x00, // C
-  0x0E,0x40,0x40,0x40,0x40,0x40,0x0E,0x00, // I
-  0x1F,0x10,0x10,0x1E,0x10,0x10,0x1F,0x00, // E
-  0x11,0x11,0x19,0x15,0x13,0x11,0x11,0x00, // N
-  0x0E,0x11,0x10,0x10,0x10,0x11,0x0E,0x00, // C
-  0x1F,0x10,0x10,0x1E,0x10,0x10,0x1F,0x00  // E
-};
+void IMU_get(){
+  int i;
+  unsigned char c1,c2;  
 
-void setCG(int src,int dst,int len) {
-  lcd_cmd(0x38);
-  lcd_cmd(0x40 + dst);
-  if (src >= 0) {
-    for (int i = 0;i < len;i++) lcd_printHex(cg[src + i]);
-  } else {
-    for (int i = 0;i < len;i++) lcd_printHex(0);
+  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);
 }
 
-void setContrast(unsigned char c) {
-  lcd_cmd(0x39);
-  lcd_cmd(0x70 | (c & 0x0f)); // contrast Low
-  lcd_cmd(0x5C | ((c >> 4) & 0x03)); // contast High/icon/power
-  lcd_cmd(0x38);
-}
 
 int main() {
-    lcd_init();
-    while(1) {
-      switch(mode) {
-        case 0:  // init
-          lcd_setCursor(0, 0);
-          for (int i = 0;i < 6;i++) lcd_printHex(i);
-          lcd_setCursor(1, 1);
-          lcd_printStr("       ");
-          setCG(-1,0,6 * 8);
-          contrast = 35;
-          setContrast(contrast);
-          CGcounter = 0;
-          mode++;
-          break;
-        case 1:  // SWITCH
-          if(CGcounter <= (6 * 8)) {
-            setCG((CGcounter / 8) * 8,((CGcounter / 8) * 8) + 7 - (CGcounter % 8),CGcounter % 8);
-            CGcounter++;
-          } else {
-            lcd_setCursor(0, 0);
-            lcd_printStr("SWITCH");
-            setCG(-1,0,3 * 8);
-            lcd_setCursor(1, 1);
-            for (int i = 0;i < 3;i++) lcd_printHex(i);
-            CGcounter = 0;
-            mode++;
-          }
-          break;
-        case 2:  // SCI
-          if(CGcounter <= (3 * 8)) {
-            setCG(((CGcounter / 8) + 6) * 8,((CGcounter / 8) * 8) + 7 - (CGcounter % 8),CGcounter % 8);
-            CGcounter++;
-          } else {
-            lcd_setCursor(1, 1);
-            lcd_printStr("SCI");
-            setCG(-1,0,4 * 8);
-            lcd_setCursor(4, 1);
-            for (int i = 0;i < 4;i++) lcd_printHex(i);
-            CGcounter = 0;
-            mode++;
-          }
-          break;
-        case 3:  // ENCE
-          if(CGcounter <= (4 * 8)) {
-            setCG(((CGcounter / 8) + 9) * 8,((CGcounter / 8) * 8) + 7 - (CGcounter % 8),CGcounter % 8);
-            CGcounter++;
-          } else {
-            lcd_setCursor(4, 1);
-            lcd_printStr("ENCE");
-            FADEcounter = 0;
-            mode++;
-          }
-          break;
-        case 4:
-          if (contrastFlag == false) {
-            if (++contrast >= 54) contrastFlag = true;
-          } else {
-            if (--contrast <= 17) {
-              contrastFlag = false;
-              if(++FADEcounter >= 2) {
-                mode = 0;
-              }
-            }
-          }
-          setContrast(contrast);
-          break;
+
+      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();
       }
-      wait(0.05);
-    }
-}
+      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
\ No newline at end of file